数学基础回顾（一）：三维空间刚体运动
各位同学，欢迎来到VIO实战教程的第二讲。今天咱们聊聊三维空间里的刚体运动。说实话，这部分内容看着有点枯燥，但它是整个VIO系统的地基。地基没打牢，后面盖楼肯定歪。
我记得刚入行那会儿，总觉得旋转矩阵、四元数这些东西太抽象，不就是个姿态嘛，搞那么复杂干嘛？直到第一次在无人机上跑VIO，发现姿态解算疯狂漂移……嗯，从那以后我老老实实把数学基础补了一遍。
1. 旋转矩阵：最直观的表示
先说说旋转矩阵。说白了，它就是描述一个坐标系相对于另一个坐标系的朝向。一个3x3的矩阵，满足两个性质：正交（RTR = I）且行列式为+1。
为什么用矩阵？因为方便啊。你想想看，一个点p在坐标系A下的坐标，想转到坐标系B下，直接乘个R就完事了：
p_B = R * p_A简洁、优雅。但问题来了——旋转矩阵有9个参数，实际自由度只有3个。这就意味着参数冗余，优化的时候容易出问题。我在做SLAM后端优化时，直接用旋转矩阵做变量，结果协方差矩阵奇异了……后来才明白，冗余参数会引入额外的约束。
核心要点：旋转矩阵是正交矩阵，RT = R-1。这个性质在后续推导中会反复用到。
2. 旋转向量：最紧凑的表示
旋转向量，也叫轴角表示。一个向量，方向代表旋转轴，模长代表旋转角度。只有3个参数，没有冗余。
怎么理解？想象你拿着一根筷子，绕着它转某个角度。这根筷子的方向就是旋转轴，转的角度就是旋转角。就这么简单。
从旋转向量到旋转矩阵，用罗德里格斯公式：
R = cosθ * I + (1 - cosθ) * nnT + sinθ * n^其中n是单位旋转轴，θ是旋转角，n^是n的反对称矩阵。
个人经验：旋转向量在插值的时候很方便。比如你要在两个姿态之间做平滑过渡，直接用旋转向量线性插值，比四元数插值简单多了。但要注意，旋转角超过π时会有歧义，我一般限制在[-π, π]范围内。
3. 四元数：最实用的表示
四元数，这玩意儿刚接触时真让人头大。一个标量加三个虚部，q = [w, x, y, z]T，满足||q|| = 1。
为什么VIO里普遍用四元数？三个原因：
- 无奇点：欧拉角有万向锁，四元数没有
- 运算高效：乘法、插值都比旋转矩阵快
- 便于优化：单位四元数在流形上，可以用李代数处理
四元数乘法要特别注意顺序。我见过太多人搞混了：
q1 * q2 表示先转q2再转q1（注意！不是先转q1）避坑指南：我曾经在代码里把四元数乘法的顺序写反了，结果IMU预积分的结果全错了，排查了两天才找到问题。记住：四元数乘法不满足交换律，顺序很重要！
4. 欧拉角：最直观但最坑
欧拉角，用三个角度表示旋转：偏航(yaw)、俯仰(pitch)、横滚(roll)。直观是直观，但坑也最多。
最大的坑——万向锁。当俯仰角达到±90°时，偏航和横滚的旋转轴重合，丢失一个自由度。你想想看，无人机在做特技飞行时，如果姿态解算用欧拉角，万向锁一出现，控制直接炸了。
所以我的建议是：人机交互时用欧拉角，内部计算千万别用。
| 表示方法 | 参数数量 | 有无奇点 | 运算效率 | 适用场景 |
|---|---|---|---|---|
| 旋转矩阵 | 9 | 无 | 中等 | 坐标变换 |
| 旋转向量 | 3 | 有(θ=π) | 高 | 插值、优化 |
| 四元数 | 4 | 无 | 高 | SLAM、VIO |
| 欧拉角 | 3 | 有(万向锁) | 低 | 人机交互 |
5. 李群与李代数：为什么需要它？
好了，前面四种表示方法各有优缺点。但在VIO中，我们面临一个核心问题：如何对旋转进行优化？
你想想看，旋转矩阵是正交的，你没法直接加一个增量然后保持正交性。四元数有单位约束，直接加法会破坏归一化。这就尴尬了——优化算法通常要求变量在欧氏空间里。
李群和李代数就是来解决这个问题的。
- SO(3)：三维旋转群，就是所有旋转矩阵的集合
- se(3)：SO(3)对应的李代数，是三维向量空间
- SE(3)：三维刚体变换群，包含旋转和平移
- se(3)：SE(3)对应的李代数
核心思想：在李代数上做加法（无约束优化），然后映射回李群（保证约束）。
李代数(无约束) --指数映射--> 李群(有约束)
李群(有约束) --对数映射--> 李代数(无约束)说白了，就是把有约束的优化问题，转化成无约束的优化问题。这个技巧在VIO的BA优化中无处不在。
我的理解：李代数就像是在切空间里做优化。你站在曲面上的一点，沿着切方向走一小步，再投影回曲面。这样既保证了约束，又能用标准的优化算法。我在做VIO后端优化时，所有姿态变量都用李代数表示，收敛速度比直接用四元数快了一倍。
6. 知识体系总览
下面这张图总结了本章的核心逻辑：
这张图把本章的逻辑串起来了。从四种旋转表示出发，引出优化难题，最后落到李群李代数这个解决方案上。后面的课程中，我们会反复用到这个框架。
学习建议：别急着把公式全背下来。先理解每种表示方法的特点和适用场景。等后面做VIO代码实现时，再回头查公式，效果会好很多。
好了，这一讲就到这里。数学基础是枯燥的，但它是VIO的骨架。下一讲我们会继续深入，聊聊李代数的求导和扰动模型——这部分在VIO的雅可比计算中至关重要。