3. 3D空间刚体运动：旋转矩阵、四元数、欧拉角、李群与李代数基础
各位同学，欢迎来到我们实战课的第一章。说实话，每次讲这部分内容，我都挺感慨的。刚入行那会儿，我觉得SLAM就是跑跑开源算法，调调参数。直到有一次，我在一个AGV项目里，因为四元数搞反了顺序，导致机器人原地转圈，差点撞到货架。从那以后，我才真正重视起这些“枯燥”的数学基础。
今天我们要聊的，是3D空间中刚体运动的几种数学表达。说白了，就是回答一个问题：一个物体在三维空间里，怎么描述它“动了”以及“转到哪了”？ 你想想看，无论是视觉SLAM还是激光雷达建图，核心都在处理这个问题。
核心观点： 旋转矩阵、四元数、欧拉角、李群与李代数，它们不是互相替代的关系，而是不同场景下的不同工具。理解它们的优缺点，比死记硬背公式重要得多。
3.1 旋转矩阵：最直观，但最“浪费”
旋转矩阵，其实就是3x3的矩阵，属于SO(3)群。它满足两个性质：行列式为+1，且是正交矩阵（转置等于逆）。
我个人习惯，在写代码时用旋转矩阵做坐标变换。为什么呢？因为矩阵乘法可以直接把点从相机坐标系转到世界坐标系，非常直接。
// C++ 示例：用旋转矩阵做坐标变换
Eigen::Matrix3d R; // 旋转矩阵
Eigen::Vector3d t; // 平移向量
Eigen::Vector3d p_cam; // 相机坐标系下的点
// 变换到世界坐标系
Eigen::Vector3d p_world = R * p_cam + t;我的经验： 旋转矩阵有9个参数，但自由度只有3。这意味着它存在冗余。在优化问题中，直接优化9个参数会引入额外的约束，导致数值不稳定。所以，旋转矩阵适合做变换，不适合做优化变量。
3.2 四元数：SLAM中的“万金油”
四元数，说白了就是一个实部加三个虚部：q = w + xi + yj + zk。它只有4个参数，没有奇点问题。我在做视觉SLAM时，几乎所有的位姿估计都用四元数。
为什么会这样？因为四元数插值非常平滑。你想想看，如果两个关键帧之间要插值出中间位姿，用欧拉角会出问题，用旋转矩阵又太复杂。四元数的球面线性插值（Slerp）刚好解决这个问题。
// C++ 示例：四元数初始化与旋转
Eigen::Quaterniond q(1.0, 0.0, 0.0, 0.0); // 单位四元数
q = Eigen::Quaterniond(0.707, 0.707, 0.0, 0.0); // 绕Z轴旋转90度
// 用四元数旋转一个点
Eigen::Vector3d v(1, 0, 0);
Eigen::Vector3d v_rot = q * v; // 注意：Eigen重载了*运算符避坑指南： 我曾经在项目中犯过一个低级错误——四元数的顺序搞反了。Eigen中四元数的构造函数是(w, x, y, z)，但有些库是(x, y, z, w)。一定要确认你用的库的约定！ 否则你会发现机器人朝完全相反的方向运动。
3.3 欧拉角：直观，但别在SLAM里用
欧拉角用三个角度表示旋转：偏航(Yaw)、俯仰(Pitch)、横滚(Roll)。非常直观，适合给人看。但它在SLAM里几乎是个“坑”。
最大的问题就是万向锁。当俯仰角达到±90度时，偏航和横滚会失去一个自由度。我记得有一次调试无人机，用欧拉角做姿态控制，结果飞机翻了个跟头——就是因为万向锁导致控制失效。
| 表达方式 | 参数数量 | 优点 | 缺点 | SLAM中使用 |
|---|---|---|---|---|
| 旋转矩阵 | 9 | 变换直接 | 冗余，有约束 | 坐标变换 |
| 四元数 | 4 | 无奇点，可插值 | 不够直观 | 位姿估计 |
| 欧拉角 | 3 | 直观 | 万向锁 | 可视化 |
| 李代数 | 3 | 无约束优化 | 需要指数映射 | BA优化 |
3.4 李群与李代数：优化的“秘密武器”
这部分可能是最抽象的，但也是最有用的。李群SO(3)是旋转矩阵的集合，而李代数so(3)是三维向量的集合。它们之间通过指数映射和对数映射关联。
说白了，李代数解决了优化中的一个核心问题：旋转矩阵的优化是有约束的（正交且行列式为1），而李代数是无约束的。在SLAM的后端优化（Bundle Adjustment）中，我们都是在李代数上做优化，然后映射回旋转矩阵。
// C++ 示例：李代数到旋转矩阵的指数映射
Eigen::Vector3d omega(0.1, 0.2, 0.3); // 李代数 so(3)
Eigen::Matrix3d R = Sophus::SO3d::exp(omega).matrix(); // 指数映射
// 旋转矩阵到李代数的对数映射
Eigen::Vector3d omega2 = Sophus::SO3d::log(Sophus::SO3d(R));核心理解： 李代数对应的是“旋转的导数”。在优化时，我们求解的是李代数上的增量，然后通过指数映射更新旋转矩阵。这样既保证了旋转矩阵的约束，又让优化变得简单。
3.5 实战中的选择建议
嗯，说了这么多，到底该用哪个？我给大家一个简单的建议：
- 做坐标变换：用旋转矩阵，直接乘就行
- 做位姿估计和滤波：用四元数，EKF和ESKF都支持
- 给人看：转成欧拉角，但注意万向锁
- 做优化：用李代数，这是SLAM后端的标准做法
我的习惯： 在代码内部，我统一用四元数+平移向量表示位姿。只有在需要做BA优化时，才转成李代数。这样既保证了代码的可读性，又兼顾了优化的效率。
好了，这一章的内容就到这里。记住，这些数学工具没有好坏之分，只有合不合适。在实际项目中，多试试不同的表达方式，你自然就能找到最适合的那一个。