坐标系统：世界坐标系、相机坐标系、图像坐标系、像素坐标系、激光雷达坐标系，以及它们之间的转换关系
做传感器融合，说白了就是让激光雷达和相机「说同一种语言」。
我刚开始接触这个领域时，最头疼的就是坐标系。激光雷达告诉你「前方3米有个障碍物」，相机告诉你「图像第200行第300列有个物体」。这两个信息怎么对应起来？嗯，这就是坐标系转换要解决的问题。
一、五个坐标系，一张图说清楚
我个人习惯把坐标系分成两类：3D世界里的和2D图像里的。先看个总览：
这张图我画了很多遍。你想想看，从世界坐标系出发，经过相机外参到了相机坐标系，再经过内参到了图像坐标系，最后到像素坐标系。激光雷达那边也是类似，只不过它有自己的外参。
二、每个坐标系是干什么的？
1. 世界坐标系 (Xw, Yw, Zw)
这是整个场景的「绝对参考系」。我习惯把世界坐标系的原点放在车辆后轴中心，或者第一帧相机的位置。说白了，它就是给所有物体一个统一的位置描述。
实际例子：路边的交通标志，在世界坐标系中的位置是 (10.5m, 0m, 2.3m)。这个坐标不会因为你的车动了就变。
2. 相机坐标系 (Xc, Yc, Zc)
以相机光心为原点，Z轴指向相机前方，X轴向右，Y轴向下（注意，这里和世界坐标系不同）。
我记得第一次做标定时，搞反了Y轴方向，结果投影出来的点全跑到天上去了。嗯，这个坑我踩过。
3. 图像坐标系 (x, y)
这是相机成像平面上的坐标系。单位是毫米，原点在图像中心（主点）。
说白了，就是把3D世界「拍扁」到2D平面上。这里有个关键参数——焦距f，它决定了投影的缩放比例。
4. 像素坐标系 (u, v)
这就是你看到的图像矩阵。原点在左上角，u轴向右，v轴向下。单位是像素。
图像坐标系到像素坐标系，其实就是个缩放+平移。每个像素的物理尺寸是dx和dy（毫米/像素），主点偏移是(u0, v0)。
5. 激光雷达坐标系 (Xl, Yl, Zl)
以激光雷达中心为原点。不同厂家的定义略有不同，但通常X轴向前，Y轴向左，Z轴向上。
注意：Velodyne和Hesai的坐标系定义不一样！我曾经因为没仔细看手册，把点云投影偏了半米。一定要先确认你用的雷达坐标系定义。
三、坐标系之间的转换关系
3.1 世界坐标系 → 相机坐标系
这个转换靠的是外参：旋转矩阵R和平移向量t。
// 世界坐标 Pw 转到相机坐标 Pc
Pc = R * Pw + t
// 用齐次坐标写更简洁
|Xc| |R11 R12 R13 tx| |Xw|
|Yc| = |R21 R22 R23 ty| |Yw|
|Zc| |R31 R32 R33 tz| |Zw|
|1 | |0 0 0 1 | |1 |R是3x3的旋转矩阵，t是3x1的平移向量。这6个参数（3个旋转角+3个平移量）就是相机的外参。
3.2 相机坐标系 → 图像坐标系
这是透视投影，靠的是内参中的焦距f。
// 相机坐标 (Xc, Yc, Zc) 到图像坐标 (x, y)
x = f * Xc / Zc
y = f * Yc / Zc注意这里有个除法！Zc在分母上。这就是为什么远处的物体看起来小——因为Zc大了，投影坐标就小了。
我的经验：实际项目中，Zc不能为0或负数。如果Zc <= 0，说明点在相机后面，应该直接丢弃。我曾经没做这个检查，结果投影出一堆乱七八糟的点。
3.3 图像坐标系 → 像素坐标系
这一步简单，就是缩放+平移：
u = x / dx + u0
v = y / dy + v0
// 写成矩阵形式
|u| |1/dx 0 u0| |x|
|v| = |0 1/dy v0| |y|
|1| |0 0 1 | |1|dx和dy是每个像素的物理尺寸，u0和v0是主点偏移。这些参数加上焦距f，合起来就是内参矩阵K：
K = |fx 0 u0|
|0 fy v0|
|0 0 1 |其中 fx = f/dx, fy = f/dy。
3.4 世界坐标系 → 像素坐标系（完整投影公式）
把上面三步合起来，就是完整的投影公式：
// 齐次坐标形式
|u| |Xw|
|v| = K * [R|t] * |Yw|
|1| |Zw|
|1 |这个公式你会在所有相机投影代码里看到。说白了，就是「外参把世界坐标转到相机坐标，内参把相机坐标投影到像素」。就这么简单。
3.5 激光雷达坐标系 ↔ 世界坐标系
和相机一样，激光雷达也有自己的外参。通常我们标定的是激光雷达到世界坐标系的转换，或者激光雷达到相机坐标系的转换。
// 激光雷达坐标 Pl 转到世界坐标 Pw
Pw = R_lidar * Pl + t_lidar
// 或者直接转到相机坐标
Pc = R_lidar2cam * Pl + t_lidar2cam关键点：激光雷达和相机的联合标定，就是求 R_lidar2cam 和 t_lidar2cam。我一般用棋盘格标定板，同时采集点云和图像，然后优化这两个矩阵。
四、实际项目中的避坑指南
我做了几年融合，踩过的坑不少。这里列几个最常见的：
- 坐标系方向搞反：相机坐标系的Y轴向下，激光雷达的Y轴向左。转换时一定要确认。
- 单位不统一：激光雷达给的是米，相机内参给的是毫米。我见过有人直接拿米和毫米算，结果差了1000倍。
- 忘记齐次坐标：用3x4矩阵投影时，世界坐标要补一个1变成4维。忘了的话矩阵乘法会报错。
- Zc为负没处理：投影前检查Zc > 0，否则点可能在相机后面。
我曾经犯过的错：有一次标定激光雷达和相机，我用了30组数据，结果重投影误差一直很大。查了两天发现，是标定板的角点检测程序有个bug，检测到的角点偏了2个像素。从那以后，我每次标定都会先可视化检查一下角点位置。
五、总结一下
坐标系转换，说白了就是三件事：
- 外参：把不同传感器放到同一个世界坐标系下
- 内参：把3D点投影到2D图像上
- 联合标定：求激光雷达到相机的转换矩阵
掌握了这些，你就能把激光雷达的点云「画」到图像上了。这是传感器融合的第一步，也是最基础的一步。
我的建议：刚开始学的时候，别急着调代码。先拿纸笔手动算一遍投影过程。我当年就是这么干的，算完一遍，所有公式都记住了。