Commit 3728892c authored by karius's avatar karius

added CRotationGrid

parent e18762dc
/*
* CRotationGrid.cpp
*
* Created on: Sep 17, 2019
* Author: kkarius
*/
#include <CRotationGrid.h>
#include <math.h>
#include <cmath>
void CRotationGrid::random_close_to_unity(float * &matrix,size_t offset){
float eps = 0.1;
int _rand_max = 1000;
//euler angles
float phi = scaled_rand(_rand_max,eps);
float theta = scaled_rand(_rand_max,eps);
float psi = scaled_rand(_rand_max,eps);
matrix[offset+0] = cos(psi)*cos(phi)-cos(theta)*sin(phi)*sin(psi);
matrix[offset+1] = cos(psi)*sin(phi)+cos(theta)*cos(phi)*sin(psi);
matrix[offset+2] = sin(psi)*sin(theta);
matrix[offset+3] = -sin(psi)*cos(phi)-cos(theta)*sin(phi)*cos(psi);
matrix[offset+4] = -sin(psi)*sin(phi)+cos(theta)*cos(phi)*cos(psi);
matrix[offset+5] = cos(psi)*sin(theta);
matrix[offset+6] = sin(theta)*sin(phi);
matrix[offset+7] = -sin(theta)*cos(phi);
matrix[offset+8] = cos(theta);
}
CRotationGrid::CRotationGrid(std::string rot_file_path, size_t chunksize) : // @suppress("Class members should be properly initialized")
m_filepath(rot_file_path), m_rotation_format(ERotationFormat::eMatrix), m_chunksize(
chunksize) {
try {
m_filestream.open(m_filepath, std::fstream::in);
} catch (std::exception& e) {
throw e;
}
//also parses total number of rotations and sanity checks chunk size
m_beginning_found = x_find_beginning();
if (m_beginning_found && m_rotation_format == ERotationFormat::eMatrix) {
m_matrix_chunk = (float *) malloc(9 * chunksize * sizeof(float));
//get first batch into m_matrix_chunk
(*this)++;
m_current_chunk_num--;
}
}
CRotationGrid& CRotationGrid::operator++(int inc) {
if (m_beginning_found) {
for (int i = 0; i < m_chunksize; i++) {
if (!m_filestream.eof()) {
if (x_parse_line()) {
if (m_rotation_format == ERotationFormat::eMatrix) {
for (int j = 0; j < 9; j++) {
m_matrix_chunk[9 * i + j] = m_current_matrix[j];
}
}
}
}
}
}
m_current_chunk_num++;
return *this;
}
void CRotationGrid::print_current(){
if (m_rotation_format == ERotationFormat::eMatrix){
printf("-------------------\n");
for (int i=0;i<3;i++){
for (int j=0;j<3;j++){
if (j!=2){
printf("%f,",m_current_matrix[i*3+j]);
} else {
printf("%f",m_current_matrix[i*3+j]);
}
}
printf("\n");
}
}
}
float *& CRotationGrid::operator *() {
return m_matrix_chunk;
}
bool CRotationGrid::operator!() {
// printf("!:%i\n",m_current_chunk_num);
if (m_current_chunk_num==0)
return true;
return (rotation_num/(m_current_chunk_num*m_chunksize) !=0);
}
void CRotationGrid::reset(){
m_beginning_found = false;
m_filestream.close();
m_filestream.open(m_filepath, std::fstream::in);
m_beginning_found = x_find_beginning();
m_current_chunk_num = 0;
m_current_rot_num = 0;
if (m_beginning_found){
(*this)++;
m_current_chunk_num--;
}
}
#include <limits>
void CRotationGrid::quat_to_euler(float4* & rotations_quat,float3* & rotations_euler, int n){
float4 q;
float3 eul;
float m01,m02,m11,m12,m20,m21,m22;
for (int i = 0;i<n;i++){
q = rotations_quat[i];
m01 = 2*q.x*q.y - 2*q.z*q.w,
m02 = 2*q.x*q.z + 2*q.y*q.w,
// m10 = 2*q.x*q.y + 2*q.z*q.w,
m11 = 1 - 2*q.x*q.x - 2*q.z*q.z,
m12 = 2*q.y*q.z - 2*q.x*q.w,
m20 = 2*q.x*q.z - 2*q.y*q.w,
m21 = 2*q.y*q.z + 2*q.x*q.w,
m22 = 1 - 2*q.x*q.x - 2*q.y*q.y;
// Taken from Ken Shoemake, "Euler Angle Conversion", Graphics Gems
// IV, Academic 1994.
//
// http://vered.rose.utoronto.ca/people/david_dir/GEMS/GEMS.html
double sy = sqrt(m02*m02 + m12*m12);
// double sy = sqrt(m10*m10 + m20*m20);
double a, b, c;
b = atan2(sy, m22);
if (sy > 16 * std::numeric_limits<double>::epsilon()) {
a = atan2(m12, m02);
c = atan2(m21, -m20);
} else {
a = atan2(-m01, m11);
c = 0;
}
eul.x = (float) a;
eul.y = (float) b;
eul.z = (float) c;
rotations_euler[i] = eul;
}
}
void CRotationGrid::find_nearest_rotation_set(float alpha_approx, float4 *& rotations,uint *& n_rotations, float & alpha)
{
std::ifstream filestream;
std::vector<std::string> strs;
std::string current_line;
std::map<float,std::string> alpha_to_path;
std::map<std::string,size_t> path_to_rot_sample_volume;
int rotation_num;
//TODO: IMPORTANT MAKE THIS ENVIRONEMNTAL VARIABLE
const char * base_dir = "/data/eclipse-workspace/fitter/cffk-orientation-dc8bb42/data/";
std::string base(base_dir);
std::string _file;
DIR * dirp = opendir(base_dir);
struct dirent * dp;
while ((dp = readdir(dirp)) != NULL){
_file = base;
_file.append(dp->d_name);
if ( _file.substr(_file.length()-5,5) == std::string(".quat")){
filestream.open(_file, std::fstream::in);
while (std::getline(filestream, current_line)){
if (current_line.find("format") != std::string::npos) {
//skip parameter m_current_lines
std::getline(filestream, current_line);
std::getline(filestream, current_line);
boost::split(strs, current_line, boost::is_any_of(" \t"),
boost::token_compress_on);
path_to_rot_sample_volume[_file] = static_cast<size_t>(std::stoi(strs[0]));
alpha_to_path[std::stof(strs[1])] = _file;
break;
}
}
filestream.close();
}
}
std::map<float,std::string>::iterator it = alpha_to_path.begin();
float _min = 180;
while (it != alpha_to_path.end()){
if (abs(it->first-alpha_approx)<_min){
_min = abs(it->first-alpha_approx);
alpha = it->first;
}
it++;
}
uint N = path_to_rot_sample_volume[alpha_to_path[alpha]];
rotations = (float4 *) malloc(sizeof(float4)*N);
filestream.open(alpha_to_path[alpha],std::fstream::in);
//jump header
for (int i=0;i<4;i++){
std::getline(filestream, current_line);
}
//populate rotation array
for (int i=0;i<N;i++){
std::getline(filestream, current_line);
boost::trim(current_line);
boost::split(strs, current_line, boost::is_any_of(" \t"),
boost::token_compress_on);
if (strs.size() >= 4) {
rotations[i].x = std::stof(strs[0]);
rotations[i].y = std::stof(strs[1]);
rotations[i].z = std::stof(strs[2]);
rotations[i].w = std::stof(strs[3]);
}
}
*n_rotations = N;
}
bool CRotationGrid::x_find_beginning() {
bool success = false;
std::vector<std::string> strs;
while (!m_filestream.eof()) {
std::getline(m_filestream, m_current_line);
if (m_current_line.find("format") != std::string::npos) {
//skip parameter m_current_lines
std::getline(m_filestream, m_current_line);
std::getline(m_filestream, m_current_line);
boost::split(strs, m_current_line, boost::is_any_of(" \t"),
boost::token_compress_on);
rotation_num = std::stoi(strs[0]);
covering_radius = std::stof(strs[1]);
success = true & m_chunksize <= MAX_CHUNKSIZE;
break;
}
}
x_parse_line(); //skip identity
return success;
}
bool CRotationGrid::x_quat_to_rot_mat() {
bool success = false;
if (m_current_quat.x * m_current_quat.x
+ m_current_quat.y * m_current_quat.y
+ m_current_quat.z * m_current_quat.z
+ m_current_quat.w * m_current_quat.w - 1.0 < 1.0e-5) {
success = true;
}
// 0 1 2
// 3 4 5 --> 0 1 2 3 4 5 6 7 8
// 6 7 8
m_current_matrix[0] = 1 - 2 * m_current_quat.z * m_current_quat.z
- 2 * m_current_quat.w * m_current_quat.w;
m_current_matrix[1] = 2 * m_current_quat.y * m_current_quat.z
- 2 * m_current_quat.x * m_current_quat.w;
m_current_matrix[2] = 2 * m_current_quat.y * m_current_quat.w
+ 2 * m_current_quat.x * m_current_quat.z;
m_current_matrix[3] = 2 * m_current_quat.z * m_current_quat.y
+ 2 * m_current_quat.x * m_current_quat.w;
m_current_matrix[4] = 1 - 2 * m_current_quat.w * m_current_quat.w
- 2 * m_current_quat.y * m_current_quat.y;
m_current_matrix[5] = 2 * m_current_quat.z * m_current_quat.w
- 2 * m_current_quat.x * m_current_quat.y;
m_current_matrix[6] = 2 * m_current_quat.w * m_current_quat.y
- 2 * m_current_quat.x * m_current_quat.z;
m_current_matrix[7] = 2 * m_current_quat.w * m_current_quat.z
+ 2 * m_current_quat.x * m_current_quat.y;
m_current_matrix[8] = 1 - 2 * m_current_quat.y * m_current_quat.y
- 2 * m_current_quat.z * m_current_quat.z;
return success;
}
size_t CRotationGrid::current_chunk_size(){
if (m_current_chunk_num==0)
return m_chunksize;
if (rotation_num-m_current_chunk_num*m_chunksize<m_chunksize){
return rotation_num-m_current_chunk_num*m_chunksize;
}
else {
return m_chunksize;
}
}
bool CRotationGrid::x_parse_line() {
bool success = true;
std::vector<std::string> strs;
std::getline(m_filestream, m_current_line);
boost::trim(m_current_line);
if (m_current_line.size() != 0) {
boost::split(strs, m_current_line, boost::is_any_of(" \t"),
boost::token_compress_on);
if (strs.size() >= 4) {
m_current_quat.x = std::stof(strs[0]);
m_current_quat.y = std::stof(strs[1]);
m_current_quat.z = std::stof(strs[2]);
m_current_quat.w = std::stof(strs[3]);
}
if (m_rotation_format == ERotationFormat::eMatrix) {
success &= x_quat_to_rot_mat();
}
}
m_current_rot_num++;
return success;
}
CRotationGrid::~CRotationGrid() {
// TODO Auto-generated destructor stub
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment