3、三维空间刚体运动：旋转矩阵、四元数、李群与李代数基础
各位同学，欢迎来到第三章。这一章，我们聊聊三维空间里最基础、也最绕不开的话题——刚体运动。
说实话，我刚入行做SLAM的时候，觉得这部分就是纯数学，枯燥得很。直到有一次，我在一个AR项目里，因为四元数顺序搞反了，导致虚拟物体在天上乱飞……嗯，从那以后，我再也不敢小看这些“基础”了。
你想想看，一个机器人或者一辆自动驾驶汽车，它在三维空间里怎么知道自己“转”了、“移”了？说白了，就是描述一个物体从位置A到位置B，姿态从朝向C变成朝向D。这背后，就是旋转矩阵、四元数、李群和李代数在撑腰。
3.1 旋转矩阵：最直观，但最啰嗦
旋转矩阵，我习惯叫它“老实人”。为什么？因为它把旋转这件事，用9个数字老老实实地写出来。
一个3x3的矩阵，正交且行列式为+1，就代表一次旋转。比如绕Z轴转θ角：
R_z(θ) = [cosθ -sinθ 0]
[sinθ cosθ 0]
[0 0 1]但问题来了——9个数字，其实只有3个自由度。冗余信息太多。而且，你连续做两次旋转，矩阵乘法一乘，数值误差慢慢累积，矩阵就不再正交了。我见过不少新手直接拿非正交矩阵去解算，结果姿态越算越歪。
3.2 四元数：紧凑且优雅
四元数，说白了就是“旋转的复数版本”。一个四元数 q = w + xi + yj + zk，其中 w 是实部，x,y,z 是虚部。它用4个数字表示旋转，没有冗余，而且没有万向锁问题。
我个人特别喜欢四元数做插值。比如你要让相机平滑地从姿态A转到姿态B，用四元数的球面线性插值（Slerp）就非常自然。旋转矩阵做插值？那简直是一场灾难。
这里给一个常用的四元数乘法代码（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
);
}3.3 李群与李代数：优化利器
好，到这里你可能要问：旋转矩阵和四元数都能表示旋转，为什么还要搞出李群和李代数？
原因很简单——优化。SLAM 的核心是求解一个优化问题，比如最小化重投影误差。而旋转矩阵和四元数都有约束（正交、单位模长），直接在上面做梯度下降非常麻烦。
李群 SO(3) 是旋转矩阵的集合，它是一个流形。李代数 so(3) 是它的切空间，是一个三维向量空间。说白了，李代数就是“旋转的微分形式”。
我建议你这样理解：
- SO(3)：旋转矩阵，是“结果”。
- so(3)：旋转向量（轴角），是“变化量”。
优化时，我们在李代数上做加法（无约束），然后通过指数映射回到李群。这样既保证了旋转的约束，又能用常规的优化方法。
指数映射公式（Rodrigues公式）：
R = exp(φ^) = I + sinθ * (n^) + (1 - cosθ) * (n^)^2其中 φ = θ * n，n 是单位旋转轴，θ 是旋转角。
3.4 知识体系总览
下面这张图，是我自己总结的本章知识结构。你可以把它当作一个“导航图”：
3.5 实际工程中的选择
在 SLAM 系统里，我一般这样选：
| 场景 | 推荐表示 | 原因 |
|---|---|---|
| 前端里程计（帧间匹配） | 四元数 | 轻量、易插值、无万向锁 |
| 后端图优化（BA） | 李代数 so(3) | 无约束、求导简单、收敛快 |
| 可视化/人机交互 | 旋转矩阵 | 直观、容易理解 |
| 传感器标定 | 四元数 + 李代数混合 | 初始化用四元数，优化用李代数 |
好了，这一章的内容就到这里。三维空间刚体运动，说难不难，说简单也不简单。关键是要理解每种表示法的“性格”——旋转矩阵老实但啰嗦，四元数优雅但需要小心，李代数强大但抽象。你多写几行代码，多跑几次实验，自然就熟了。