坐标系基础：世界坐标系、相机坐标系、图像坐标系、像素坐标系
做传感器融合，坐标系是绕不开的第一道坎。我见过不少新手，算法模型写得漂亮，结果一跑数据，目标位置全对不上——十有八九是坐标系搞混了。
说白了，摄像头和雷达各自生活在不同的坐标系里。雷达告诉你「前方2米处有个目标」，摄像头告诉你「图像第320列有个目标」。这两个信息怎么对齐？你得先搞清楚它们各自在说什么。
四个坐标系，一个都不能少
我们先捋一遍这四个坐标系。我个人习惯把它们分成两组：三维组和二维组。
1. 世界坐标系 (World Coordinate System)
这是真实世界的绝对参考系。你开车在路上，路边的电线杆、对面的来车，它们的位置都是用世界坐标描述的。通常我们用 (Xw, Yw, Zw) 表示。
实际项目中，我一般把世界坐标系的原点选在车辆后轴中心，或者GPS天线的安装位置。为什么？因为这样标定起来方便，后续做车辆运动补偿也顺手。
2. 相机坐标系 (Camera Coordinate System)
相机坐标系的原点在相机的光心。Z轴指向相机的前方（也就是镜头朝向），X轴向右，Y轴向下——嗯，这里要注意，相机坐标系的Y轴是朝下的，跟世界坐标系不一样。
用 (Xc, Yc, Zc) 表示。世界坐标系里的一个点，经过刚体变换（旋转+平移），就到了相机坐标系里。
3. 图像坐标系 (Image Coordinate System)
这是二维的。相机把三维世界投影到成像平面上，就有了图像坐标系。原点在光轴与成像平面的交点（也就是主点），单位是毫米。用 (x, y) 表示。
你想想看，相机坐标系里的一个点 (Xc, Yc, Zc)，通过小孔成像模型，投影到图像平面上，就变成了 (x, y)。
4. 像素坐标系 (Pixel Coordinate System)
这是最终我们看到的图像。原点在图像的左上角，u轴向右，v轴向下。单位是像素。用 (u, v) 表示。
图像坐标系里的 (x, y) 经过缩放和平移，就变成了像素坐标 (u, v)。
核心要点：这四个坐标系的关系，说白了就是一条流水线：
世界坐标 → 刚体变换 → 相机坐标 → 投影变换 → 图像坐标 → 仿射变换 → 像素坐标
坐标系转换关系
好，坐标系介绍完了。接下来我们看看它们之间怎么转换。我把它拆成两个核心变换：刚体变换和投影变换。
刚体变换：从世界到相机
刚体变换，说白了就是旋转加平移。物体本身不会变形，只是换个角度看它。
数学上写出来是这样的：
| Xc | | R11 R12 R13 | | Xw | | Tx |
| Yc | = | R21 R22 R23 | | Yw | + | Ty |
| Zc | | R31 R32 R33 | | Zw | | Tz |简写一下：Pc = R · Pw + T
R 是3x3的旋转矩阵，T 是3x1的平移向量。这6个参数（3个旋转角 + 3个平移量）就是相机的外参。
我的经验：标定外参时，我建议用棋盘格法。我曾经在一个项目中，直接用激光雷达点云和相机图像手动选对应点来算外参，结果误差大得离谱。后来老老实实摆了棋盘格，用张正友标定法，一次搞定。
投影变换：从相机到图像
这是小孔成像模型。相机坐标系里的点，投影到成像平面上：
x = f * Xc / Zc
y = f * Yc / Zc其中 f 是焦距。写成齐次坐标形式：
| x | | f 0 0 0 | | Xc |
| y | = | 0 f 0 0 | | Yc |
| 1 | | 0 0 1 0 | | Zc |
| 1 |这里有个关键点：深度信息 Zc 丢失了。这就是为什么单目相机无法直接测距——你从图像里只能知道目标的方向，不知道它离你多远。
避坑指南：我曾经在融合项目中，直接用单目视觉的检测结果去匹配雷达点云，结果发现视觉检测框的横向位置总是偏的。查了半天，发现是投影变换时忘了考虑镜头畸变。嗯，畸变校正一定要做，尤其是广角镜头。
从图像坐标到像素坐标
这一步是离散化。图像坐标 (x, y) 是连续的，像素坐标 (u, v) 是离散的：
u = x / dx + u0
v = y / dy + v0dx 和 dy 是每个像素的物理尺寸（毫米/像素），(u0, v0) 是主点坐标。
写成矩阵形式：
| u | | 1/dx 0 u0 | | x |
| v | = | 0 1/dy v0 | | y |
| 1 | | 0 0 1 | | 1 |完整的投影矩阵
把上面所有步骤串起来，从世界坐标到像素坐标的完整变换是：
| u | | fx 0 u0 0 | | R11 R12 R13 Tx | | Xw |
| v | = | 0 fy v0 0 | | R21 R22 R23 Ty | | Yw |
| 1 | | 0 0 1 0 | | R31 R32 R33 Tz | | Zw |
| 0 0 0 1 | | 1 |前面那个3x4矩阵是内参矩阵 K，后面那个4x4矩阵是外参矩阵 [R|T]。
所以：像素坐标 = K · [R|T] · 世界坐标
实际应用：做目标级融合时，我们通常把雷达检测到的目标从世界坐标（或车辆坐标）投影到像素坐标，然后跟视觉检测框做关联匹配。反过来，也可以把视觉检测到的目标反投影到世界坐标，跟雷达目标做空间对齐。
坐标系转换的工程实现
理论讲完了，我们看看代码怎么写。我习惯用 Eigen 库做矩阵运算，简洁高效。
// 定义内参矩阵 K
Eigen::Matrix3d K;
K << fx, 0, u0,
0, fy, v0,
0, 0, 1;
// 定义外参：旋转矩阵 R 和平移向量 T
Eigen::Matrix3d R;
Eigen::Vector3d T;
// ... 从标定文件读取 R 和 T
// 世界坐标点
Eigen::Vector3d Pw(10.0, 2.0, 0.0); // 前方10米，右侧2米
// 转到相机坐标
Eigen::Vector3d Pc = R * Pw + T;
// 投影到像素坐标
Eigen::Vector3d p_pixel = K * Pc;
double u = p_pixel(0) / p_pixel(2);
double v = p_pixel(1) / p_pixel(2);小技巧：调试时，我经常在图像上画一个十字线，把投影点标出来。如果投影点跟实际目标位置对不上，十有八九是外参标定有问题。我曾经花了一整天调外参，最后发现是旋转矩阵的符号搞反了——Y轴方向搞混了。
总结一下
坐标系转换是传感器融合的基石。你想想看，如果连目标在哪儿都说不清楚，后面的匹配、跟踪、决策全是空中楼阁。
我个人建议，做融合项目时，先把坐标系转换的代码封装成一个独立的模块，写好单元测试。这样后面不管加什么传感器，只要标定好外参，直接调用就行。
下一章我们会讲雷达坐标系和相机坐标系的对齐方法，包括时间同步和空间同步。到时候你会看到，坐标系转换的知识会反复用到。