坐标系与刚体变换基础
做SLAM也好，自动驾驶也好，我入行时第一个绕不开的坎就是坐标系。说实话，这东西看着简单，但坑特别多。今天咱们就把世界坐标系、相机坐标系、IMU坐标系，还有四元数、旋转矩阵、欧拉角、李群李代数这些基础，一次性讲透。
1. 世界坐标系：我们到底在哪儿？
世界坐标系，说白了就是整个场景的“绝对参考系”。你可以把它想象成地球上的经纬度——不管你在哪儿，经纬度都能唯一确定你的位置。
在自动驾驶里，世界坐标系通常以车辆启动位置为原点，或者用GPS的经纬度+海拔。SLAM里更常见的是以第一帧相机位置为原点。我个人习惯把世界坐标系固定下来，这样后面做回环检测、全局优化都方便。
关键点：世界坐标系一旦选定，整个轨迹的“绝对位置”就有了意义。所有其他坐标系都是相对它定义的。
2. 相机坐标系：从“眼睛”看世界
相机坐标系的原点在相机光心，Z轴指向相机前方（有的定义是Z轴朝前，也有X轴朝前的，注意区分）。
我记得刚做VIO时，有个坑：相机坐标系和IMU坐标系之间的外参标定。如果你搞反了旋转方向，整个系统直接发散。嗯，这里要注意——相机坐标系是右手系，Z轴朝前，X轴朝右，Y轴朝下（常见于OpenCV）。
避坑指南：我曾经因为相机坐标系定义不一致，导致投影到图像上的点全反了。后来我养成了一个习惯：每次拿到新传感器，先打印出坐标系定义，贴在设备上。
3. IMU坐标系：加速度计和陀螺仪的“小世界”
IMU坐标系通常定义在IMU芯片的物理中心。加速度计测量的是比力（包含重力），陀螺仪测量的是角速度。
你想想看，IMU和相机之间有个固定的旋转和平移——这就是外参。在VINS-Mono、ORB-SLAM3这些系统里，外参标定是第一步。我建议你直接用标定板+imu_utils工具，别手算，容易出错。
| 坐标系 | 原点 | Z轴方向 | 常见用途 |
|---|---|---|---|
| 世界坐标系 | 固定点（如起点） | 通常朝上（重力反方向） | 全局轨迹、地图 |
| 相机坐标系 | 相机光心 | 朝前（或朝右） | 特征点投影、位姿估计 |
| IMU坐标系 | IMU芯片中心 | 朝上（常见） | 加速度、角速度测量 |
4. 旋转矩阵：最直观的旋转表达
旋转矩阵R是一个3x3的正交矩阵，行列式为+1。它把一个向量从一个坐标系旋转到另一个坐标系。
公式很简单：p' = R * p。但要注意，旋转矩阵有9个元素，却只有3个自由度——这意味着它是有冗余的。实际工程中，我们很少直接用旋转矩阵做优化，因为要保证正交性约束，太麻烦。
我的经验：旋转矩阵适合做坐标变换的计算，但不适合做优化变量。你想想看，每次迭代都要重新正交化，多累啊。
5. 四元数：SLAM里的“万金油”
四元数q = [w, x, y, z]，其中w是实部，x,y,z是虚部。它没有万向锁问题，插值也平滑。我个人最喜欢用四元数做状态估计。
四元数的乘法：q1 * q2表示先做q2旋转，再做q1旋转。注意顺序！我见过太多人搞反了。
// 四元数乘法示例（Eigen）
Eigen::Quaterniond q1(0.707, 0.707, 0, 0); // 绕Z轴旋转90度
Eigen::Quaterniond q2(1, 0, 0, 0); // 单位四元数
Eigen::Quaterniond q_result = q1 * q2; // 结果还是绕Z轴90度避坑指南：我曾经在优化时忘了对四元数做归一化，结果轨迹越跑越偏。记住：四元数必须保持单位长度，否则物理意义就变了。
6. 欧拉角：直观但危险
欧拉角用三个角度（roll, pitch, yaw）表示旋转。直观是直观，但有两个大问题：
- 万向锁：当pitch接近±90度时，roll和yaw会耦合，丢失一个自由度
- 多值性：同一个旋转对应多组欧拉角
我建议：只在人机交互界面用欧拉角（比如显示姿态），算法内部别用。你想想看，如果飞机做特技飞行时pitch到了90度，欧拉角直接崩了。
7. 李群与李代数：SLAM优化的核心
李群SO(3)和SE(3)是旋转和刚体变换的“连续群”。李代数so(3)和se(3)是它们的“切空间”。
为什么要引入李代数？因为旋转矩阵的优化是有约束的（正交+行列式为1），而李代数是无约束的。说白了，你把旋转用李代数表示，就可以直接用高斯牛顿法优化了。
公式：R = exp(φ^)，其中φ是李代数向量，φ^是它的反对称矩阵。指数映射把李代数映射到李群。
核心思想：李代数做优化，李群做变换。优化时用李代数加法，更新时用指数映射回到李群。这就是SLAM后端优化的标准流程。
我记得第一次用g2o做BA优化时，看到代码里全是SE3Quat和Vector6，当时一脸懵。后来理解了李群李代数，才明白为什么这样设计——因为优化变量是6维的（3维平移+3维旋转），而不是12维的（旋转矩阵+平移向量）。
知识体系总览
下面这张图是我自己总结的坐标系与刚体变换知识结构，你可以对照着看：
这张图把整个知识体系串起来了。你从左边开始：先搞清楚坐标系定义，然后选择旋转表达方式，最后用李群李代数做优化。每一步都有对应的工程实践。
我的建议：初学者先掌握四元数和旋转矩阵的互转，再学李群李代数。别一上来就啃李代数，容易劝退。我当年就是先跑通VINS-Mono，再回头理解理论，效果更好。
好了，坐标系与刚体变换的基础就这些。记住：坐标系定义要统一，旋转表达要选对，优化要用李代数。这三条做到了，你的轨迹估计就稳了一半。