3、旋转与平移：旋转矩阵、四元数、欧拉角、平移向量
说到坐标变换，绕不开的就是旋转和平移。这两个东西，说白了就是描述一个物体「怎么转」和「怎么挪」。我见过太多新手，甚至有些老手，在这上面栽跟头。今天咱们就把这四种表达方式掰扯清楚。
3.1 旋转矩阵：最直观，但也最啰嗦
旋转矩阵是个3x3的方阵。它描述了一个坐标系相对于另一个坐标系的朝向。你想想看，一个坐标系的三根轴，在另一个坐标系里怎么表示？把这三根轴的方向余弦拼在一起，就是旋转矩阵。
数学形式：
R = [r11 r12 r13]
[r21 r22 r23]
[r31 r32 r33]它的性质很漂亮：正交矩阵，行列式为+1。这意味着它的逆等于它的转置。嗯，这个性质在实际计算中非常有用。
R.T 或者 R.inverse()，前提是你确认它确实是正交矩阵。我曾经因为一个数值误差，导致矩阵不正交，结果整个SLAM系统跑飞了。
3.2 四元数：SLAM里的香饽饽
四元数，很多人一听就头大。其实它没那么可怕。说白了，四元数就是一个超复数，由实部和三个虚部组成：
q = w + xi + yj + zk其中 w 是实部，x, y, z 是虚部。约束条件：w² + x² + y² + z² = 1。
为什么SLAM里爱用四元数？
- 没有万向锁问题（欧拉角的死穴）
- 插值平滑（做轨迹平滑时特别好用）
- 计算效率高（比旋转矩阵少几个乘法）
3.3 欧拉角：直观但危险
欧拉角用三个角度描述旋转：偏航（Yaw）、俯仰（Pitch）、滚转（Roll）。听起来很直观对吧？但我要说，能不用就别用。
为什么？因为万向锁。当俯仰角接近±90°时，偏航和滚转就分不清了。你想想看，飞机垂直向上飞的时候，你还能分清它是在转弯还是在翻滚吗？
| 表示方式 | 优点 | 缺点 |
|---|---|---|
| 旋转矩阵 | 直观，易组合 | 9个参数，冗余 |
| 四元数 | 无奇点，易插值 | 不够直观 |
| 欧拉角 | 直观，参数少 | 万向锁，不唯一 |
3.4 平移向量：最简单，也最容易出错
平移向量就是个3x1的向量，表示从原点到目标点的位移。听起来简单吧？但坑就在「从哪到哪」。
在SLAM里，我们经常看到这样的写法：
T_wc = [R_wc, t_wc]
[0, 1 ]这里的 t_wc 表示：从相机坐标系原点，到世界坐标系原点，在相机坐标系下的表示。嗯，有点绕。我换个说法：
t_wc：世界原点在相机坐标系下的坐标t_cw：相机原点在世界坐标系下的坐标
这两个互为相反数吗？不！它们之间还隔着旋转：
t_cw = -R_cw * t_wc3.5 四种方式的相互转换
在实际工程中，我们经常需要在四种表示方式之间切换。下面是我常用的转换关系：
// 四元数 -> 旋转矩阵 (Eigen)
Eigen::Matrix3d R = q.toRotationMatrix();
// 旋转矩阵 -> 欧拉角
Eigen::Vector3d euler = R.eulerAngles(2, 1, 0); // ZYX顺序
// 欧拉角 -> 四元数
Eigen::Quaterniond q =
Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());3.6 知识体系总览
下面这张图，是我自己总结的旋转与平移知识体系。每次排查坐标变换错误时，我都会对照着看一遍：
嗯，这张图我每次做坐标变换调试时都会看一遍。特别是当SLAM系统出现漂移或者地图错位时，按这个清单检查，十有八九能找到问题。
最后说一句：不要相信直觉，要相信数学。坐标变换的坑，我踩过太多。每次觉得「应该没问题」的时候，往往就是出问题的时候。老老实实把变换矩阵写出来，一步一步验证，才是正道。