3. 旋转与平移：旋转矩阵、欧拉角、四元数、平移向量、齐次坐标与变换矩阵
说到外参标定，绕不开的就是旋转和平移。说白了，我们要描述两个坐标系之间怎么转、怎么挪。我刚开始做SLAM那会儿，觉得这些数学工具就是些公式，后来踩坑多了才明白——选错表达方式，轻则代码跑不通，重则定位直接炸掉。
这一节，我把最常用的几种表达方式掰开揉碎讲清楚。你想想看，一个IMU和相机的相对位姿，你用什么来表示？旋转矩阵？四元数？还是欧拉角？各有各的脾气。
3.1 旋转矩阵：最直观，但最啰嗦
旋转矩阵是个3x3的方阵，记作 R。它描述了一个坐标系相对于另一个坐标系的朝向。比如，相机坐标系到IMU坐标系的旋转，就可以用一个R来表示。
它的好处是直观。一个点p在相机系下的坐标是p_c，在IMU系下就是 p_i = R * p_c。直接矩阵乘法，干净利落。
但坏处也很明显——冗余。9个元素，实际上只有3个自由度。你想想，每次优化要维护9个参数，还得保证它是正交矩阵（R^T R = I），这约束条件写起来就够烦的。
我在项目中遇到过：有一次做手眼标定，直接用旋转矩阵做优化变量。结果迭代几次后矩阵不再正交，投影出来的点全歪了。后来我学乖了，要么用四元数，要么加正交约束。
3.2 欧拉角：直观但小心万向锁
欧拉角用三个角度表示旋转：绕X轴转多少（roll）、绕Y轴转多少（pitch）、绕Z轴转多少（yaw）。
这玩意儿在调试时特别方便。我跟算法同事沟通时，常说“相机绕Z轴偏了5度”，而不是说“四元数的实部是多少”。
但欧拉角有个臭名昭著的问题——万向锁。当pitch接近±90度时，roll和yaw会变得无法区分。说白了，你丢失了一个自由度。
我曾经在无人机飞控里见过这问题。飞机垂直爬升时，姿态解算突然跳变，差点炸机。从那以后，我在任何需要连续旋转估计的场景里，都优先用四元数。
3.3 四元数：SLAMer的标配
四元数是个超复数，形式是 q = w + xi + yj + zk，其中 w 是实部，x,y,z 是虚部。它用4个参数表示旋转，没有冗余，也没有万向锁。
我个人习惯用四元数做状态估计。为什么呢？
- 紧凑：4个参数，比旋转矩阵的9个少一半多
- 无奇异性：不会像欧拉角那样突然死掉
- 容易插值：做平滑旋转时，球面线性插值（SLERP）非常自然
但四元数也有坑。比如，q 和 -q 表示同一个旋转。你优化时如果不加约束，它可能在正负之间来回跳。嗯，这里要注意，通常我们会固定实部为正，或者在代价函数里加个符号约束。
// 四元数乘法表示旋转叠加
// q1 表示从A到B的旋转，q2 表示从B到C的旋转
// 那么从A到C的旋转是 q = q2 * q1
Eigen::Quaterniond q1(0.707, 0.707, 0, 0); // 绕Z轴转90度
Eigen::Quaterniond q2(1, 0, 0, 0); // 单位四元数
Eigen::Quaterniond q = q2 * q1;
// 注意：Eigen中四元数乘法顺序是 q2 * q1，不是 q1 * q2
3.4 平移向量：简单但别搞错坐标系
平移向量 t 就是个3维向量，表示从一个坐标系原点到另一个坐标系原点的位移。比如，t 表示相机原点在IMU坐标系下的位置。
这里最容易犯的错误是——平移向量是在哪个坐标系下表达的？
我见过有人把平移向量直接当成“从A到B的向量”，结果坐标变换时符号搞反。记住：平移向量通常是在目标坐标系下表达的。比如，从相机系到IMU系的变换，平移向量 t 是在IMU系下的相机位置。
一个小技巧：写代码时，把平移向量的含义写在注释里。比如 // t: camera position in IMU frame。这样三个月后回来看代码，不会骂自己。
3.5 齐次坐标与变换矩阵：把旋转和平移打包
齐次坐标就是在普通3D坐标后面加个1，变成4维向量 [x, y, z, 1]^T。变换矩阵 T 是个4x4矩阵，左上角3x3是旋转矩阵R，右上角3x1是平移向量t，最后一行是 [0, 0, 0, 1]。
为什么要这么搞？因为这样可以把旋转和平移合并成一次矩阵乘法：
| R t | | p | | R*p + t |
| 0 1 | * | 1 | = | 1 |
说白了，一次乘法搞定坐标变换。而且多个变换可以连乘：T13 = T12 * T23。这在做传感器融合时特别有用——把IMU、相机、激光雷达的位姿串成一条链。
我在做多传感器标定时，习惯把所有外参都存成4x4的变换矩阵。这样不管是正向变换还是反向变换，直接求逆就行（T的逆就是 [R^T, -R^T * t; 0, 1]）。
3.6 知识体系总览
下面这张图把几种表达方式的关系画清楚了。你一看就明白，它们之间可以互相转换，但各有适用场景。
3.7 实际标定中的选择建议
说了这么多，到底怎么选？我根据自己的经验，给个实用建议：
| 场景 | 推荐表达 | 原因 |
|---|---|---|
| 状态估计（EKF/图优化） | 四元数 | 无奇异性，紧凑，容易做误差状态 |
| 手眼标定 | 变换矩阵 T | 方便连乘和求逆，AX=XB形式直接解 |
| 调试可视化 | 欧拉角 | 一眼看出偏了多少度 |
| 坐标变换计算 | 齐次坐标 | 一次乘法搞定，代码简洁 |
| 存储/传输 | 四元数+平移向量 | 7个浮点数，比4x4矩阵省空间 |
我个人习惯：在代码内部统一用四元数+平移向量做优化，只在输出和调试时转成欧拉角或变换矩阵。这样既避免了万向锁，又方便人看。
好了，旋转和平移的数学工具就这些。你只要记住——四元数做优化，欧拉角做调试，变换矩阵做计算。下次标定时，别再被这些表达方式绕晕了。