第2章 数学基础（上）：三维空间刚体运动
各位同学，欢迎来到数学基础部分。说实话，很多做SLAM的同学一看到数学就头疼，觉得这是拦路虎。我当年刚入行时也一样，看到旋转矩阵就犯晕。但后来我发现，只要把几何意义搞明白，这些数学工具其实挺直观的。
这一章我们聊三维空间里的刚体运动。说白了，就是研究一个物体怎么转、怎么动。你想想看，机器人或者相机在三维空间里移动，无非就是旋转加平移。嗯，就这么简单。
2.1 旋转矩阵
先说说旋转矩阵。它是个3×3的方阵，用来描述一个坐标系相对于另一个坐标系的旋转关系。我习惯把它想象成「坐标转换器」——把一个向量从一个坐标系转到另一个坐标系。
旋转矩阵有两个重要性质：
- 正交性：RTR = I，也就是说它的逆等于转置
- 行列式为+1：这保证了它是纯旋转，没有缩放或镜像
我在项目中遇到过一个问题：用旋转矩阵做连续旋转时，由于浮点数误差，矩阵会慢慢失去正交性。嗯，这里要注意，必须定期做正交化处理，否则误差会累积。
核心公式：
// 绕x轴旋转θ角
Rx(θ) = [1, 0, 0;
0, cosθ, -sinθ;
0, sinθ, cosθ]
// 绕y轴旋转θ角
Ry(θ) = [cosθ, 0, sinθ;
0, 1, 0;
-sinθ, 0, cosθ]
// 绕z轴旋转θ角
Rz(θ) = [cosθ, -sinθ, 0;
sinθ, cosθ, 0;
0, 0, 1]2.2 旋转向量与欧拉角
旋转矩阵有9个参数，但自由度只有3个。说白了就是冗余。所以就有了更紧凑的表达方式。
旋转向量：用一个三维向量表示旋转，方向是旋转轴，长度是旋转角度。我特别喜欢这种表达，因为它很直观——「绕这个轴转这么多度」。
欧拉角：把旋转分解成绕三个轴的依次旋转。常见的顺序有ZYX、ZYZ等。欧拉角的问题在于万向锁——当第二个旋转角为±90°时，第一个和第三个旋转轴会重合，丢失一个自由度。
避坑指南：我曾经在无人机姿态估计中吃过万向锁的亏。当时用欧拉角做姿态插值，结果在俯仰角接近90°时，航向角突然跳变。后来改用四元数才解决。所以，如果你要做平滑的旋转插值，千万别用欧拉角。
2.3 四元数
四元数是我个人最推荐的旋转表达方式。它没有万向锁问题，插值平滑，计算效率也高。
四元数可以看作复数的推广：q = w + xi + yj + zk，其中i² = j² = k² = -1。单位四元数（模长为1）可以用来表示旋转。
为什么用四元数？三个理由：
- 紧凑：4个参数，比旋转矩阵的9个少
- 无奇点：不像欧拉角有万向锁
- 易插值：球面线性插值（SLERP）非常平滑
实用技巧：四元数乘法不满足交换律，顺序很重要！我习惯记成「先旋转的放在右边」。比如从坐标系A到B的旋转，再转到C，就是qAC = qBC * qAB。
2.4 李群与李代数基础
这部分是SLAM优化的核心。你想想看，我们要优化相机位姿，但旋转矩阵有正交约束，直接优化很麻烦。李群和李代数就是来解决这个问题的。
李群SO(3)：三维旋转矩阵的集合，构成一个群。它是个流形，也就是说在局部看起来像欧氏空间。
李代数so(3)：SO(3)在单位元处的切空间。说白了，就是旋转的「速度」空间。每个so(3)元素是一个三维向量，对应一个反对称矩阵。
它们之间通过指数映射和对数映射相互转换：
// 指数映射：从李代数到李群
R = exp(φ^) // φ是三维向量，φ^是它的反对称矩阵
// 对数映射：从李群到李代数
φ = log(R)我在做视觉惯性里程计时，就是用李代数来表示位姿增量。这样在优化时，不需要考虑旋转矩阵的约束，直接做无约束优化就行。方便得很。
为什么用李代数？
- 旋转矩阵有6个约束（正交+行列式为1），优化困难
- 李代数是无约束的，可以直接用高斯牛顿法
- 指数映射保证了结果一定在流形上
2.5 实践中的选择
说了这么多，到底该用哪种？我根据经验给个建议：
| 场景 | 推荐表达 | 原因 |
|---|---|---|
| 坐标变换计算 | 旋转矩阵 | 方便组合，效率高 |
| 姿态插值 | 四元数 | 平滑无奇点 |
| 人机交互 | 欧拉角 | 直观易理解 |
| 优化求解 | 李代数 | 无约束优化 |
嗯，这一章的内容就到这里。说白了，这些数学工具没有谁好谁坏，关键看场景。我建议你先把旋转矩阵和四元数练熟，这两个用得最多。李代数可以先理解概念，等做到后端优化时再深入。
下一章我们继续聊李代数的求导和扰动模型，那是SLAM优化的核心技巧。到时候我会结合具体代码，带你手撕一遍。