2. 坐标系与刚体运动：世界坐标系、IMU坐标系、相机坐标系、刚体旋转与平移、四元数与李代数基础

说实话，做VIO初始化，最绕不开的就是坐标系。我见过不少新手，代码写了大半年，最后发现IMU和相机之间的外参标定反了，整个系统直接崩掉。所以这一章，咱们把坐标系和刚体运动的基础打扎实。

2.1 三个核心坐标系

VIO系统里，我们天天跟三个坐标系打交道。你想想看，IMU在动，相机在拍，世界坐标系是锚点。这三者之间的关系，就是VIO要解的核心问题。

2.1.1 世界坐标系 (World Frame, W)

世界坐标系是固定的。我习惯把它定义在IMU启动时的位置，或者某个已知的地面点。它提供绝对参考，重力方向通常对齐Z轴向下。嗯，这里要注意：不同数据集的定义可能不同，比如EuRoC数据集和TUM数据集，重力方向就不一样。

2.1.2 IMU坐标系 (Body Frame, B)

IMU坐标系跟着传感器走。它的三轴定义通常是：X向前，Y向左，Z向上（右手系）。但实际硬件不一定按这个来。我在项目中遇到过一款IMU，它的Z轴是向下的，结果加速度计读数全是反的。所以拿到新硬件，第一件事就是确认坐标系定义。

2.1.3 相机坐标系 (Camera Frame, C)

相机坐标系的原点在光心，Z轴指向相机前方（也就是拍摄方向）。X向右，Y向下。这个坐标系跟图像像素坐标之间的转换，就是内参矩阵的事。说白了，相机坐标系是连接3D世界和2D图像的桥梁。

2.2 刚体旋转与平移

刚体运动，说白了就是物体在空间里又转又移，但形状不变。旋转和平移是分开处理的，因为旋转是非线性的，处理起来比平移麻烦得多。

2.2.1 旋转矩阵

旋转矩阵R是一个3x3的正交矩阵，行列式为+1。它把向量从一个坐标系转到另一个坐标系。比如vB = RBC · vC，就是把相机坐标系下的向量转到IMU坐标系下。

我刚开始做SLAM时，总搞混旋转矩阵的转置和逆。其实对于旋转矩阵，RT = R-1。这个性质在代码里经常用到，比如从世界到IMU的旋转，反过来就是转置一下。

2.2.2 平移向量

平移就简单多了，就是三个方向的位移。但要注意：平移向量通常是在目标坐标系下表达的。比如tWB表示IMU原点在世界坐标系下的位置。

2.3 四元数

四元数？很多人一听就头大。其实它就是个更紧凑的旋转表示方式。一个四元数q = [w, x, y, z]，其中w是实部，x,y,z是虚部。它没有万向锁问题，插值也平滑，所以VIO里几乎都用它。

我记得第一次手写四元数乘法时，把顺序搞反了，结果旋转方向全错了。四元数乘法不满足交换律，q1 * q2和q2 * q1结果不同。这个坑，我踩过。

2.3.1 四元数到旋转矩阵

转换公式长这样：

R = | 1-2(y²+z²)   2(xy-wz)     2(xz+wy)   |
    | 2(xy+wz)     1-2(x²+z²)   2(yz-wx)   |
    | 2(xz-wy)     2(yz+wx)     1-2(x²+y²) |

嗯，不用硬背。Eigen库里有现成的.toRotationMatrix()方法。但理解原理很重要，调试时能帮你快速定位问题。

2.4 李代数基础

李代数？听起来高大上。其实它解决的是旋转的求导问题。旋转矩阵是正交的，直接求导很麻烦。李代数把旋转映射到三维向量空间，求导就变成了普通的向量微积分。

2.4.1 so(3)与SE(3)

so(3)对应旋转，是三维向量。SE(3)对应刚体变换，是六维向量（三维旋转+三维平移）。在VIO初始化里，我们经常用SE(3)来表示相机或IMU的位姿。

李代数到李群的指数映射：R = exp(φ^)，其中φ是so(3)向量，φ^是它的反对称矩阵。这个映射把旋转向量变成了旋转矩阵。

2.5 知识体系总览

下面这张图，是我自己总结的坐标系与刚体运动的知识结构。每次带新人，我都会先让他们看这个图，建立整体认知。

2.6 实践中的坐标系管理

说了这么多理论，来点实际的。我在写VIO代码时，会遵循几条铁律：

1. 统一命名规范：所有坐标系变换都用R_AB表示从B到A的旋转。比如R_WB是IMU到世界。
2. 外参标定文件：IMU到相机的变换T_BC，一定要单独存一个文件。我见过有人写死在代码里，换硬件就崩。
3. 单元测试：写一个测试函数，验证坐标系变换的一致性。比如把一个点从相机转到世界，再转回来，看是否回到原点。

好了，坐标系和刚体运动的基础就这些。这些概念会贯穿整个VIO初始化过程。下一节我们聊IMU的测量模型和预积分，到时候你会看到，坐标系变换无处不在。

---