3. 坐标系与变换：从世界到像素的“变形记”

做自动驾驶感知，说白了就是搞清楚“车周围的东西在哪”。但传感器看到的，和我们脑子里的“世界”，用的不是同一套坐标系统。相机有相机的视角，激光雷达有激光雷达的坐标系，而车本身又有自己的位置。怎么把这些乱七八糟的坐标统一起来？这就是我们今天要聊的核心。

我个人习惯把这件事想象成一个“翻译”过程。世界坐标系是“普通话”，相机坐标系是“方言”，图像坐标系是“笔记”，像素坐标系是“屏幕上的点”。我们要做的，就是把这些方言和笔记，准确无误地翻译成普通话。

3.1 四个坐标系，一个故事

先认识一下这四位主角。它们之间的关系，说白了就是一条流水线：从真实世界的一个点，到相机拍到的照片上的一个像素。

3.2 世界坐标系 → 相机坐标系：刚体变换

这是最关键的一步。世界坐标系是固定的，比如我们以车启动时的位置为原点。相机坐标系是跟着车动的，原点在相机光心。从一个坐标系到另一个坐标系，需要做两件事：旋转和平移。合起来就叫刚体变换。

为什么叫“刚体”？因为物体本身不会变形，只是位置和朝向变了。你想想看，你拿着手机转个角度，手机本身没变，但拍到的画面完全不同了。

3.2.1 旋转矩阵 R

旋转用矩阵 R 表示。它是一个 3x3 的正交矩阵。说白了，就是告诉你坐标轴怎么转。

我记得有一次做标定，怎么都对不上。后来发现是旋转矩阵的符号搞反了。嗯，这里要注意：旋转矩阵的逆等于它的转置。这个性质在代码里经常用到。

// 绕 Z 轴旋转 θ 角
R_z = [
  [cos(θ), -sin(θ), 0],
  [sin(θ),  cos(θ), 0],
  [0,       0,      1]
]

3.2.2 平移向量 t

平移就简单了，就是两个坐标系原点之间的偏移量。用 t = [tx, ty, tz]^T 表示。

3.2.3 齐次坐标与变换矩阵

为了把旋转和平移写在一起，我们引入齐次坐标。把三维点 [X, Y, Z] 变成 [X, Y, Z, 1]。这样，刚体变换就可以用一个 4x4 的矩阵 T 来表示：

T = [R  t]
    [0  1]

// 变换公式
P_cam = T * P_world

3.3 相机坐标系 → 图像坐标系：透视投影

这一步是把三维空间中的点，投影到二维成像平面上。用的是小孔成像模型。

公式很简单：

x = f * X / Z
y = f * Y / Z

其中 f 是焦距，(X, Y, Z) 是相机坐标系下的点，(x, y) 是图像坐标系下的点。

你想想看，为什么远处的物体看起来小？就是因为 Z 变大了，x 和 y 就变小了。这就是透视效应。

3.4 图像坐标系 → 像素坐标系：离散化

图像坐标系是连续的，单位是毫米。像素坐标系是离散的，单位是像素。这一步就是把连续坐标变成整数坐标。

公式：

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

其中 dx, dy 是每个像素的物理尺寸，u0, v0 是图像中心（主点）的像素坐标。

说白了，就是把毫米换算成像素个数。比如一个像素是 0.01mm，那么 10mm 就是 1000 个像素。

3.5 内参矩阵 K：相机的“身份证”

把上面两步合并，就得到了相机的内参矩阵 K：

K = [fx,  0, u0]
    [ 0, fy, v0]
    [ 0,  0,  1]

其中 fx = f/dx, fy = f/dy。这个矩阵是相机的固有属性，出厂就定了。标定的目的，就是求出这个矩阵。

Z * [u, v, 1]^T = K * [R | t] * [X, Y, Z, 1]^T

3.6 实战中的坑与技巧

做点云与图像融合时，坐标系变换是第一步，也是最容易出错的一步。我总结几个常见问题：

• 坐标系定义不一致： 不同传感器厂商对坐标轴的定义可能不同。比如有的相机 Z 轴朝前，有的朝右。一定要先统一坐标系。
• 旋转顺序： 旋转矩阵不满足交换律。先绕 X 轴转再绕 Y 轴转，和反过来结果不同。我建议固定使用 ZYX 顺序。
• 精度问题： 浮点数运算有误差。尤其是在做逆变换时，误差会放大。建议使用 double 类型。

3.7 总结

坐标系变换是自动驾驶感知的基石。说白了，就是搞清楚“我在哪，相机在哪，像素在哪”。刚体变换解决位置和朝向，投影解决“怎么看”，离散化解决“怎么存”。

我个人觉得，理解这些概念最好的方式，就是动手写代码。从最简单的单点变换开始，逐步扩展到整个点云。遇到问题不要慌，画个图，理清坐标系关系，问题往往就迎刃而解了。