PCL transformation error using quaternion

37 views Asked by At

I am using 5 kinect in pentagon to capture whole body 360°
enter image description here I have already got quaternion by using colmap.

enter image description here 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);

enter image description here

0

There are 0 answers