数学基础（一）：三维空间刚体运动、旋转矩阵、旋转向量、欧拉角、四元数
各位同学好，我是你们的讲师。今天咱们来啃一块硬骨头——三维空间里的刚体运动。说实话，这块内容我当年学的时候也绕了不少弯路。你想想看，一个物体在三维空间里又转又移，怎么用数学精确描述它？这问题要是搞不清楚，后面整个SLAM系统都得翻车。
我个人的习惯是，先别急着背公式。咱们先想清楚一个核心问题：我们到底需要描述什么？说白了，就是两件事——位置和姿态。位置好办，一个三维向量搞定。姿态呢？麻烦就来了。一个物体朝哪个方向？歪了多少？这玩意儿有无数种表示方法。今天咱们就把最常用的四种：旋转矩阵、旋转向量、欧拉角、四元数，一个一个捋清楚。
核心观点：刚体运动 = 旋转 + 平移。旋转是难点，也是今天的主菜。
1. 旋转矩阵：最直观，但最啰嗦
旋转矩阵，说白了就是一个3x3的矩阵。它把一个向量从坐标系A变换到坐标系B。比如，你在相机坐标系下看到一个点，想知道它在世界坐标系下的坐标，乘上旋转矩阵就行。
旋转矩阵有个硬性要求：必须是正交矩阵，行列式为+1。什么意思？就是它的每一列都是单位向量，而且两两垂直。我刚开始做SLAM时，有一次手算旋转矩阵，算完发现行列式是0.98，结果整个轨迹都飘了。嗯，这里要注意，数值误差积累起来很要命。
旋转矩阵的好处是直观，你一眼就能看出三个轴分别指向哪。但坏处也很明显——9个参数，冗余啊！而且你每次更新它，还得保证它仍然是正交矩阵，这计算量不小。
我的经验：在SLAM后端优化里，我几乎不用旋转矩阵做优化变量。为什么？因为9个参数有6个约束，优化起来太麻烦。我更喜欢用后面讲的李代数。
2. 旋转向量：简洁，但有奇点
旋转向量，也叫轴角表示。它用一个三维向量来描述旋转：方向代表旋转轴，长度代表旋转角度。比如，绕z轴转90度，旋转向量就是 (0, 0, π/2)。
这玩意儿比旋转矩阵省地方，3个参数搞定。而且物理意义很清晰——你想象一下，拿根筷子插在物体上，转一个角度，筷子指向就是旋转轴。
但是，它有个大坑：奇点问题。当旋转角度接近0度时，旋转轴变得不确定。你想想看，转0度，任何轴都可以。这就导致在优化时，角度接近0的地方梯度会出问题。我在做视觉惯性里程计时，就因为这个奇点，导致初始化阶段老是收敛不到正确值。
避坑指南：我曾经在写VIO初始化代码时，直接用旋转向量做优化变量。结果角度接近0时，雅可比矩阵爆炸了。后来我改用四元数，问题才解决。所以，如果你的系统里可能有小角度旋转，慎用旋转向量。
3. 欧拉角：最符合直觉，但最坑
欧拉角，就是大家熟悉的俯仰角、偏航角、滚转角。它把一次旋转拆成绕三个轴的依次旋转。比如，先绕z轴转yaw，再绕y轴转pitch，最后绕x轴转roll。
这玩意儿的好处是，你给飞行员说「左转30度，抬头10度」，他秒懂。但坏处呢？万向锁。当pitch接近±90度时，yaw和roll的旋转轴会重合，丢失一个自由度。我当年做无人机仿真时，就因为这个bug，飞机在空中突然失控了。查了半天才发现是欧拉角导致的。
所以，在SLAM里，我几乎不用欧拉角做内部计算。它只适合做用户交互，比如显示给操作员看。
| 表示方法 | 参数数量 | 优点 | 缺点 |
|---|---|---|---|
| 旋转矩阵 | 9 | 直观，无奇点 | 冗余，需正交约束 |
| 旋转向量 | 3 | 紧凑，物理意义清晰 | 有奇点（0度附近） |
| 欧拉角 | 3 | 符合直觉 | 万向锁，不连续 |
| 四元数 | 4 | 无奇点，可插值 | 需归一化，不直观 |
4. 四元数：SLAM里的王者
四元数，说实话，我第一次看到它时也是一脸懵。一个标量加三个虚部，这玩意儿怎么就能表示旋转了？
但用久了你会发现，它真香。为什么？没有奇点，而且可以平滑插值。你想想看，两个姿态之间怎么过渡？用旋转矩阵插值，结果可能不是旋转矩阵。用欧拉角插值，可能遇到万向锁。但四元数可以用球面线性插值（SLERP），平滑得很。
四元数表示旋转的公式很简单：p' = q * p * q^{-1}。其中q是单位四元数，p是纯虚四元数（把三维向量塞进去）。注意，这里一定要用单位四元数，否则旋转完长度会变。
// 四元数旋转示例（C++伪代码）
Eigen::Quaterniond q(0.707, 0.707, 0, 0); // 绕x轴转90度
Eigen::Vector3d p(1, 0, 0); // 原始点
Eigen::Vector3d p_rotated = q * p; // 旋转后的点
// 结果应该是 (1, 0, 0) 绕x轴转90度，不变
我个人的习惯是，在SLAM系统里，状态变量用四元数，误差变量用旋转向量。为什么？因为四元数做全局表示没有奇点，而旋转向量做局部扰动（比如优化时的增量）很方便。这个技巧在很多开源VIO系统里都能看到，比如VINS-Mono。
小技巧：四元数乘法不满足交换律，顺序很重要。我刚开始写代码时，经常把 q1 * q2 和 q2 * q1 搞混。记住：q1 * q2 表示先转q2再转q1。
5. 它们之间的转换
实际工程中，这四种表示法经常需要互相转换。比如，你从IMU读到的可能是欧拉角，但优化时需要四元数。我整理了一个转换关系图，方便大家理解。
从图上可以看出，四元数和旋转矩阵之间可以直接转换，没有奇点。而旋转向量和四元数之间通过指数/对数映射。欧拉角呢？它和旋转向量之间需要指定旋转顺序（比如ZYX），否则结果不唯一。
6. 实战中的选择建议
说了这么多，到底该用哪个？我给大家一个实用建议：
- 做SLAM前端（特征匹配、位姿估计）：用四元数。因为要频繁做插值和优化，四元数最稳定。
- 做SLAM后端（图优化、BA）：用四元数+旋转向量。四元数做全局变量，旋转向量做局部扰动。
- 做可视化、人机交互：用欧拉角。操作员看着直观，但内部计算完再转成欧拉角显示。
- 做理论推导：用旋转矩阵。公式推导时矩阵形式最方便。
重要提醒：无论你用哪种表示法，一定要保持一致性。我见过一个项目，前端用四元数，后端用欧拉角，中间转换时顺序搞错了，结果整个系统跑飞了。所以，选定一种表示法，贯穿整个系统，只在必要时做转换。
好了，今天的内容就到这里。旋转表示法是SLAM的基石，搞不懂它，后面所有东西都是空中楼阁。我建议大家动手写写代码，把四种表示法互相转换的函数实现一遍。相信我，写一遍比看十遍都管用。
公众号:蓝海资料掘金营，微信deep3321