第4章坐标系与刚体变换

4、坐标系与刚体变换：世界坐标系、相机坐标系、IMU坐标系、四元数与李代数基础

各位同学，欢迎来到第四章。说实话，坐标系和刚体变换这玩意儿，是SLAM系统里最基础但也最容易翻车的地方。我见过太多项目，标定参数算得贼漂亮，结果一跑起来轨迹乱飞，最后发现是坐标系定义搞反了。嗯，咱们今天就把这事彻底捋清楚。

做视觉惯性SLAM，说白了就是回答三个问题：我在哪？我在看哪？我在怎么动？这三个问题分别对应三个坐标系。

我个人的习惯：世界坐标系一律用Z轴向上。为什么？因为IMU的重力测量直接就是Z方向，省去一次旋转。你想想看，如果世界坐标系Y轴向上，每次把IMU的加速度投影到世界系时，多一次旋转就多一次误差。

从一个坐标系到另一个坐标系，无非就是先旋转，再平移。数学上写成：

P_w = R_wc * P_c + t_wc

其中R是旋转矩阵，t是平移向量。这里有个坑——下标顺序。R_wc表示「从相机坐标系到世界坐标系的旋转」。我曾经在代码里把R_wc和R_cw搞混，结果标定出来的外参全是错的，排查了整整两天。

避坑指南：我建议你在代码里统一用「from_to」的命名方式，比如R_from_camera_to_world。虽然名字长一点，但永远不会搞错。

旋转矩阵有9个参数，但自由度只有3，存在冗余。欧拉角虽然直观，但会碰到万向锁。所以SLAM里最常用的是四元数。

四元数长这样：

q = [w, x, y, z]

其中w是实部，x、y、z是虚部。它满足一个约束：w² + x² + y² + z² = 1。嗯，单位四元数。

为什么用四元数？三个理由：

小技巧：四元数乘法不满足交换律。q1 * q2 和 q2 * q1 结果不同。我刚开始学的时候老记反，后来干脆写了个单元测试，每次写完都跑一遍验证。

你可能会问：四元数不是挺好的吗？为什么还要搞李代数？

原因很简单——优化。在SLAM后端优化里，我们需要对位姿求导。四元数虽然有4个参数，但受限于单位长度约束，不能直接做加法。而李代数（so3）只有3个参数，无约束，可以直接加减。

李群SO(3)和李代数so(3)的关系：

// 指数映射示例（Sophus库）
Eigen::Vector3d so3(0.1, 0.2, 0.3);  // 微小旋转
Sophus::SO3d R = Sophus::SO3d::exp(so3);  // 变成旋转矩阵

核心思想：优化时我们在李代数上做加法（无约束），然后通过指数映射得到旋转矩阵。这样既保证了旋转矩阵的正交性，又避免了约束优化。说白了，就是把一个带约束的问题变成了无约束问题。

在实际系统中，我们通常需要串联多个变换。比如：

P_w = T_wc * T_ci * P_i

其中T_wc是相机到世界的变换，T_ci是IMU到相机的变换（这就是我们标定要算的东西）。

整个链条长这样：

IMU坐标系 → 相机坐标系 → 世界坐标系
   T_ci          T_wc

这里有个关键点：IMU和相机之间的变换是固定的，一旦标定好就不变了。而相机到世界的变换每帧都在变。

我曾经踩过的坑：在初始化时，我直接用第一帧的相机位姿作为世界坐标系原点，但忘了把IMU的初始姿态对齐到世界系。结果前几秒的轨迹全是歪的。正确的做法是：第一帧时，把IMU的姿态设为单位四元数，然后所有后续位姿都相对于这个初始姿态。

下面这张图总结了本章的核心逻辑：

最后，给你几个我在实际项目中总结的经验：

一个小工具：我写了个Python脚本，输入任意两个坐标系之间的旋转矩阵，自动输出对应的四元数、欧拉角和轴角。每次标定完都用它交叉验证，确保没算错。

好了，坐标系和刚体变换就讲到这里。这些东西看着简单，但真到了实际系统里，每一个细节都可能让你调试到怀疑人生。记住：坐标系定义清楚，SLAM就成功了一半。