PCL四元数变换异常：5台Kinect环形采集点云无法正确配准

我使用5台Kinect按五边形布局采集人体360°点云数据，已通过Colmap获取相机位姿的四元数和平移参数，但无法利用这些参数正确变换PLY格式的点云文件。推测问题原因在于Kinect采用右手坐标系，与Colmap/PCL的坐标系存在适配问题。

目前使用的代码 #

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;

// 剩余Sub相机变换矩阵（省略）
......

// 点云变换
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);

核心问题分析 #

• 坐标系轴方向不匹配：Kinect的相机坐标系为右手系（X轴向右，Y轴向下，Z轴向前），而Colmap默认相机坐标系是右手系但Y轴向上，PCL的点云坐标系通常也是X右、Y上、Z前。直接套用Colmap的变换会导致点云出现翻转或错位。
• 位姿变换方向混淆：Colmap输出的是相机在世界坐标系中的位姿（即从世界到相机的变换），而PCL的transformPointCloud需要的是点云从相机坐标系转到世界坐标系的变换，这时候需要使用Colmap位姿矩阵的逆矩阵。
• 四元数符号/顺序验证：虽然代码中Eigen四元数的构造顺序（w,x,y,z）与Colmap输出一致，但仍需确认Colmap是否存在坐标系翻转导致的符号问题。

修正方案示例 #

1. 先做Kinect到PCL的坐标系转换

添加Y轴翻转矩阵，将Kinect的向下Y轴转换为PCL的向上Y轴：

// Kinect到PCL的坐标系转换：翻转Y轴
Eigen::Matrix4f kinect_to_pcl = Eigen::Matrix4f::Identity();
kinect_to_pcl(1, 1) = -1; // Y轴取反

2. 组合坐标系转换与Colmap位姿变换

如果Colmap输出的是相机到世界的变换矩阵，那么组合顺序为：

// 先转换Kinect坐标系，再应用世界变换
Eigen::Matrix4f combined_transform1 = transform_matrix1 * kinect_to_pcl;
pcl::transformPointCloud(*cloud_master, *master_transformed, combined_transform1);

如果Colmap输出的是世界到相机的变换矩阵，则需要先求逆再组合：

// 先求逆得到相机到世界的变换，再叠加坐标系转换
Eigen::Matrix4f combined_transform1 = kinect_to_pcl * transform_matrix1.inverse();
pcl::transformPointCloud(*cloud_master, *master_transformed, combined_transform1);

3. 单独验证坐标系转换

可以先对单台Kinect的点云仅做Y轴翻转，观察点云朝向是否符合预期，再逐步叠加Colmap的位姿变换，定位问题所在。

内容的提问来源于stack exchange，提问作者anonymous