世界坐标系下四元数转旋转矩阵及点云配准问题咨询

我正在进行点云配准的变换操作：

已获取到四元数，但使用点云库（PCL）变换时，发现是在局部坐标系而非世界坐标系下对点云进行变换，导致配准失败。我想了解四元数转旋转矩阵的正确公式。

以下是我的PCL变换代码：

float qw, qx, qy, qz, tx, ty, tz;
qw = kinectmatrix4f[num][0];
qx = kinectmatrix4f[num][1];
qy = kinectmatrix4f[num][2];
qz = kinectmatrix4f[num][3];
tx= kinectmatrix4f[num][4];
ty = kinectmatrix4f[num][5];
tz = kinectmatrix4f[num][6];
//qx = -qx;
qy = -qy;

//init transformation matrix
Eigen::Matrix4f transform_matrix = Eigen::Matrix4f::Identity();
transform_matrix <<
    1 - 2 * pow(qy, 2) - 2 * pow(qz, 2), 2 * qx*qy - 2 * qz*qw, 2 * qx*qz + 2 * qy*qw, 0,
    2 * qx*qy + 2 * qz*qw, 1 - 2 * pow(qx, 2) - 2 * pow(qz, 2), 2 * qy*qz - 2 * qx*qw, 0,
    2 * qx*qz - 2 * qy*qw, 2 * qy*qz + 2 * qx*qw, 1 - 2 * pow(qx, 2) - 2 * pow(qy, 2), 0,
    0, 0, 0, 1;

printf("Transforming point cloud %i in rough\n", num);
std::cout << transform_matrix << std::endl;

pcl::transformPointCloud(*cloud_input, *cloud_input, transform_matrix);

不过我在Unity中使用以下代码变换时，旋转效果正常：

Quaternion rot=new Quaternion(0.4f,0.5f,0.9f,1);
transform.rotation=rot;    

问题核心与解决方案 #

1. 四元数转旋转矩阵的标准公式 #

对于单位四元数 ( \mathbf{q} = q_w + q_x\mathbf{i} + q_y\mathbf{j} + q_z\mathbf{k} )，对应的右手坐标系旋转矩阵为：
[
\begin{bmatrix}
1-2q_y^2-2q_z2 & 2q_xq_y-2q_zq_w & 2q_xq_z+2q_yq_w \
2q_xq_y+2q_zq_w & 1-2q_x^2-2q_z2 & 2q_yq_z-2q_xq_w \
2q_xq_z-2q_yq_w & 2q_yq_z+2q_xq_w & 1-2q_x^2-2q_y2
\end{bmatrix}
]
但PCL与Unity的坐标系、四元数分量顺序存在本质差异，这是导致变换异常的直接原因。

2. 代码问题分析 #

• 四元数分量顺序不匹配：Unity的Quaternion构造参数是(x,y,z,w)，而你的PCL代码中四元数取值顺序是(w,x,y,z)，直接转换会导致旋转轴映射错误。
• 坐标系方向差异：Unity是左手坐标系（y轴向上），PCL默认右手坐标系（y轴向下），仅手动取反qy无法完全适配坐标系差异。
• 未包含平移分量：你的变换矩阵平移项全设为0，但已获取tx,ty,tz，这会导致点云仅旋转不平移，无法完成配准。

3. 修正后的PCL代码 #

float qw, qx, qy, qz, tx, ty, tz;
qw = kinectmatrix4f[num][0];
qx = kinectmatrix4f[num][1];
qy = kinectmatrix4f[num][2];
qz = kinectmatrix4f[num][3];
tx = kinectmatrix4f[num][4];
ty = kinectmatrix4f[num][5];
tz = kinectmatrix4f[num][6];

// 适配Unity到PCL的坐标系：修正y轴方向
qy = -qy;
ty = -ty;

// 用Eigen四元数直接转换，避免手动写矩阵出错
Eigen::Quaternionf q(qw, qx, qy, qz);
Eigen::Matrix3f rotation_matrix = q.toRotationMatrix();

// 构造完整变换矩阵（包含平移）
Eigen::Matrix4f transform_matrix = Eigen::Matrix4f::Identity();
transform_matrix.block<3,3>(0,0) = rotation_matrix;
transform_matrix(0,3) = tx;
transform_matrix(1,3) = ty;
transform_matrix(2,3) = tz;

printf("Transforming point cloud %i in rough\n", num);
std::cout << transform_matrix << std::endl;

pcl::transformPointCloud(*cloud_input, *cloud_input, transform_matrix);

4. 关键注意事项 #

• 优先使用Eigen的Quaternion类转换旋转矩阵，避免手动计算的误差。
• 必须统一坐标系：PCL与Unity的y轴方向相反，旋转和平移分量都需要取反适配。
• 确保变换矩阵包含平移参数，否则点云无法移动到世界坐标系的正确位置。

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