3、坐标系与标定：世界坐标系、车辆坐标系、相机坐标系、传感器联合标定基础
说实话，坐标系和标定这块，是很多刚入行同学的第一道坎。
我记得自己刚做感知那会儿，总觉得坐标系不就是个数学概念嘛，有什么难的？结果第一次做多传感器融合，点云和图像怎么都对不上，折腾了两天才发现是标定参数搞反了。嗯，从那以后我再也不敢小看坐标系了。
3.1 为什么需要这么多坐标系？
你想想看，一辆自动驾驶车上，有激光雷达、摄像头、毫米波雷达、IMU、GPS……每个传感器都有自己的“视角”。
摄像头看到的是像素坐标，激光雷达看到的是三维点云，GPS给的是经纬度。这些数据要融合到一起，就必须有一个统一的“语言”——坐标系。
说白了，坐标系就是给空间中的点一个“地址”。不同传感器用不同的地址本，我们要做的就是把这些地址本翻译成同一本。
核心原则：所有传感器数据最终都要转换到同一个坐标系下，才能进行融合、规划和控制。
3.2 四大坐标系详解
3.2.1 世界坐标系
世界坐标系是全局的参考系。它固定在地球上，不随车辆移动而改变。
通常我们使用东北天坐标系（ENU）：X轴指向东，Y轴指向北，Z轴指向天。GPS给出的经纬高数据，最终都会转换到这个世界坐标系下。
我在项目中遇到过一个问题：世界坐标系的原点选在哪里？
我的习惯是：如果做高精地图匹配，原点选在车辆启动位置；如果做全局路径规划，原点选在地图的某个固定参考点。没有绝对标准，但一定要在整个系统里保持一致。
3.2.2 车辆坐标系
车辆坐标系是跟着车走的。ISO标准定义：
- X轴：车辆前进方向
- Y轴：车辆左侧方向
- Z轴：车辆上方方向
原点通常在后轴中心，或者车辆质心。我个人习惯用后轴中心，因为做运动学模型时方便。
小技巧：车辆坐标系的原点选择会影响后续的标定和运动补偿。建议在项目初期就固定下来，不要中途更换，否则所有标定参数都要重做。
3.2.3 相机坐标系
相机坐标系是感知算法中最常用的坐标系之一。
它的定义有点反直觉：
- Z轴：相机光轴方向（指向被拍摄物体）
- X轴：图像水平方向
- Y轴：图像垂直方向（向下）
为什么Y轴向下？因为图像坐标系的原点在左上角，Y轴向下为正。相机坐标系为了和图像坐标系对齐，也沿用了这个习惯。
我曾经犯过一个低级错误：把相机坐标系的Y轴搞反了，结果3D点投影到图像上，所有物体都上下颠倒。排查了半天才发现是坐标系符号问题。嗯，这种错误犯一次就够了。
3.2.4 像素坐标系与图像坐标系
这两个容易混淆，我简单区分一下：
- 图像坐标系：物理单位（毫米），原点在图像中心
- 像素坐标系：像素单位，原点在图像左上角
它们之间的转换就是内参矩阵干的事。
3.3 坐标系转换：从3D到2D
把三维世界中的一个点投影到图像上，需要经过四步转换：
- 世界坐标系 → 车辆坐标系（外参：旋转+平移）
- 车辆坐标系 → 相机坐标系（外参：旋转+平移）
- 相机坐标系 → 图像坐标系（内参：透视投影）
- 图像坐标系 → 像素坐标系（内参：缩放+平移）
公式写出来是这样的：
// 世界坐标到像素坐标的完整转换
// P_w: 世界坐标点 (X, Y, Z, 1)
// T_v2w: 车辆到世界的变换矩阵
// T_c2v: 相机到车辆的变换矩阵
// K: 相机内参矩阵
// 1. 世界 -> 车辆
P_v = T_v2w * P_w
// 2. 车辆 -> 相机
P_c = T_c2v * P_v
// 3. 相机 -> 像素
pixel = K * P_c // 归一化后得到 (u, v, 1)
注意：这里的矩阵乘法顺序不能搞错。我见过有人把T_v2w和T_c2v的顺序写反了，结果投影出来的点全跑到车后面去了。建议每次写完都做一次可视化验证。
3.4 传感器联合标定基础
标定，说白了就是求传感器之间的外参（旋转矩阵R和平移向量t）。
常见的标定组合：
| 传感器对 | 标定方法 | 精度要求 |
|---|---|---|
| 相机-激光雷达 | 棋盘格/标定板 | 亚像素级 |
| 相机-相机 | 立体标定 | 0.1像素 |
| 激光雷达-IMU | 手眼标定 | 0.1度 |
| 相机-IMU | Kalibr工具 | 0.05度 |
3.4.1 相机-激光雷达联合标定
这是最常用的标定对。我推荐用棋盘格法：
- 在棋盘格上提取角点（图像中的2D点）
- 在点云中提取棋盘格平面上的3D点
- 建立2D-3D对应关系
- 用PnP算法求解外参
我曾经踩过一个坑：棋盘格太小，点云中根本看不清。后来我改用1.2m×0.9m的大棋盘格，效果立竿见影。建议棋盘格至少占激光雷达视场角的30%以上。
3.4.2 标定流程实战
我一般按这个流程走：
- 第一步：固定传感器，确保标定过程中不移动
- 第二步：采集多组数据（不同距离、不同角度）
- 第三步：用工具自动提取特征点
- 第四步：手动检查匹配质量（这一步不能省！）
- 第五步：优化求解，得到外参
- 第六步：可视化验证（投影点云到图像上）
避坑指南：我曾经在标定完成后直接用了结果，没做可视化验证。结果发现点云投影到图像上偏移了5个像素。后来排查发现是标定板不平整导致的。所以，标定完成后一定要做投影验证，看看边缘是否对齐。
3.5 标定精度验证
标定做完了，怎么知道准不准？
我常用的验证方法：
- 重投影误差：把3D点投影到图像上，计算与真实2D点的像素距离。一般要求小于1像素。
- 边缘对齐：把激光雷达点云投影到图像上，看建筑物边缘是否和图像边缘对齐。
- 距离一致性：在不同距离下验证，确保标定参数没有尺度漂移。
嗯，这里要注意：重投影误差小不代表标定一定准。如果标定板只放在一个位置，误差可能很小但外参是错的。所以一定要多角度、多距离采集数据。
3.6 小结
坐标系和标定是感知算法的基石。说白了，没有准确的标定，后面的一切都是空中楼阁。
我个人建议：在项目初期就花时间把标定做好，建立标准流程。不要等到融合出问题了再回头排查，那时候代价就大了。
下一章我们会讲传感器的时间同步，这也是个容易踩坑的地方。到时候再聊。