第三章 刚体变换与姿态表示：旋转矩阵、欧拉角、四元数、齐次变换矩阵、变换的链式法则
各位同学，欢迎来到第三章。这一章，我们聊聊点云配准里最核心的数学基础——刚体变换。
说白了，点云配准要解决什么问题？就是找到两个点云之间的那个变换关系。一个点云怎么旋转、怎么平移，才能和另一个点云对上。这个变换，在三维空间里就是刚体变换。物体不会变形，只会转一转、挪一挪位置。
我刚开始做点云配准时，觉得这玩意儿不就是个矩阵乘法嘛，有啥难的？结果真上手才发现，姿态表示方式五花八门，选错了方法，后面调试能让你怀疑人生。嗯，今天我们就把它彻底讲清楚。
核心要点：刚体变换 = 旋转 + 平移。旋转有3种主流表示方式：旋转矩阵、欧拉角、四元数。再加上齐次变换矩阵，就能把旋转和平移统一成一个4x4矩阵。变换的链式法则，则是拼接多个变换的关键。
3.1 旋转矩阵——最直观，但最啰嗦
旋转矩阵，就是一个3x3的正交矩阵。它的列向量互相垂直，而且行列式等于1。你想想看，这保证了什么？保证了旋转前后，向量的长度不变，夹角不变。说白了，就是刚体不会变形。
我记得第一次写点云配准代码时，直接用旋转矩阵去乘每个点。代码跑起来没问题，但调试时想微调一下旋转角度，就傻眼了——矩阵里9个数，改哪个？改多少？完全没直觉。
旋转矩阵的优点是：没有奇异性，计算稳定。缺点是：冗余参数太多。9个数描述3个自由度，浪费啊。而且人眼根本看不出这个矩阵对应什么姿态。
我的建议：在底层计算、矩阵乘法、优化求解时，用旋转矩阵。因为数学性质好，求导方便。但在人机交互、参数调试时，别用它，你会疯的。
3.2 欧拉角——直观，但小心万向锁
欧拉角用三个角度来描述旋转：绕X轴转（roll）、绕Y轴转（pitch）、绕Z轴转（yaw）。也就是我们常说的RPY角。这个表示法非常直观，一看就懂。
但欧拉角有个大坑——万向锁。当pitch角接近±90度时，roll和yaw的旋转轴会重合，丢失一个自由度。这时候你无论怎么调roll和yaw，效果都一样。我在做机械臂手眼标定时就踩过这个坑，标定结果死活不对，查了两天才发现是欧拉角表示在奇异点附近出了问题。
避坑指南：我曾经在无人机姿态估计项目里，直接用欧拉角做卡尔曼滤波的状态量。结果飞机做大机动时，万向锁导致滤波器发散，差点炸机。后来全部换成四元数，问题解决。所以，涉及连续旋转、大角度运动时，慎用欧拉角。
欧拉角的另一个问题是：旋转顺序很重要。XYZ、ZYX、ZYZ……不同顺序结果完全不同。你想想看，如果团队里有人用XYZ，有人用ZYX，那代码合在一起就是灾难。
3.3 四元数——低调但强大
四元数，很多人一听就头大。其实没那么复杂。四元数就是一个四维向量：q = (w, x, y, z)，其中w是实部，(x,y,z)是虚部。它表示绕某个轴旋转某个角度。
为什么用四元数？三个理由：
- 无奇异性——不存在万向锁问题
- 紧凑——4个参数描述3个自由度，比旋转矩阵少
- 插值方便——球面线性插值（SLERP）可以平滑过渡
我个人习惯，在SLAM、点云配准的优化后端里，全部用四元数表示姿态。因为优化时四元数的约束条件（模长为1）比旋转矩阵的正交约束好处理得多。
四元数乘法也不难：
// 四元数乘法（C++风格伪代码）
Quaternion operator*(const Quaternion& q1, const Quaternion& q2) {
return Quaternion(
q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z,
q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y,
q1.w * q2.y - q1.x * q2.z + q1.y * q2.w + q1.z * q2.x,
q1.w * q2.z + q1.x * q2.y - q1.y * q2.x + q1.z * q2.w
);
}小技巧：四元数表示旋转时，一定要归一化。我见过太多bug是因为四元数模长不是1导致的。每次更新完四元数，顺手做一次归一化，养成习惯。
3.4 齐次变换矩阵——把旋转和平移统一起来
旋转矩阵处理旋转，平移向量处理平移。但实际应用中，我们经常需要把旋转和平移组合在一起。齐次变换矩阵就是干这个的。
它是一个4x4矩阵：
T = | R t |
| 0 1 |其中R是3x3旋转矩阵，t是3x1平移向量。最后一行是(0,0,0,1)。
用齐次坐标表示一个三维点：p = (x, y, z, 1)^T。那么变换就是：p' = T * p。
为什么用齐次形式？因为可以把多次变换连乘起来。比如从世界坐标系到相机坐标系，再投影到图像平面，全部可以用矩阵乘法搞定。这在点云配准的ICP算法里特别有用。
注意：齐次变换矩阵的逆矩阵也很好求：
T_inv = | R^T -R^T * t |
| 0 1 |不需要真的去求4x4矩阵的逆，直接用这个公式，又快又准。
3.5 变换的链式法则——拼接的艺术
点云配准很少是一次变换搞定的。通常是：先粗配准，再精配准；或者先转到一个中间坐标系，再转到目标坐标系。这就涉及到多个变换的拼接。
链式法则很简单：多个变换矩阵按顺序相乘。
假设有变换T1（从A到B），T2（从B到C），那么从A到C的变换就是：T = T2 * T1。
注意顺序！先发生的变换在右边，后发生的在左边。这个顺序搞反了，结果就完全不对。我见过太多新手在这里栽跟头。
// 链式变换示例
Eigen::Matrix4d T_world_to_camera = T_camera_to_robot * T_robot_to_world;
// 注意：先经过T_robot_to_world，再经过T_camera_to_robot在实际的点云配准流程中，我们经常维护一个变换树。每个节点代表一个坐标系，边代表变换关系。配准时，沿着树找到从源坐标系到目标坐标系的路径，把路径上的变换矩阵乘起来就行。
避坑指南：我曾经在拼接多帧点云时，链式变换累积了十几层。结果因为浮点数精度问题，最后一帧的点云漂移了几厘米。解决方案是：定期做闭环检测，用图优化来修正累积误差。这个后面我们会详细讲。
3.6 几种表示方式的对比
| 表示方式 | 参数数量 | 奇异性 | 插值难度 | 人机交互 | 推荐场景 |
|---|---|---|---|---|---|
| 旋转矩阵 | 9 | 无 | 难 | 差 | 底层计算、优化求解 |
| 欧拉角 | 3 | 有（万向锁） | 中等 | 好 | 人机交互、简单场景 |
| 四元数 | 4 | 无 | 容易（SLERP） | 差 | SLAM、优化、动画插值 |
| 齐次变换矩阵 | 16 | 无 | — | — | 统一表示旋转+平移 |
好了，这一章的内容就到这里。刚体变换是点云配准的基石，理解透了，后面的ICP、NDT算法才能学得轻松。记住：选对表示方式，能让你少踩一半的坑。
一句话总结：计算用矩阵，交互用欧拉角，优化用四元数，统一用齐次矩阵。链式法则就是矩阵连乘，注意顺序别搞反。
公众号:蓝海资料掘金营，微信deep3321