坐标系统与坐标系变换：世界坐标系、相机坐标系、图像坐标系、像素坐标系、激光雷达坐标系、刚体变换与齐次坐标
做传感器融合，说白了就是让不同传感器「说同一种语言」。激光雷达告诉你「前方5米有个障碍物」，相机告诉你「图像第300行有个行人」。这两个信息怎么对应起来？靠的就是坐标系变换。
我刚开始做融合的时候，觉得坐标系不就是几个矩阵相乘嘛，没什么难的。直到有一次标定完，发现投影点全偏了半个车身……嗯，从那以后我再也不敢小看坐标系变换了。
1. 五大坐标系，一个都不能少
在自动驾驶中，我们通常要打交道的坐标系有五个。我习惯把它们分成两组：3D世界组和2D图像组。
| 坐标系 | 维度 | 原点位置 | 单位 | 典型用途 |
|---|---|---|---|---|
| 世界坐标系 | 3D (Xw, Yw, Zw) | 自定义（如车辆后轴中心） | 米 | 全局定位、地图构建 |
| 激光雷达坐标系 | 3D (Xl, Yl, Zl) | 激光雷达几何中心 | 米 | 点云采集、障碍物检测 |
| 相机坐标系 | 3D (Xc, Yc, Zc) | 相机光心 | 米 | 3D到2D投影的中间桥梁 |
| 图像坐标系 | 2D (x, y) | 图像中心（光轴与像平面交点） | 毫米 | 物理成像平面 |
| 像素坐标系 | 2D (u, v) | 图像左上角 | 像素 | 最终图像索引 |
核心要点：激光雷达和相机都在各自坐标系下采集数据。融合的本质，就是把激光雷达坐标系下的点，变换到像素坐标系下，实现「点云到像素」的映射。
2. 刚体变换：旋转+平移，搞定3D到3D
先说说3D坐标系之间的变换。比如把激光雷达坐标系下的点，变换到相机坐标系下。这其实就是一个刚体变换——物体本身不形变，只是位置和朝向变了。
刚体变换由两部分组成：
- 旋转矩阵 R（3×3）：描述两个坐标系之间的朝向差异
- 平移向量 t（3×1）：描述两个坐标系原点之间的位移
公式很简单：
P_cam = R * P_lidar + t其中 P_lidar 是激光雷达坐标系下的3D点，P_cam 是变换到相机坐标系下的3D点。
我的小技巧：实际项目中，R 和 t 是通过标定得到的。我个人习惯用棋盘格标定板，先让激光雷达和相机同时采集，然后通过点云和图像上的角点匹配，解算出 R 和 t。这个过程叫「外参标定」。
3. 齐次坐标：为什么我们需要它？
你想想看，上面的公式里，旋转是乘法，平移是加法。如果连续做多次变换，公式会变得很啰嗦：
P_world = R1 * (R2 * P_lidar + t2) + t1有没有办法把旋转和平移统一成一种运算？有——齐次坐标。
齐次坐标的核心思想，就是把3D点 (x, y, z) 扩展成4D向量 (x, y, z, 1)。同时把旋转矩阵 R 和平移向量 t 组合成一个4×4的变换矩阵 T：
T = | R t |
| 0 1 |这样，变换就变成了纯粹的矩阵乘法：
P_cam_homo = T * P_lidar_homo连续变换也只需要把矩阵乘起来：
T_total = T1 * T2 * T3为什么我喜欢齐次坐标？因为代码写起来特别干净。你只需要维护一个4×4矩阵，所有变换（旋转、平移、缩放、甚至投影）都能用矩阵乘法搞定。我在项目中把所有传感器的外参都存成4×4矩阵，融合时直接链式相乘，代码量减少了一半。
4. 从3D到2D：相机投影模型
好了，现在我们已经把激光雷达点云变换到了相机坐标系下。下一步，要把3D点投影到2D图像上。这一步靠的是相机内参。
相机投影分为两步：
第一步：从相机坐标系到图像坐标系（归一化平面）
x = Xc / Zc
y = Yc / Zc这一步本质上是「透视投影」——距离越远，物体在图像上越小。
第二步：从图像坐标系到像素坐标系
u = fx * x + cx
v = fy * y + cy其中 fx, fy 是焦距（单位：像素），cx, cy 是主点坐标（光轴在像素坐标系下的位置）。
把两步合并，写成矩阵形式：
| u | | fx 0 cx | | Xc |
| v | = | 0 fy cy | * | Yc |
| 1 | | 0 0 1 | | Zc |注意，这里的结果是齐次坐标 (u, v, 1)，实际像素坐标需要做归一化：u = u / Zc, v = v / Zc。
我曾经踩过的坑：相机内参矩阵里的 fx, fy 单位是像素，不是毫米。如果你从相机手册上看到焦距是 12mm，传感器像元尺寸是 3.45μm，那么 fx = 12 / 0.00345 ≈ 3478 像素。千万别直接用毫米值去算，否则投影点会飞到天上去。
5. 完整融合流程：从激光雷达到像素
把上面所有步骤串起来，就得到了激光雷达点云到像素坐标的完整变换链：
1. 激光雷达坐标系 → 世界坐标系（可选，如果要做全局融合）
2. 世界坐标系 → 相机坐标系（外参变换）
3. 相机坐标系 → 像素坐标系（内参投影）
用齐次坐标一步到位：
P_pixel = K * T_ext * P_lidar_homo
其中：
- P_lidar_homo：激光雷达坐标系下的齐次点 (x, y, z, 1)
- T_ext：外参矩阵 (4×4)，将激光雷达坐标系变换到相机坐标系
- K：内参矩阵 (3×4)，将相机坐标系投影到像素坐标系
- P_pixel：像素坐标 (u, v, 1)实际项目中的经验：我一般会在代码里维护一个「变换管线」——把所有传感器到像素坐标系的变换矩阵预先算好。运行时只需要一次矩阵乘法，就能把点云投影到图像上。这样效率很高，能跑到实时。
6. 知识体系总览
下面这张图，是我自己总结的坐标系变换知识体系。你可以把它当作一个「导航图」，每次做融合时对照着看，不容易出错。
7. 避坑指南：我踩过的那些坑
- 坐标系方向搞反：我曾经把激光雷达的坐标系定义成「前-左-上」，而相机是「右-下-前」。结果投影出来的点全在图像外面。后来我统一用「前-x，左-y，上-z」作为世界坐标系，所有传感器都往这个坐标系对齐。
- 齐次坐标忘记归一化：矩阵乘法算出来的是 (u, v, w)，实际像素坐标是 (u/w, v/w)。我刚开始写代码时忘了除 w，结果图像上的点全挤在左上角。
- 外参标定精度不够：标定板放得太近，导致旋转矩阵误差很大。我建议标定板放在 5-10 米处，多采集几个角度，用非线性优化求解。
我的调试小技巧：每次做完坐标系变换，我都会做两个验证：一是把激光雷达点云投影到图像上，看轮廓是否对齐；二是把图像上的2D点反投影到3D空间，看是否落在对应的物体上。两个方向都对了，才说明变换没问题。
坐标系变换是传感器融合的基石。你想想看，如果连「点在哪里」都说不清楚，后面的目标检测、跟踪、规划就全是空中楼阁。把这一章吃透，后面的内容会顺畅很多。