3. 三维空间刚体运动：旋转矩阵、四元数、李群李代数
各位同学，欢迎来到第三章。
上一章我们聊了传感器和坐标系。这一章，咱们得把机器人怎么在三维空间里「动」这件事说清楚。说白了，就是怎么描述一个物体从A姿态转到B姿态。
你想想看，一个机器人要导航，它得知道自己朝哪看、往哪走。这背后全是刚体运动。我个人习惯把这一章看作整个SLAM系统的「数学地基」。地基不稳，后面全白搭。
3.1 旋转矩阵：最直观，但最啰嗦
旋转矩阵，其实就是一组正交基。它把一个向量从一个坐标系映射到另一个坐标系。
我记得刚入行时，总觉得旋转矩阵很完美。9个元素，约束条件一堆，看起来挺严谨。直到我在项目里做实时位姿优化，才发现这东西有个致命问题——冗余。
核心公式：
R * R^T = I
det(R) = 1旋转矩阵是正交矩阵，行列式为+1。这保证了它不会把物体「镜像」过去。
为什么说它啰嗦？9个参数，实际自由度只有3。你想想看，优化一个9维变量，还要时刻盯着它是不是正交矩阵，多累啊。我在做VIO初始化时，就吃过这个亏——优化器跑着跑着，矩阵就不正交了，还得强制投影回去。
我的习惯：只在理论推导时用旋转矩阵。代码里？我尽量不用。除非是简单的坐标变换，比如把相机坐标系下的点转到世界坐标系。
3.2 四元数：SLAM界的「真香」选择
四元数，说白了就是复数的升级版。一个实部加三个虚部。
q = w + xi + yj + zk
其中 i^2 = j^2 = k^2 = ijk = -1为什么SLAM里几乎都用四元数？两个原因：
- 无奇点：欧拉角有万向锁，四元数没有。你想想看，无人机做特技飞行时，俯仰角到90度，欧拉角直接崩了。四元数稳如老狗。
- 插值平滑：做轨迹平滑时，四元数的球面线性插值（Slerp）非常自然。
我曾经在项目里用欧拉角做IMU姿态解算，结果飞机翻了个跟头，角度直接跳变。从那以后，我再也不敢在代码里直接用欧拉角做运算了。
避坑指南：四元数不是唯一的！q和-q表示同一个旋转。我在做图优化时，经常忘记归一化，结果优化出来的四元数模长不是1，导致旋转矩阵不正交。嗯，这里要注意：每次更新后都要归一化。
3.3 李群李代数：SLAM优化的「加速器」
这一节可能是本章最抽象的部分。但我尽量用大白话讲清楚。
李群，就是连续光滑的群。三维旋转群SO(3)就是一个李群。李代数，是李群在单位元处的切空间。
为什么要引入李代数？因为旋转矩阵不能直接做加法。你想想看，两个旋转矩阵相加，结果大概率不是旋转矩阵。但优化器需要加法啊！
李代数解决了这个问题：
- 李代数是一个向量空间，可以自由加减
- 通过指数映射，可以把李代数映射回李群
- 通过对数映射，可以把李群映射回李代数
核心公式：
SO(3) = {R ∈ R^(3×3) | R^T R = I, det(R) = 1}
so(3) = {φ ∈ R^3 | φ^∧ ∈ R^(3×3)}
指数映射：R = exp(φ^∧)
对数映射：φ = ln(R)^∨我个人习惯在优化时，把位姿表示成李代数。这样优化变量就是无约束的向量，直接用高斯牛顿法就行。优化完再映射回旋转矩阵或四元数。
我记得第一次在g2o里用李代数做BA优化，收敛速度比用四元数快了一倍。为什么？因为李代数的雅可比矩阵计算简单，没有复杂的链式法则。
实用技巧：如果你用Ceres Solver，它内置了SO(3)和SE(3)的LocalParameterization。直接用就行，不用自己手写。但理解原理很重要，不然出bug了你都不知道怎么调。
3.4 三种表示方法的对比
| 表示方法 | 自由度 | 冗余度 | 奇点 | 优化友好度 |
|---|---|---|---|---|
| 旋转矩阵 | 3 | 6 | 无 | 差（需约束） |
| 四元数 | 3 | 1 | 无 | 中（需归一化） |
| 李代数 | 3 | 0 | 无 | 好（无约束） |
从表里能看出来，李代数在优化方面优势明显。但四元数在插值和状态传递方面更直观。所以实际项目中，我通常是：
- IMU预积分用四元数
- 图优化用李代数
- 可视化用旋转矩阵
3.5 实战：从四元数到李代数的转换
下面给一段我常用的代码片段。这段代码在VIO初始化时经常用到：
// 四元数转李代数
Eigen::Vector3d quatToLie(const Eigen::Quaterniond& q) {
// 先转旋转矩阵
Eigen::Matrix3d R = q.toRotationMatrix();
// 再转李代数
Eigen::AngleAxisd aa(R);
return aa.angle() * aa.axis();
}
// 李代数转四元数
Eigen::Quaterniond lieToQuat(const Eigen::Vector3d& phi) {
double angle = phi.norm();
if (angle < 1e-10) {
return Eigen::Quaterniond::Identity();
}
Eigen::Vector3d axis = phi / angle;
return Eigen::Quaterniond(
Eigen::AngleAxisd(angle, axis)
);
}注意：当角度接近0时，李代数的模长很小，直接除会出问题。我一般加一个阈值判断，小于1e-10就直接返回单位四元数。
3.6 本章小结
这一章内容不少，但核心就三句话：
- 旋转矩阵是基础，但别在优化里用
- 四元数是工程首选，记得归一化
- 李代数是优化利器，理解指数映射
嗯，我当年学这一块时，也是花了不少时间才搞明白。特别是李群李代数，一开始总觉得抽象。后来在项目里用多了，发现其实就是个「换坐标系」的工具。
下一章，我们会把这些数学工具用到实际的传感器数据处理中。到时候你会发现，今天学的这些东西，全都会用上。