坐标系基础：世界坐标系、车辆坐标系、相机坐标系、图像坐标系

做自动驾驶定位，说白了就是搞清楚「车在哪、周围有啥」。而这一切的起点，就是坐标系。我见过不少新人，一上来就猛怼算法，结果坐标系搞混了，整个系统全崩。嗯，今天咱们就把这四个坐标系彻底捋清楚。

1. 世界坐标系（World Coordinate System）

世界坐标系，你可以把它理解成「绝对参考系」。它固定在大地上，不随车辆移动而改变。我个人习惯用 ENU 坐标系（东-北-天）作为世界坐标系，因为直观——东就是东，北就是北。

在项目中，我们通常用 GPS/RTK 来获取世界坐标系下的车辆位置。但要注意，GPS 给的是经纬高（WGS84），需要先转成平面坐标才能用。我曾经踩过这个坑：直接拿经纬度算距离，结果误差大得离谱。

// WGS84 转 ENU 的简化示例
// 输入：纬度 lat, 经度 lon, 高度 alt
// 输出：东向 x, 北向 y, 天向 z

double lat_rad = lat * M_PI / 180.0;
double lon_rad = lon * M_PI / 180.0;

double x = (lon_rad - ref_lon_rad) * R * cos(ref_lat_rad);
double y = (lat_rad - ref_lat_rad) * R;
double z = alt - ref_alt;

3. 车辆坐标系（Vehicle Coordinate System）

车辆坐标系是跟着车走的。一般定义：车头方向为 X 轴正方向，车身左侧为 Y 轴正方向，车顶方向为 Z 轴正方向。说白了，就是「坐在驾驶座上看到的坐标系」。

为什么需要它？因为传感器都装在车上。激光雷达、摄像头、IMU，它们的数据都是相对于车辆坐标系来说的。你想想看，如果不知道传感器在车上的安装位置，那数据就是一堆乱码。

3. 相机坐标系（Camera Coordinate System）

相机坐标系就更有意思了。它是以相机光心为原点，Z 轴指向镜头前方（也就是拍摄方向），X 轴向右，Y 轴向下。为什么 Y 轴向下？因为图像坐标系的原点在左上角，Y 轴是朝下的，这样转换起来方便。

这里有个关键点：相机坐标系是 3D 的，而图像是 2D 的。怎么把 3D 的点投影到 2D 图像上？靠的是相机内参。

// 相机投影模型（针孔相机）
// 3D 点 (X, Y, Z) 在相机坐标系下
// 投影到图像平面 (u, v)

u = fx * X / Z + cx
v = fy * Y / Z + cy

// 其中 fx, fy 是焦距（像素单位）
// cx, cy 是主点坐标

我记得第一次手写这个公式时，死活想不通为什么要有 cx 和 cy。后来才明白，因为图像传感器的中心不一定和光轴完全对齐，需要补偿这个偏移。

4. 图像坐标系（Image Coordinate System）

图像坐标系就是像素坐标系。原点在左上角，u 轴向右，v 轴向下。每个像素的坐标 (u, v) 就是它在图像中的位置。

但注意，图像坐标系是离散的，单位是像素。而相机坐标系是连续的，单位是米。所以从相机坐标系到图像坐标系，其实做了两件事：投影（3D→2D）和量化（连续→离散）。

5. 坐标系之间的转换关系

好了，四个坐标系都认识了。现在的问题是：它们之间怎么转换？

说白了，就是一条链：

世界坐标系 → 车辆坐标系 → 相机坐标系 → 图像坐标系

每一步都是一个变换矩阵。我习惯用齐次坐标来统一处理，这样旋转和平移可以写在一个矩阵里。

// 齐次变换矩阵示例
// 从车辆坐标系到世界坐标系

| X_world |   | R  t |   | X_vehicle |
| Y_world | = |      | * | Y_vehicle |
| Z_world |   | 0  1 |   | Z_vehicle |
|    1    |   |      |   |     1     |

// 其中 R 是 3x3 旋转矩阵
// t 是 3x1 平移向量

整个转换流程是这样的：

1. 世界坐标系 → 车辆坐标系：用车辆的位姿（位置 + 朝向）
2. 车辆坐标系 → 相机坐标系：用传感器的外参（安装位置 + 安装角度）
3. 相机坐标系 → 图像坐标系：用相机内参（焦距 + 主点）

6. 知识体系总览

下面这张图，是我自己总结的坐标系转换全流程。你看一遍，应该就能把整个逻辑串起来了。

你看，整个流程其实不复杂。但每个环节都有坑。比如车辆位姿的精度直接影响后续所有转换，传感器外参如果标定不准，那 BEV 感知的结果就是歪的。