3. 刚体变换

各位，欢迎来到第三章。这一章我们要啃的，是传感器融合里最基础也最绕不开的一块硬骨头——刚体变换。

说白了，就是描述一个物体在三维空间里怎么转、怎么动。你想想看，激光雷达扫了一堆点云，摄像头拍了一张图像，这两个数据怎么对齐？靠的就是刚体变换。我当年刚入行时，觉得这东西不就是矩阵乘来乘去嘛，结果第一次做多传感器标定，旋转矩阵用错了顺序，点云直接飞到了天上……嗯，从那以后我再也不敢小看它了。

3.1 旋转矩阵：最直观，但也最啰嗦

旋转矩阵，一个3x3的矩阵，记作 R。它满足两个性质：正交（R^TR = I）和行列式为+1。所有这样的矩阵构成了SO(3)群——特殊正交群。

为什么叫“特殊”？因为行列式为+1，排除了镜像反射的情况。我在项目中遇到过有人直接把旋转矩阵当普通矩阵用，结果求逆的时候用了转置，但矩阵本身不是正交的，标定结果一塌糊涂。记住：旋转矩阵的逆就是它的转置，这是它最优雅的地方。

旋转矩阵的优点是直观，每个元素都有明确的几何意义。但缺点也很明显：9个参数，冗余度太高。你想想看，描述一个旋转其实只需要3个自由度，用9个参数去表示，不仅计算量大，还容易引入数值误差。

3.2 欧拉角：直观但危险

欧拉角用三个角度来描述旋转：偏航（Yaw）、俯仰（Pitch）、滚转（Roll）。听起来很直观对吧？但这里有个大坑——万向锁。

为什么会这样？当俯仰角达到±90°时，偏航和滚转的旋转轴会重合，丢失一个自由度。我曾在做无人机姿态估计时遇到过这个问题，飞控数据里欧拉角突然跳变，差点以为是传感器坏了。后来才发现是万向锁在作怪。

另外，欧拉角的旋转顺序也很关键。常见的顺序有ZYX、ZYZ、XYZ等。不同的顺序，同样的角度值，结果完全不同。我个人习惯用ZYX顺序，因为跟大多数机器人库的默认设置一致。

3.3 四元数：工程界的宠儿

四元数，一个四维向量 q = (w, x, y, z)，其中 w 是实部，(x, y, z) 是虚部。它没有奇点，插值平滑，计算效率高。说白了，它就是旋转矩阵的“压缩版”，用4个参数表示3个自由度，多出来的1个参数用来保证归一化。

四元数的乘法不满足交换律，这点要特别注意。我在做IMU数据融合时，经常需要把多个旋转叠加起来，顺序搞反了，结果就是天差地别。

四元数还有一个很实用的性质：球面线性插值（SLERP）。如果你想让相机从姿态A平滑过渡到姿态B，用四元数做SLERP是最自然的方式。旋转矩阵做不到这一点，欧拉角插值又容易出问题。

// 四元数乘法示例（C++风格伪代码）
Quaternion q1 = {0.707, 0.707, 0, 0}; // 绕X轴旋转90°
Quaternion q2 = {0.707, 0, 0.707, 0}; // 绕Y轴旋转90°
Quaternion q_result = q1 * q2;        // 注意顺序：先q2后q1
// 结果表示先绕Y轴转90°，再绕X轴转90°

3.4 平移向量：简单但别忽略

平移向量 t = (x, y, z)^T，就是三维空间里的位移。它跟旋转组合在一起，就构成了刚体变换的完整描述。

这里有个容易忽略的点：平移向量是在哪个坐标系下定义的？ 我见过有人把平移向量定义在源坐标系下，有人定义在目标坐标系下，结果标定出来的外参符号都是反的。通常，平移向量表示从源坐标系原点到目标坐标系原点的向量，在源坐标系下的表示。

3.5 SE(3)群：把旋转和平移打包

SE(3)群，全称是特殊欧几里得群。它把旋转矩阵和平移向量打包成一个4x4的齐次变换矩阵：

T = | R   t |
    | 0   1 |

这个矩阵的好处是，你可以把多个变换串联起来：T₁₃ = T₁₂ · T₂₃。而且，它的逆也很容易求：

T<sup>-1</sup> = | R<sup>T</sup>   -R<sup>T</sup>t |
         | 0       1     |

我在做多传感器标定时，经常需要把激光雷达坐标系下的点变换到相机坐标系下，再变换到车体坐标系下。用齐次变换矩阵，一次矩阵乘法就搞定，非常方便。

3.6 知识体系总览

下面这张图，把本章的核心概念串了起来。你可以把它当作一个快速参考。

3.7 实践中的选择建议

说了这么多，到底该用哪种表示方式？我的建议是：

• 做理论推导：用旋转矩阵，公式清晰，易于求导。
• 做传感器标定：用四元数，避免万向锁，优化时参数少。
• 做可视化调试：用欧拉角，方便人理解。
• 做坐标变换流水线：用SE(3)齐次矩阵，统一接口。

嗯，这一章的内容就到这里。刚体变换是传感器融合的基石，理解透了，后面的多视角标定、点云配准、SLAM等问题，都会轻松很多。记住：旋转是SO(3)，平移加旋转是SE(3)，这两个群贯穿了整个课程。