数学基础（上）：三维空间刚体运动
做SLAM这么多年，我最大的感受是：数学基础决定了你能走多远。很多同学一上来就调库、跑开源，遇到问题就懵了。说白了，SLAM的核心就是数学建模——你用什么方式描述机器人的运动？怎么估计它的位姿？这些问题的答案，都藏在我们今天要聊的内容里。
这一章，我们聚焦三维空间中的刚体运动。嗯，刚体就是不会变形的物体——你的手机、无人机、汽车，都可以近似看作刚体。我们要解决的核心问题是：如何用数学精确描述一个刚体在三维空间中的旋转和平移。
核心知识点速览
- 旋转矩阵：最直观的旋转表示，但参数冗余
- 四元数：紧凑、无奇异性，工程首选
- 李群与李代数：解决优化问题的利器
- 指数映射：连接李代数与李群的桥梁
1. 旋转矩阵：最直观，但别被它骗了
旋转矩阵是描述旋转最直接的方式。一个 3×3 的正交矩阵，行列式为 +1，就构成了 SO(3) 群。你想想看，把一个向量从坐标系 A 转到坐标系 B，不就是左乘一个旋转矩阵吗？
但我在实际项目中踩过坑——旋转矩阵有 9 个参数，但自由度只有 3。这意味着什么？意味着你优化的时候要额外施加约束，否则矩阵会「漂移」成非正交的。嗯，这很麻烦。
我的建议：在 SLAM 前端（帧间匹配）可以用旋转矩阵，因为直观好理解。但到了后端优化，赶紧换成四元数或李代数，否则约束条件会让你头疼。
2. 四元数：工程界的宠儿
四元数这东西，刚接触时觉得玄乎。一个标量加三个虚部，怎么就表示旋转了？说白了，它就是轴角表示法的升级版——绕单位轴 n 旋转 θ 角，对应四元数 q = [cos(θ/2), n·sin(θ/2)]。
为什么工程界偏爱四元数？三个理由：
- 紧凑：4 个参数，没有冗余
- 无奇异性：不会像欧拉角那样出现万向锁
- 插值方便：球面线性插值（SLERP）做相机轨迹平滑，效果极好
我记得有一次做无人机 SLAM，用欧拉角做姿态估计，飞到某个角度时突然炸了——万向锁导致控制失效。从那以后，我所有涉及旋转的项目，内部计算一律用四元数，只在可视化时才转成欧拉角给人看。
注意：四元数乘法不满足交换律！q1 * q2 和 q2 * q1 结果不同。我见过不少新手在这里栽跟头，调试半天发现是乘法顺序反了。
3. 李群与李代数：优化问题的救星
好，现在进入重头戏。李群和李代数，听起来高大上，其实解决的是一个实际问题：如何在流形上做优化。
你想想看，旋转矩阵 SO(3) 是一个流形，不是欧氏空间。你在上面做加法？不行，两个旋转矩阵相加就不是旋转矩阵了。那梯度下降怎么做？
答案就是李代数。SO(3) 对应的李代数是 so(3)，它是一个三维向量空间。说白了，李代数就是李群在单位元处的切空间。我们在李代数上做优化（加法、梯度下降），然后通过指数映射回到李群。
我个人习惯用 Sophus 库，它封装了 SO(3) 和 SE(3) 的常用操作。代码写起来很清爽：
#include <sophus/so3.hpp>
#include <sophus/se3.hpp>
// 创建旋转矩阵
Eigen::Matrix3d R = Eigen::AngleAxisd(M_PI/4, Eigen::Vector3d::UnitZ()).toRotationMatrix();
Sophus::SO3d SO3_R(R); // 从旋转矩阵构造
// 李代数（三维向量）
Eigen::Vector3d so3 = SO3_R.log(); // 对数映射
std::cout << "so3 = " << so3.transpose() << std::endl;
// 指数映射回来
Sophus::SO3d SO3_updated = Sophus::SO3d::exp(so3 + update);
核心公式：指数映射 exp(φ^) = I + sinθ/θ · φ^ + (1-cosθ)/θ² · (φ^)²，其中 φ 是 so(3) 中的向量，θ = ||φ||。
4. SE(3) 的指数映射：带上平移一起玩
实际 SLAM 中，我们不仅要旋转，还要平移。SE(3) 就是旋转加平移的完整刚体变换。它的李代数是 se(3)，一个六维向量（3 旋转 + 3 平移）。
指数映射从 se(3) 到 SE(3) 的公式稍微复杂一点，但原理一样。我建议你记住一个关键点：se(3) 的指数映射不是简单的旋转指数映射 + 平移，而是有一个耦合项。具体来说：
exp(ξ^) = [ exp(φ^) V·ρ ]
[ 0^T 1 ]
其中 V = I + (1-cosθ)/θ² · φ^ + (θ-sinθ)/θ³ · (φ^)²
这个 V 矩阵，说白了就是平移部分受旋转影响的修正项。我在做视觉惯性里程计（VIO）时，这个公式每天都要打交道。
避坑指南：我曾经在优化时直接对 SE(3) 的 12 个参数（3×4 矩阵）做加法，结果变换矩阵变得不伦不类。后来老老实实用李代数做扰动模型，问题就解决了。记住：在流形上优化，永远用李代数。
5. 实践中的选择建议
讲了这么多，你可能想问：实际项目中到底用哪个？我根据经验给个参考：
| 场景 | 推荐表示 | 原因 |
|---|---|---|
| 前端特征匹配 | 旋转矩阵 | 直观，方便与图像坐标转换 |
| 后端图优化 | 李代数（so3/se3） | 无约束优化，收敛快 |
| 姿态滤波（EKF） | 四元数 | 插值方便，无奇异性 |
| 人机交互显示 | 欧拉角 | 人类友好，但仅用于显示 |
嗯，这一章的内容就到这里。数学基础是 SLAM 的根基，我建议你亲手推导一遍指数映射公式，再写代码验证。只有自己推过、写过，才能真正理解。
一句话总结：旋转矩阵是表象，四元数是工具，李群李代数是灵魂。三者缺一不可。