坐标系与刚体变换基础：世界坐标系、相机坐标系、齐次坐标、李群与李代数基础
做SLAM轨迹评估，说白了就是跟坐标系打交道。我刚开始入行时，觉得坐标系这东西太简单了，不就是xyz嘛。结果第一次做多传感器融合，坐标系没对齐，轨迹跑出来像喝醉了酒——歪歪扭扭的。嗯，从那以后我再也不敢小看坐标系了。
这一章，咱们把坐标系和刚体变换的底子打扎实。你想想看，轨迹评估的本质是什么？就是比较两条轨迹在同一个坐标系下的差异。如果坐标系都搞不清楚，评估结果就是空中楼阁。
1. 世界坐标系与相机坐标系
世界坐标系，就是咱们定义的一个全局参考系。通常我们取第一帧相机的位置作为原点，或者用GPS的经纬度作为基准。我个人习惯把世界坐标系的Z轴朝上，这样符合直觉。
相机坐标系呢？它跟着相机一起运动。相机坐标系的原点在相机光心，Z轴指向相机前方，X轴向右，Y轴向下——嗯，这是OpenCV的惯例。注意了，不同库的坐标系定义可能不一样，我在项目中就吃过这个亏。
关键点：世界坐标系是静止的，相机坐标系是运动的。SLAM要做的，就是估计相机坐标系相对于世界坐标系的位姿。
举个例子：你在房间里拿着手机走一圈。房间的墙角就是世界坐标系的原点。手机每移动到一个位置，就有一个对应的相机坐标系。SLAM算法输出的，就是这些相机坐标系相对于墙角的位置和朝向。
2. 齐次坐标：为什么需要它？
齐次坐标，说白了就是给三维坐标加一个维度。原来(x, y, z)变成(x, y, z, 1)。为什么要这么干？
因为旋转和平移可以用一个4x4矩阵统一表示了。你想想看，如果没有齐次坐标，旋转用3x3矩阵，平移用3x1向量，写起来麻烦，计算起来更麻烦。有了齐次坐标，一个矩阵搞定一切。
// 齐次变换矩阵 T = [R t; 0 1]
// 将世界点 Pw 变换到相机坐标系 Pc
Pc = T * Pw
// 具体形式
// | Xc | | r11 r12 r13 tx | | Xw |
// | Yc | = | r21 r22 r23 ty | * | Yw |
// | Zc | | r31 r32 r33 tz | | Zw |
// | 1 | | 0 0 0 1 | | 1 |
小技巧：齐次坐标的最后一个分量不一定是1。如果是0，表示一个方向向量（无穷远处的点），平移对它无效。这在处理光线方向时很有用。
我曾经在写轨迹评估代码时，忘记把齐次坐标转回普通坐标，结果画出来的轨迹全在无穷远处。排查了半天才发现是齐次坐标没归一化。嗯，这种低级错误，犯一次就记住了。
3. 旋转矩阵与变换矩阵
旋转矩阵R是一个3x3的正交矩阵，满足R^T * R = I，行列式为+1。它描述了坐标系之间的旋转关系。
变换矩阵T是一个4x4的齐次矩阵，包含了旋转和平移：
T = [R t; 0 1]
// 其中 R 是 3x3 旋转矩阵
// t 是 3x1 平移向量
// 最后一行是 [0, 0, 0, 1]
在SLAM轨迹评估中，我们经常需要比较两个变换矩阵的差异。比如，估计的位姿T_est和真值位姿T_gt之间的误差。怎么比较？
常用的方法是计算相对位姿误差：
// 计算相对变换
T_rel = T_est_inv * T_gt
// 从 T_rel 中提取旋转误差和平移误差
// 旋转误差：角度
// 平移误差：距离
注意：变换矩阵的乘法不满足交换律。T1 * T2 和 T2 * T1 结果完全不同。在代码中一定要搞清楚乘法的顺序。
4. 李群与李代数基础
说到李群和李代数，很多人觉得头大。其实没那么复杂。你想想看，旋转矩阵R有9个参数，但自由度只有3。这就意味着参数之间存在约束。优化时处理这些约束很麻烦。
李代数就是来解决这个问题的。它把旋转和变换映射到一个无约束的向量空间上。说白了，就是让你可以用普通的优化方法去优化位姿。
常用的李群和李代数对应关系：
| 李群 | 李代数 | 含义 | 自由度 |
|---|---|---|---|
| SO(3) | so(3) | 三维旋转 | 3 |
| SE(3) | se(3) | 三维刚体变换 | 6 |
so(3)的元素是一个三维向量，对应旋转轴和旋转角。se(3)的元素是一个六维向量，前三维是旋转，后三维是平移。
李群和李代数之间通过指数映射和对数映射相互转换：
// 指数映射：从李代数到李群
// so(3) -> SO(3)
R = exp(phi^)
// se(3) -> SE(3)
T = exp(xi^)
// 对数映射：从李群到李代数
// SO(3) -> so(3)
phi = log(R)
// SE(3) -> se(3)
xi = log(T)
其中phi^表示phi的反对称矩阵。这个符号^在代码中经常用到，我习惯把它叫做"hat算子"。
核心思想：李代数上的加法，对应李群上的乘法。这就把位姿优化变成了一个无约束优化问题。
我在做轨迹平滑时，就用李代数来表示位姿增量。每次迭代，在se(3)上加上一个小的增量，然后通过指数映射得到新的位姿。这样既保证了位姿的合法性，又方便了优化计算。
5. 坐标系变换在轨迹评估中的应用
好了，理论说完了，咱们看看实际怎么用。轨迹评估的核心步骤：
- 对齐坐标系：估计轨迹和真值轨迹通常不在同一个坐标系下。需要先做对齐。
- 计算相对位姿：用变换矩阵计算两帧之间的相对运动。
- 比较误差：在同一个参考系下比较估计值和真值的差异。
举个例子，假设你有估计轨迹T_est和真值轨迹T_gt，都是SE(3)的序列。评估绝对轨迹误差（ATE）的步骤：
// 1. 对齐：计算一个变换 T_align，使得估计轨迹与真值轨迹对齐
// 通常用最小二乘法求解
// 2. 对齐后的估计轨迹
T_est_aligned = T_align * T_est
// 3. 计算每帧的误差
for each frame i:
error_i = T_est_aligned[i]^{-1} * T_gt[i]
// 提取平移误差和旋转误差
// 4. 统计误差
ATE_translation = sqrt(mean(||t_error||^2))
ATE_rotation = mean(angle_error)
避坑指南：我曾经在计算ATE时，忘记了对齐坐标系，结果误差大得离谱。后来才发现是坐标系没对齐。记住：任何轨迹评估的第一步，一定是坐标系对齐。
6. 知识体系总览
为了让你更直观地理解本章的知识结构，我画了一张图：
这张图把本章的知识点串起来了。从世界坐标系到相机坐标系，中间经过齐次坐标、旋转矩阵、变换矩阵，再到李群李代数，最后应用到轨迹评估中。每一步都有它的意义。
好了，坐标系和刚体变换的基础就讲到这里。这些概念在后面的轨迹评估中会反复用到。你想想看，没有这些基础，评估结果就是无源之水。我个人建议你把变换矩阵的乘法多练几遍，写代码时手到擒来。