added CRotationGrid

CRotationGrid.cu

+/*
+ * 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
+}
+