2. 坐标系与变换基础：世界坐标系、相机坐标系、IMU坐标系、刚体变换与李群李代数
做传感器融合，说白了就是跟坐标系打交道。你想想看，激光雷达扫出来的点云是在雷达自己的坐标系里，IMU 报的加速度和角速度是在 IMU 本体坐标系里，相机拍到的像素是在图像坐标系里。这些数据要融合到一起，第一步就是得把它们对齐到同一个参考系下。
我个人习惯把坐标系比作「语言」。每个传感器都在说自己的方言，你要做的是当个翻译官，让它们用普通话交流。这个「普通话」，就是世界坐标系。
2.1 四大核心坐标系
先搞清楚我们日常打交道的几个坐标系长什么样。
| 坐标系 | 原点位置 | 轴定义（右手系） | 典型用途 |
|---|---|---|---|
| 世界坐标系 | 人为选定（如第一帧位置） | Z 轴朝上（ENU）或朝下（NED） | 全局定位、地图构建 |
| 相机坐标系 | 相机光心 | Z 轴指向拍摄方向，X 向右，Y 向下 | 视觉 SLAM、3D 重建 |
| IMU 坐标系 | IMU 芯片中心 | X 前，Y 左，Z 上（常见） | 惯性导航、姿态估计 |
| 车身/机器人坐标系 | 车辆后轴中心或机器人质心 | X 前，Y 左，Z 上 | 运动控制、路径规划 |
这里有个坑，我刚开始做项目时踩过。IMU 的坐标系定义各家厂商不一样。有的把 Z 轴朝上，有的朝下。你拿到一个 IMU 数据手册，第一件事就是看它的轴定义，别想当然。
2.2 刚体变换：旋转 + 平移
把一个坐标系下的点变换到另一个坐标系下，数学上就是刚体变换。它由两部分组成：旋转和平移。
公式很简单：
P_w = R * P_c + t其中 P_w 是世界坐标系下的点，P_c 是相机坐标系下的点，R 是旋转矩阵，t 是平移向量。
旋转矩阵 R 是 3x3 的正交矩阵，行列式为 +1。它有 9 个元素，但自由度只有 3。说白了，你只需要 3 个参数就能描述一个旋转——比如欧拉角（横滚、俯仰、偏航）。
2.3 齐次坐标与变换矩阵
为了把旋转和平移写成一个矩阵，我们引入齐次坐标。把三维点 (x, y, z) 扩展成 (x, y, z, 1)。
变换矩阵 T 是 4x4 的：
T = [ R t ]
[ 0 1 ]这样，一次变换就变成了矩阵乘法：
P_w_homo = T * P_c_homo我个人习惯把所有传感器到车身的变换都写成 4x4 矩阵，存成一个配置文件。标定的时候只需要更新这个矩阵里的数值，代码逻辑不用动。
2.4 李群与李代数：为什么需要它？
你可能会问：旋转矩阵不是挺好的吗？为什么还要搞什么李群李代数？
原因很简单：优化。
旋转矩阵有正交约束（R * R^T = I），你不能直接对它做加法。比如 R1 + R2 大概率不是一个合法的旋转矩阵。但在优化问题里，我们经常需要求导、迭代更新。
李群和李代数就是来解决这个问题的。
- SO(3)：三维旋转群，元素是旋转矩阵。
- se(3)：三维旋转的李代数，元素是 3 维向量（其实就是旋转轴乘以角度）。
- SE(3)：三维刚体变换群，元素是 4x4 变换矩阵。
- se(3)：三维刚体变换的李代数，元素是 6 维向量（平移 + 旋转）。
李代数是一个向量空间，你可以直接对它做加法、求导。然后通过指数映射回到李群。
R = exp(φ^) // 从李代数到李群
φ = log(R) // 从李群到李代数其中 φ 是 3 维向量，φ^ 是它的反对称矩阵。
2.5 实战中的坐标系变换链
在一个典型的自动驾驶或机器人系统中，坐标系变换链是这样的：
- 传感器原始数据：在各自坐标系下（雷达点云、IMU 加速度、相机像素）。
- 传感器到车身：通过外参标定得到 T_sensor_to_body。
- 车身到世界：通过定位模块（如 GPS+IMU 融合）得到 T_body_to_world。
- 传感器到世界：T_sensor_to_world = T_body_to_world * T_sensor_to_body。
我曾经在一个项目中，激光雷达和相机的外参标定差了 2 厘米。结果点云投影到图像上总是偏了几个像素。找了三天 bug，最后发现是标定板角点提取时有一个像素级的系统误差。嗯，标定这件事，细节决定成败。
2.6 知识结构图
下面这张图总结了本章的核心知识点和它们之间的关系：
2.7 常见坐标系约定
不同领域对坐标系的定义有差异。做融合时一定要确认好约定，否则数据对齐全是错的。
| 领域 | 常用坐标系 | Z 轴方向 | 备注 |
|---|---|---|---|
| 自动驾驶 | ENU（东-北-天） | 向上 | 与 GPS 经纬度对齐方便 |
| 无人机 | NED（北-东-地） | 向下 | 与飞控惯导对齐 |
| 机器人 | FLU（前-左-上） | 向上 | ROS 默认约定 |
| 计算机视觉 | RDF（右-下-前） | 指向场景 | OpenCV 相机坐标系 |
我记得有一次做车路协同项目，路侧激光雷达用的是 ENU，车端用的是 FLU。两个坐标系差了 90 度旋转。一开始没注意，融合出来的轨迹直接飞到了天上。嗯，坐标系约定这种事，提前对齐比事后 debug 省心一万倍。