坐标系与空间变换：自动驾驶的“空间语言”
各位同学，大家好。我是你们这堂课的主讲人。
今天咱们聊一个非常基础、但又极其关键的话题——坐标系与空间变换。你想想看，一辆自动驾驶车在路上跑，它得知道自己在哪里，周围的车、行人、障碍物又在哪里。这些信息，本质上都是通过不同的“坐标系”来描述的。
我个人习惯把坐标系比作“空间语言”。车有车的语言，相机有相机的语言，世界有世界的语言。而我们做感知系统的核心任务之一，就是当个“翻译官”，让这些语言能互相理解。
核心观点： 没有坐标系，感知就是一团乱麻。没有空间变换，决策就是空中楼阁。
1. 四大坐标系：各司其职
在自动驾驶里，我们主要跟四个坐标系打交道。它们各有各的“地盘”，也各有各的“脾气”。
- 世界坐标系： 这是全局的、绝对的空间参考。比如，基于GPS或高精地图定义的经纬度、海拔。它不随车辆移动而改变，是“上帝视角”。
- 车辆坐标系： 这是以自车为中心的坐标系。通常原点在车辆后轴中心或质心，X轴指向车头，Y轴指向左侧，Z轴指向上方。它描述的是“我”与周围物体的相对关系。
- 相机坐标系： 这是以相机光心为原点的坐标系。Z轴指向相机正前方（光轴方向），X轴向右，Y轴向下。它描述的是“相机看到的世界”。
- 像素坐标系： 这是最终图像上的坐标系。原点在图像左上角，u轴向右，v轴向下。单位是像素。它描述的是“照片上的点”。
嗯，这里要注意：像素坐标系和相机坐标系是初学者最容易搞混的。像素坐标是离散的、二维的；相机坐标是连续的、三维的。从相机坐标到像素坐标，中间还隔着一个“图像坐标系”（物理成像平面），这个我们后面会细说。
我的经验： 我在项目中遇到过，有同事把像素坐标直接当成相机坐标去算距离，结果算出来的障碍物位置飘忽不定。后来排查了半天，才发现是坐标系搞错了。所以，每次做坐标变换前，先问自己一句：“我现在在哪个坐标系里？”
2. 坐标变换矩阵：空间变换的“数学引擎”
坐标系之间怎么转换？靠的就是坐标变换矩阵。说白了，就是用一个4x4的矩阵，把点从一个坐标系“搬”到另一个坐标系。
这个矩阵长这样：
| R t |
| 0 1 |其中，R 是3x3的旋转矩阵，描述旋转关系；t 是3x1的平移向量，描述平移关系。最后一行是 [0, 0, 0, 1]，用来保持齐次坐标的形式。
为什么要用齐次坐标？因为这样可以把旋转和平移统一成一个矩阵乘法，方便计算。你想想看，如果不用齐次坐标，你得先旋转、再平移，两步操作。用齐次坐标，一步到位。
公式： P目标 = T目标←源 · P源
其中 T 就是变换矩阵。
3. 刚体变换与投影：从3D到2D的“魔法”
坐标变换主要分两类：刚体变换和投影变换。
- 刚体变换： 只改变物体的位置和朝向，不改变形状和大小。比如，从世界坐标系到车辆坐标系，或者从车辆坐标系到相机坐标系（在相机不动的前提下）。它由旋转和平移组成。
- 投影变换： 这是从3D到2D的“降维打击”。比如，从相机坐标系到像素坐标系。这个过程会丢失深度信息，所以是不可逆的。你没法从一张2D图片精确还原出3D物体的位置，除非你有其他信息（比如深度估计）。
为什么会这样？因为投影变换本质上是“小孔成像”模型的数学表达。一个3D空间中的点，通过相机光心，投影到成像平面上，变成一个2D点。这个过程中，所有在一条直线上的3D点都会投影到同一个2D点上。
避坑指南： 我曾经在标定相机时，忽略了镜头畸变对投影的影响。结果投影出来的点总是有偏差，导致后续的感知结果不准。后来加上了畸变校正，问题才解决。所以，相机内参（包括畸变参数）的标定精度，直接决定了投影变换的准确性。
4. 核心变换流程：一张图看懂
下面这张图，是我自己总结的“坐标系变换全家桶”。它清晰地展示了从世界坐标到像素坐标的完整链路。
从这张图你可以看到，整个变换链路是单向的。从世界到车辆，再到相机，最后到像素。反过来，从像素坐标恢复3D坐标，就需要深度信息了。
5. 实战：一个简单的坐标变换示例
光说不练假把式。我们来看一个具体的例子。假设你有一个激光雷达点，在世界坐标系下的坐标是 (10, 5, 0) 米。现在你想知道它在像素坐标系下的位置。
你需要做以下几步：
- 世界 -> 车辆： 通过车辆定位信息（GPS+IMU+里程计），得到车辆在世界坐标系下的位姿。然后计算变换矩阵 T车←世，将点转换到车辆坐标系。
- 车辆 -> 相机： 通过相机外参（相机相对于车辆的安装位置和朝向），得到变换矩阵 T相←车，将点转换到相机坐标系。
- 相机 -> 像素： 通过相机内参（焦距、主点、畸变参数），进行投影变换，得到像素坐标。
代码实现（伪代码）如下：
// 输入：世界坐标点 P_w
// 输出：像素坐标点 p_uv
// 1. 世界 -> 车辆
P_v = T_vehicle_world * P_w;
// 2. 车辆 -> 相机
P_c = T_cam_vehicle * P_v;
// 3. 相机 -> 像素（投影）
p_uv = project(P_c, K, dist_coeffs);嗯，这里要注意：变换矩阵的乘法顺序不能搞反。矩阵乘法不满足交换律，顺序错了，结果就完全不对了。
我的习惯： 我一般会在代码里把每个变换矩阵都单独定义，并加上注释说明“从哪个坐标系到哪个坐标系”。这样即使过了一个月再回来看，也不会搞混。
6. 总结与避坑
好了，今天的内容就到这里。我们回顾一下重点：
- 四大坐标系：世界、车辆、相机、像素，各有各的用途。
- 坐标变换矩阵：用4x4矩阵统一描述旋转和平移。
- 刚体变换 vs 投影变换：前者可逆，后者不可逆。
- 变换链路：从世界到像素，一步都不能少。
最后，再分享一个我踩过的坑：单位一致性。我曾经在融合毫米波雷达和相机数据时，雷达用的是米，相机外参用的是毫米，结果算出来的目标位置差了十万八千里。所以，在做任何坐标变换之前，先确认所有输入数据的单位是否一致。
希望今天的分享对你有帮助。咱们下节课见。