2、传感器坐标系：IMU坐标系定义、相机/激光雷达坐标系、刚体变换与SE(3)
做多传感器融合，说白了就是跟坐标系打交道。我见过不少新手，算法推得头头是道，结果一跑数据就崩——十有八九是坐标系搞反了。今天咱们就把这块硬骨头啃下来。
2.1 IMU坐标系：那个让人头疼的“右手系”
IMU的坐标系定义，其实有行业标准。大多数IMU遵循右手坐标系：
- X轴：指向载体前进方向（前）
- Y轴：指向载体右侧（右）
- Z轴：指向载体上方（上）
嗯，这里要注意——加速度计和陀螺仪的测量值都是在这个坐标系下的。我踩过最大的坑是什么？有一次拿到一个工业级IMU，手册上写的是“ENU坐标系”（东-北-天），结果我按常规的“前-右-上”去解算，姿态直接飞了。后来才发现，ENU和车体坐标系差了90度旋转。
我曾经因为IMU坐标系定义不统一，浪费了整整两天。建议你拿到新IMU后，第一件事就是画个坐标系贴在传感器上，然后手动旋转验证每个轴的方向。
2.2 相机坐标系：光心才是老大
相机坐标系和IMU不一样。它的原点在光心（也就是镜头的光学中心），而不是传感器芯片的中心。坐标系定义如下：
- Z轴：指向相机前方（沿着光轴方向）
- X轴：指向图像右侧
- Y轴：指向图像下方
你想想看，为什么Y轴要朝下？因为图像坐标系的原点在左上角，像素行号往下增加。这样定义，从3D世界到2D图像的投影公式就简洁了。
我个人习惯在标定前，先确认相机内参矩阵的格式。OpenCV用的是[fx, 0, cx; 0, fy, cy; 0, 0, 1]，但有些工业相机厂商会用不同的排列顺序。这个细节不注意，外参标定结果就是错的。
2.3 激光雷达坐标系：旋转的“眼睛”
激光雷达的坐标系相对统一，但不同厂商有细微差别。以最常见的Velodyne/Udapte风格为例：
- X轴：指向雷达前方
- Y轴：指向雷达左侧
- Z轴：指向雷达上方
等等，你发现没有？激光雷达的Y轴方向和IMU是反的！IMU的Y轴朝右，激光雷达的Y轴朝左。这就是为什么做LiDAR-IMU融合时，外参的旋转矩阵里总有一个180度的翻转。
我在做自动驾驶项目时，遇到过激光雷达点云和IMU数据对不上的情况。排查了半天，发现是激光雷达的坐标系定义和IMU差了90度。后来我写了个小脚本，每次标定前先可视化两个传感器的原始数据，看一眼就知道坐标系对不对。
2.4 刚体变换与SE(3)：数学工具箱
好了，现在我们有三个坐标系：IMU坐标系{I}、相机坐标系{C}、激光雷达坐标系{L}。它们之间怎么转换？答案就是刚体变换。
刚体变换包含两部分：
- 旋转：用旋转矩阵
R ∈ SO(3)表示 - 平移：用平移向量
t ∈ ℝ³表示
合在一起，就是 T = [R | t] ∈ SE(3)。SE(3)是特殊欧氏群，说白了就是所有刚体变换的集合。
举个例子，从IMU坐标系到相机坐标系的变换：
// 假设已知外参 T_ic (IMU到相机)
// 将IMU坐标系下的点 p_i 转换到相机坐标系
p_c = R_ic * p_i + t_ic
// 或者用齐次坐标形式
[p_c; 1] = T_ic * [p_i; 1]为什么用齐次坐标？因为可以把旋转和平移写成一个矩阵乘法，方便链式变换。比如从IMU到世界坐标，再投影到图像：
// 世界坐标系下的3D点 P_w
// 先转到IMU坐标系
P_i = T_wi^(-1) * P_w
// 再转到相机坐标系
P_c = T_ic * P_i
// 最后投影到图像
p = K * P_c // K是内参矩阵这个链式变换，就是多传感器融合的核心。我建议你把每个传感器的外参都写成T_传感器_参考系的形式，比如T_ic、T_il，这样代码可读性高，也不容易搞混。
标定外参时，我习惯用李代数se(3)来表示变换。se(3)是SE(3)的切空间，优化起来更方便。比如用g2o或Ceres做图优化时，直接优化se(3)的6个参数（3个旋转+3个平移），比优化旋转矩阵的9个参数稳定得多。
2.5 知识体系总览
下面这张图，是我自己总结的坐标系与变换关系。你看一眼，就能把今天的内容串起来。
这张图里，T_ic、T_il就是我们要标定的外参。标定精度越高，后续融合效果越好。我见过有人直接用标定板手动对齐，精度能到0.1度、1毫米级别。但如果你用自动标定算法，记得多采集几组数据，覆盖不同的旋转和平移。
好了，坐标系这块就聊到这儿。记住一句话：坐标系定义是标定的地基，地基歪了，楼盖得再高也得塌。
- IMU坐标系：前-右-上，右手系
- 相机坐标系：光心为原点，Z轴朝前，Y轴朝下
- 激光雷达坐标系：前-左-上，注意Y轴方向与IMU相反
- 刚体变换 T = [R|t] ∈ SE(3)，用齐次坐标方便链式变换
- 标定前务必验证每个传感器的坐标系方向
可可精品资料库