第2章坐标系基础

2. 坐标系基础：世界坐标系、相机坐标系、图像坐标系、像素坐标系

做传感器融合，说白了就是让雷达和摄像头「说同一种语言」。

我刚开始入行那会儿，最头疼的就是坐标系转换。明明雷达报了一个目标在正前方10米，摄像头却告诉我它在画面左上角。后来才明白——它们各自活在自己的坐标系里，不「对齐」一下，根本没法聊。

这一节，我们就来把这四个坐标系彻底捋清楚。

世界坐标系，你可以把它想象成整个场景的「上帝视角」。它是一个固定的三维直角坐标系，通常用 (X_w, Y_w, Z_w) 表示。

在实际项目中，我们一般把世界坐标系的原点选在车体中心、或者第一个摄像头安装位置的正下方。为什么这么选？我个人习惯把原点放在车体后轴中心，因为车辆运动模型在这个点下计算最方便。

关键点：世界坐标系是唯一的、不变的。所有传感器（雷达、摄像头、激光雷达）最终都要转换到这个坐标系下，才能进行数据融合。

举个例子：路面上一个锥桶，它在世界坐标系中的位置是 (5.0m, 1.2m, 0.0m)。不管用哪个传感器看它，这个坐标值是不变的。

相机坐标系，就是摄像头自己的小世界。它以摄像头的光心为原点，Z轴指向摄像头前方，X轴向右，Y轴向下（注意，这个Y轴方向和世界坐标系是反的）。

用 (X_c, Y_c, Z_c) 表示。

为什么会这样定义？嗯，这里有个历史原因——图像的行列索引是从左上角开始的，所以相机坐标系的Y轴朝下，方便后续映射到图像上。

我的经验：做标定时，最容易出错的就是相机坐标系的Y轴方向。我曾经因为搞反了Y轴，导致雷达目标全部投影到了图像的上半部分，排查了整整一个下午。后来我养成了一个习惯——标定完第一件事，就是拿一个已知距离的物体验证投影是否正确。

从世界坐标系到相机坐标系的转换，需要一个旋转矩阵 R 和一个平移向量 t。公式很简单：

X_c = R * X_w + t

这个 R 和 t，就是所谓的外参。每个摄像头都有自己的外参，因为安装位置和朝向不同。

图像坐标系，是相机坐标系中的三维点投影到成像平面后得到的二维坐标。用 (x, y) 表示，单位是毫米（mm）。

这里用到了小孔成像模型。说白了，就是把三维空间中的点，通过光心投影到成像平面上。公式是：

x = f * X_c / Z_c
y = f * Y_c / Z_c

其中 f 是焦距。

你想想看，这个转换丢掉了深度信息 Z_c。所以从图像坐标系反推回三维坐标，理论上是不可能的——除非你知道目标的深度。这就是为什么我们需要雷达来提供距离信息。

注意：图像坐标系的单位是物理单位（毫米），不是像素。很多初学者在这里搞混。图像坐标系描述的是「物体在成像平面上的物理位置」，而像素坐标系描述的是「这个位置对应图像中的第几行第几列」。

像素坐标系，就是我们最终在图像上看到的行列索引。用 (u, v) 表示，单位是像素（pixel）。

从图像坐标系到像素坐标系，需要知道两个参数：

转换公式：

u = x / dx + u0
v = y / dy + v0

把前面图像坐标系的公式代进来，就得到了从相机坐标系到像素坐标系的完整转换：

u = f * X_c / (Z_c * dx) + u0
v = f * Y_c / (Z_c * dy) + v0

这个 f/dx 和 f/dy，就是内参矩阵中的 fx 和 fy。内参矩阵 K 长这样：

K = | fx  0  u0 |
    |  0  fy v0 |
    |  0   0  1 |

一句话总结：世界坐标系 → 相机坐标系（外参）→ 图像坐标系（投影）→ 像素坐标系（内参）。这就是整个坐标转换链条。

在实际项目中，我们做雷达和摄像头数据对齐，一般分三步走：

我记得有一次做量产项目，标定完外参后，发现雷达目标总是偏右半个车身。查了半天，原来是雷达的安装位置测量错了5厘米。你看，坐标系对齐这件事，差之毫厘，谬以千里。

避坑指南：我曾经在标定时忽略了雷达坐标系的方向定义。有些雷达的Y轴朝左，有些朝右。如果不确认这一点，转换出来的结果全是错的。所以拿到一个新传感器，第一件事就是看它的坐标系定义文档。

这一节我们讲了四个坐标系：

搞清楚了这些，后面的数据对齐工作就顺理成章了。下一节，我们开始讲具体的坐标转换矩阵推导，以及如何用代码实现。

嗯，坐标系这块内容确实有点枯燥，但它是整个传感器融合的基石。你想想看，如果连坐标系都没对齐，后面再花哨的算法都是空中楼阁。