I am using 5 kinect in pentagon to capture whole body 360°
I have already got quaternion by using colmap.
But it can't transform the .ply
file correctly.
I think the cause of the problem is kinect is a right hand camera
Here is my code
float qw1 = 0.980613, qx1 = -0.0777902, qy1 = -0.176786, qz1 = -0.0330758, tx1 = -0.798112, ty1 = -0.774293, tz1 = 3.76053;
float qw2 = 0.861117, qx2 = -0.0716478, qy2 = 0.427619, qz2 = 0.265493, tx2 = -2.94326, ty2 = -1.91445, tz2 = 6.074;
float qw3 = 0.954216, qx3 = -0.0519415, qy3 = -0.265356, qz3 = -0.127908, tx3 = 0.983777, ty3 = 0.099799, tz3 = 1.74569;
float qw4 = 0.977833, qx4 = -0.0748412, qy4 = 0.148912, qz4 = 0.126758, tx4 = -1.72221, ty4 = -0.292148, tz4 = 2.7061;
float qw5 = 0.771623, qx5 = -0.0842691, qy5 = -0.547153, qz5 = -0.313242, tx5 = 2.53668, ty5 = -0.921194, tz5 = 3.55139;
//master
Eigen::Quaternionf quat1(qw1, qx1, qy1, qz1);
Eigen::Matrix4f transform_matrix1 = Eigen::Matrix4f::Identity();
Eigen::Matrix3f rot_mat1 = quat1.toRotationMatrix();
transform_matrix1.block(0, 0, 3, 3) = rot_mat1;
transform_matrix1.block(0, 3, 3, 1) << tx1, ty1, tz1;
std::cout << "Master : " << std::endl;
std::cout << transform_matrix1 << std::endl;
//sub01
Eigen::Quaternionf quat2(qw2, qx2, qy2, qz2);
Eigen::Matrix4f transform_matrix2 = Eigen::Matrix4f::Identity();
Eigen::Matrix3f rot_mat2 = quat2.toRotationMatrix();
transform_matrix2.block(0, 0, 3, 3) = rot_mat2;
transform_matrix2.block(0, 3, 3, 1) << tx2, ty2, tz2;
std::cout << "Sub01 : " << std::endl;
std::cout << transform_matrix2 << std::endl;
//sub02
Eigen::Quaternionf quat3(qw3, qx3, qy3, qz3);
Eigen::Matrix4f transform_matrix3 = Eigen::Matrix4f::Identity();
Eigen::Matrix3f rot_mat3 = quat3.toRotationMatrix();
transform_matrix3.block(0, 0, 3, 3) = rot_mat3;
transform_matrix3.block(0, 3, 3, 1) << tx3, ty3, tz3;
std::cout << "Sub02 : " << std::endl;
std::cout << transform_matrix3 << std::endl;
......
pcl::transformPointCloud(*cloud_master, *master_transformed, transform_matrix1);
pcl::transformPointCloud(*cloud_sub01, *sub01_transformed, transform_matrix2);
pcl::transformPointCloud(*cloud_sub02, *sub02_transformed, transform_matrix3);
pcl::transformPointCloud(*cloud_sub03, *sub03_transformed, transform_matrix4);
pcl::transformPointCloud(*cloud_sub04, *sub04_transformed, transform_matrix5);