坐标系与刚体运动：世界坐标系、IMU坐标系、相机坐标系、四元数与旋转矩阵、李群与李代数基础

说实话，搞视觉惯性导航这么多年，我最大的体会就是——坐标系搞不清楚，后面全白搭。你想想看，IMU说我在动，相机说我也在动，但这两个「动」到底是不是一回事？嗯，这就要从坐标系说起。

1. 世界坐标系：我们到底在哪儿？

世界坐标系，说白了就是整个系统的「绝对参考系」。我习惯把它定义为东北天坐标系，也就是x轴指向东，y轴指向北，z轴指向天。为什么这么选？因为IMU的加速度计测的是重力方向，而重力正好沿着z轴向下，这样处理起来特别方便。

不过要注意，世界坐标系其实是个「虚拟」的东西。你没法真的在地上画个坐标轴。它只是我们用来描述机器人位置的一个基准。我在做第一个VINS项目时，就犯过这个错——把起始时刻的IMU坐标系当成了世界系，结果跑了几分钟，位置漂移得一塌糊涂。

3. 相机坐标系：眼睛看到的世界

相机坐标系就更有意思了。它通常定义在相机的光心位置，z轴指向相机前方，x轴向右，y轴向下。为什么y轴向下？因为图像坐标系的原点在左上角，y轴是朝下的，这样方便投影计算。

我遇到过一个问题：相机和IMU之间的外参标定。说白了，就是搞清楚相机坐标系和IMU坐标系之间的旋转和平移关系。这个搞不定，后面所有数据融合都是扯淡。我曾经花了两周时间调一个标定板，最后发现是相机坐标系定义反了——嗯，这种坑踩过一次就记住了。

4. 四元数与旋转矩阵：旋转的两种表达

旋转矩阵，3x3的矩阵，9个元素，但只有3个自由度。它直观，但计算起来麻烦。四元数，4个元素，没有万向锁问题，插值也方便。我个人更偏爱四元数，尤其是在做姿态估计的时候。

举个例子，你要把相机坐标系下的一个点转到IMU坐标系下：

// 旋转矩阵方式
p_imu = R_cam_to_imu * p_cam

// 四元数方式
p_imu = q_cam_to_imu * p_cam * q_cam_to_imu.conjugate()

你看，四元数写法更紧凑，而且避免了旋转矩阵的冗余。但要注意，四元数必须归一化，否则旋转会变形。我见过有人直接把未归一化的四元数丢进优化器，结果姿态估计直接炸了。

5. 李群与李代数基础：为什么需要它？

你可能会问：旋转矩阵和四元数不是已经够用了吗？为什么还要搞李群李代数？

原因很简单——优化。在SLAM里，我们要不断优化位姿。旋转矩阵是正交矩阵，优化时不能直接加减，否则会破坏正交性。四元数虽然好一些，但也要满足单位模长约束。李代数就不一样了，它是个向量空间，可以随便加减，然后通过指数映射回到李群。

我记得第一次接触李代数时，觉得这东西太抽象了。后来在写BA（光束法平差）时，发现用李代数做增量更新，代码简洁了不止一倍。说白了，李代数就是给旋转「松绑」，让优化变得自由。

表达方式	自由度	约束	优化友好度
旋转矩阵	9	正交+行列式为1	差
四元数	4	单位模长	中等
李代数(so3)	3	无	好

6. 知识体系总览

下面这张图是我自己整理的坐标系与刚体运动的知识结构。你可以看到，所有坐标系最终都通过旋转和平移关联起来，而李群李代数是连接「几何表达」和「数值优化」的桥梁。

嗯，这张图基本概括了本章的核心。你从下往上看：先理解三个坐标系各自的特点，然后掌握旋转的几种表达方式，最后用李群李代数把优化问题搞定。每一步都有坑，但每一步也都有规律可循。

好了，坐标系与刚体运动这部分就聊到这儿。记住一句话：坐标系是骨架，旋转是灵魂，李代数是工具。把这三样搞明白，后面的初始化、重定位、回环检测，都会顺很多。