第四章 坐标系与刚体运动：世界坐标系、相机坐标系、IMU坐标系、旋转矩阵、四元数、李群李代数
做SLAM这么多年，我越来越觉得坐标系和刚体运动是整座大厦的地基。你想想看，IMU说“我往东走了1米”，相机说“我看到特征点在右前方”，要是坐标系没对齐，这些数据就是鸡同鸭讲。今天咱们就把这块硬骨头啃下来。
4.1 三个核心坐标系
在VIO系统里，我们打交道最多的就是三个坐标系。我习惯把它们比作三个不同视角的“观察者”。
- 世界坐标系（W系）：通常选在机器人启动位置，或者GPS原点。它是绝对的参考系，IMU和相机的位姿最终都要转换到这个世界系下。
- IMU坐标系（I系）：固定在IMU芯片上。加速度计和陀螺仪测量的原始数据，都是在这个坐标系下表达的。注意，IMU的轴定义各家厂商可能不同，我踩过这个坑。
- 相机坐标系（C系）：光心为原点，Z轴指向相机前方（也有定义指向后方的，看具体模型）。图像上的像素点，最终要反投影到这个坐标系下。
核心原则：所有传感器数据，必须先统一到同一个坐标系下，才能进行融合。我个人习惯把IMU坐标系作为“中间桥梁”，因为IMU的测量频率高，适合做状态预测。
4.2 旋转矩阵：最直观的旋转表达
旋转矩阵R是一个3x3的正交矩阵，行列式为+1。它描述了一个向量在两个坐标系之间的旋转变换。
// 假设有一个点p在世界坐标系下的坐标为p_w
// 我们想把它转换到相机坐标系下
p_c = R_cw * p_w + t_cw
// 其中R_cw就是世界系到相机系的旋转矩阵
// t_cw是平移向量旋转矩阵的好处是直观，可以直接做矩阵乘法。但坏处也很明显——9个参数只有3个自由度，存在冗余。而且连续旋转时，矩阵乘法容易累积误差。我在做纯视觉里程计时，就遇到过因为旋转矩阵累积误差导致漂移的问题。
我的经验：旋转矩阵适合做“中转站”，但不适合做优化变量。因为优化时你需要保证它始终是正交矩阵，这很麻烦。
4.3 四元数：SLAM里的“万金油”
四元数q = [w, x, y, z]^T，本质上是复数在三维空间的推广。它用4个参数表示旋转，没有奇异性，而且插值方便。
为什么SLAM里大家都爱用四元数？说白了就两个原因：
- 无万向锁：欧拉角在俯仰角±90度时会丢失一个自由度，四元数不会。
- 运算高效：两个旋转的复合，四元数乘法比矩阵乘法快。
// 四元数乘法表示旋转复合
q_combined = q2 * q1 // 先旋转q1，再旋转q2
// 用四元数旋转一个向量
p' = q * p * q_conj // 其中p是纯四元数形式注意：四元数必须归一化！我曾经在优化过程中忘记归一化，结果旋转矩阵变得不伦不类，定位直接飞了。嗯，这个坑我记忆犹新。
4.4 李群与李代数：优化的利器
说到李群李代数，很多初学者会觉得抽象。其实你把它当成“旋转的另一种数学工具”就行。
- SO(3)：三维旋转群，对应旋转矩阵R。
- so(3)：SO(3)的李代数，对应三维向量φ（其实就是旋转向量）。
- SE(3)：三维刚体变换群，对应变换矩阵T。
- se(3)：SE(3)的李代数，对应六维向量ξ。
为什么要引入李代数？因为旋转矩阵R是正交矩阵，你在优化时不能直接对R做加法（R+ΔR就不是正交矩阵了）。但李代数φ是向量，可以随便加减，然后通过指数映射回到SO(3)。
// 指数映射：从李代数到李群
R = exp(φ^) // φ^是φ的反对称矩阵
// 对数映射：从李群到李代数
φ = log(R)我在做基于图优化的VIO时，就是用李代数作为优化变量。每次迭代更新δφ，然后通过指数映射更新R。这样既保证了旋转矩阵的正交性，又让优化变得简单。
4.5 坐标系转换实战：从IMU到世界
假设我们有一个IMU测量值a_imu（加速度），想转换到世界坐标系下：
- 先知道IMU当前姿态的四元数q_w_i（世界系到IMU系）。
- 用q_w_i将a_imu旋转到世界系：a_w = q_w_i * a_imu * q_w_i_conj。
- 减去重力g_w，得到真正的运动加速度。
// 伪代码
Eigen::Quaterniond q_w_i; // 当前姿态
Eigen::Vector3d a_imu; // IMU测量的加速度
Eigen::Vector3d g_w(0, 0, -9.81); // 重力
// 旋转到世界系
Eigen::Vector3d a_w = q_w_i * a_imu;
// 去除重力
Eigen::Vector3d a_motion = a_w - g_w;避坑指南：我曾经在IMU初始化时，忘记考虑重力对齐。结果IMU静止时，加速度计读数是[0, 0, 9.81]（在IMU系下），但世界系下重力是[0, 0, -9.81]。这个符号搞反了，积分出来的速度直接往天上飞。嗯，从那以后我每次都会仔细检查坐标系定义。
4.6 知识体系总览
下面这张图是我自己整理的坐标系与刚体运动知识结构，你可以把它当成学习地图：
4.7 总结与建议
说了这么多，我总结几条实战建议：
- 坐标系定义要写死：在代码里用枚举或常量定义好每个坐标系的轴方向，不要靠记忆。
- 四元数做状态传递：在EKF或因子图里，用四元数表示姿态，避免奇异性。
- 李代数做优化：在BA或图优化里，用李代数作为优化变量，保证约束自然满足。
- 多画图验证：我每次写完坐标系转换代码，都会把点云画出来看看，是不是真的转对了。
一个小技巧：如果你不确定旋转方向对不对，可以用右手定则比划一下。拇指指向旋转轴，四指弯曲方向就是正方向。这个方法我用了十年，从来没出过错。
好了，坐标系与刚体运动这部分就聊到这儿。记住，这些数学工具不是用来背的，是用来解决问题的。下次你写VIO代码时，多想想“这个旋转到底是在哪个坐标系下”，很多bug自然就解了。
公众号:蓝海资料掘金营，微信deep3321