3. 坐标系与变换：LiDAR坐标系、IMU坐标系、世界坐标系、刚体变换与SE(3)
做多传感器融合，第一道坎就是坐标系。我见过不少新手，算法推得头头是道，最后发现点云和IMU数据对不上，折腾了两天，结果只是坐标系定义反了。说白了，坐标系就是所有传感器的「共同语言」，语言不通，啥都白搭。
3.1 三个核心坐标系
我们做LiDAR-IMU标定，打交道最多的就是这三个坐标系：
- LiDAR坐标系 (L系)：原点在激光雷达的测量中心。通常x轴朝前（车头方向），y轴朝左，z轴朝上。但注意，不同厂家的定义可能不同。我遇到过某款国产雷达，它的y轴是朝右的，当时没仔细看文档，结果标定出来的外参全是反的。
- IMU坐标系 (I系)：原点在IMU的加速度计和陀螺仪的交点（实际上很难精确测量，我们一般用外壳的几何中心近似）。x轴通常指向载体前进方向，y轴朝左，z轴朝上。IMU的坐标系定义相对统一，但安装方向千奇百怪。
- 世界坐标系 (W系)：也叫全局坐标系。我们通常选第一帧LiDAR或者IMU的位置作为原点。z轴一般指向上（重力反方向），x和y轴在水平面内。说白了，这就是我们描述机器人运动的大地基准。
核心要点：标定的本质，就是求LiDAR坐标系和IMU坐标系之间的刚体变换关系。这个关系一旦确定，我们就能把LiDAR测到的点云，投影到IMU的坐标系下，或者反过来。
3.2 刚体变换与SE(3)
刚体变换，说白了就是「旋转+平移」。一个刚体在三维空间里，不管你怎么转、怎么移，它的形状和大小不变。数学上我们用SE(3)来表示这个变换群。
SE(3) = { (R, t) | R ∈ SO(3), t ∈ ℝ³ }
其中R是旋转矩阵，t是平移向量。一个完整的刚体变换可以写成4×4的齐次矩阵：
T = [ R t ]
[ 0 1 ]举个例子，如果我们想将LiDAR坐标系下的一个点 p_L，变换到IMU坐标系下，公式就是：
p_I = R * p_L + t或者用齐次坐标：
p_I_homo = T_L_I * p_L_homo这里的 T_L_I 就是我们常说的「外参矩阵」。我在项目中习惯把外参写成 T_L_I，意思是「从LiDAR系到IMU系的变换」。你想想看，这个命名方式很直观：下标从右往左读，就是变换的起点和终点。
个人习惯：我建议你在代码里统一用 Eigen::Isometry3d 或者 Sophus::SE3d 来存储外参。千万别用4×4的原始矩阵手动乘，容易搞错顺序。我曾经在调试时发现点云投影总是不对，查了半天，原来是旋转矩阵的转置用错了。
3.3 坐标系变换的链式法则
实际系统中，我们经常需要做多次变换。比如，我们要把LiDAR点云投影到世界坐标系下：
p_W = T_W_I * T_I_L * p_L这里 T_W_I 是IMU在世界坐标系下的位姿（由IMU里程计或VIO提供），T_I_L 是LiDAR到IMU的外参。你看，变换就像链条一样，一环扣一环。
为什么会这样？因为IMU的测量频率高（200Hz以上），而LiDAR的频率低（10Hz左右）。我们通常用IMU来做状态估计，然后利用外参把LiDAR的观测「挂载」到IMU的轨迹上。
避坑指南：我曾经在标定过程中，直接把LiDAR的位姿当作IMU的位姿来用，结果优化出来的轨迹全是扭曲的。记住，LiDAR和IMU之间有一个固定的刚体变换，这个变换必须在优化过程中被显式地建模。
3.4 旋转的多种表示
旋转矩阵SO(3)有9个参数，但只有3个自由度。实际工程中，我们很少直接用旋转矩阵做优化，因为正交性约束太难处理了。常用的替代方案有：
| 表示方式 | 参数数量 | 优点 | 缺点 |
|---|---|---|---|
| 旋转矩阵 | 9 | 直观，容易理解 | 冗余，有约束 |
| 欧拉角 | 3 | 参数少，物理意义明确 | 有万向锁问题 |
| 四元数 | 4 | 无奇异性，适合插值 | 有单位长度约束 |
| 李代数(so3) | 3 | 无约束，适合优化 | 需要指数/对数映射 |
我个人最推荐用李代数做优化，用四元数做存储。为什么呢？因为李代数是无约束的，你可以直接用高斯牛顿法去优化，不用担心破坏正交性。而四元数做插值和乘法非常方便，适合在代码里传递。
3.5 知识体系图
下面这张图，是我自己总结的坐标系与变换的核心逻辑。你把它理清了，标定和融合的框架就搭起来了。
3.6 实战中的坐标系约定
嗯，这里要注意。不同开源项目对坐标系的命名习惯不一样。有的用 body 表示IMU系，有的用 base_link。我建议你在项目一开始就定好命名规范，写在文档里。
我自己常用的约定是：
world：世界坐标系imu：IMU坐标系lidar：LiDAR坐标系- 变换矩阵命名：
T_imu_lidar表示从lidar到imu的变换
这样命名，代码的可读性会好很多。你想想看，如果别人接手你的代码，看到 T_imu_lidar 就知道是外参，看到 T_world_imu 就知道是位姿，一目了然。
小技巧：在写代码时，可以定义一个 Transform 类，封装旋转和平移。这样你就不用每次都手动拼矩阵了。我在项目中就是这么做的，后来标定代码复用起来特别方便。
坐标系与变换，是整个标定系统的地基。地基没打好，上层算法再漂亮也是空中楼阁。下一节，我们会把这些变换关系，真正用到联合优化的公式里去。
公众号:蓝海资料掘金营，微信deep3321