坐标系与空间变换：世界坐标系、车辆坐标系、相机坐标系、图像坐标系；刚体变换与齐次坐标

各位同学，咱们今天聊点硬核的——坐标系与空间变换。

说实话，我刚入行那会儿，觉得坐标系这东西不就是个数学概念嘛，有啥好学的？直到第一次做传感器标定，发现激光雷达和相机对同一个障碍物的位置差了半米多，我才意识到：坐标系搞不清楚，后面的所有工作都是白搭。

你想想看，自动驾驶系统里，传感器各自为政——激光雷达有自己的坐标系，相机有自己的坐标系，IMU也有自己的坐标系。要让它们协同工作，就必须把这些坐标系统一起来。说白了，就是给每个传感器发一个「身份证」，告诉系统：我在哪、我朝哪看。

1. 四大坐标系，一个都不能少

在自动驾驶的标定工作中，我们主要跟四个坐标系打交道。我习惯把它们分成两组：

• 全局坐标系：世界坐标系、车辆坐标系
• 局部坐标系：相机坐标系、图像坐标系

1.1 世界坐标系（World Coordinate System）

世界坐标系是绝对的参考系。它固定在地球上，不随车辆移动而改变。通常我们用 X_w, Y_w, Z_w 表示。

我记得有一次做高精地图标定，发现地图上的车道线和实际道路差了30厘米。查了半天，原来是世界坐标系的原点定义出了问题——有人把原点定在了路边的电线杆上，而不是标准的UTM投影原点。

1.2 车辆坐标系（Vehicle Coordinate System）

车辆坐标系是固定在车身上的。原点一般在车辆后轴中心或质心，X_v 指向车头，Y_v 指向左侧，Z_v 指向上方。

嗯，这里有个坑——不同车厂的车辆坐标系定义可能不一样。有的把 X_v 指向右侧，有的把原点放在前轴。所以每次拿到新车，我第一件事就是确认车辆坐标系定义文档。

1.3 相机坐标系（Camera Coordinate System）

相机坐标系的原点在相机光心，X_c 指向右侧，Y_c 指向下方，Z_c 指向相机前方（沿光轴方向）。

为什么 Y_c 指向下方？因为图像坐标系的原点在左上角，u 轴向右，v 轴向下。这样设计是为了方便从相机坐标系投影到图像坐标系。

1.4 图像坐标系（Image Coordinate System）

图像坐标系是二维的，原点在图像左上角，u 轴向右，v 轴向下。单位是像素。

这里有个容易混淆的地方：图像坐标系和像素坐标系其实是同一个东西，只是叫法不同。我见过不少新人把图像坐标系和相机坐标系搞混，结果投影公式怎么算都不对。

2. 坐标系之间的变换

搞清楚了四个坐标系，接下来就是它们之间的变换关系。我画了一张图，帮你理清思路：

从这张图你能看到，数据流是从世界坐标系一路变换到图像坐标系的。反过来，如果我们想从图像中恢复三维信息，就需要做逆变换。

3. 刚体变换与齐次坐标

刚体变换，说白了就是「旋转+平移」。它不改变物体的形状和大小，只改变位置和朝向。在自动驾驶中，传感器之间的相对位置关系就是刚体变换。

3.1 旋转矩阵

旋转矩阵是一个3x3的正交矩阵，满足 R^T R = I，行列式为1。它描述了坐标系之间的旋转关系。

我曾经在标定激光雷达和相机时，发现旋转矩阵算出来行列式是-1。查了半天，原来是坐标系定义搞反了——一个用的是右手系，另一个用的是左手系。嗯，这种低级错误，犯过一次就记住了。

3.2 平移向量

平移向量是一个3x1的向量，表示两个坐标系原点之间的偏移。比如，相机在车辆坐标系中的位置就是 t = [x, y, z]^T。

3.3 齐次坐标

为什么要引入齐次坐标？因为用普通的3x1向量表示三维点，旋转和平移是分开的——旋转用乘法，平移用加法。这很不方便。

齐次坐标把三维点 [x, y, z]^T 扩展成四维 [x, y, z, 1]^T，这样旋转和平移就可以合并成一个4x4的变换矩阵：

| R   t |
| 0   1 |

你看，一个矩阵搞定所有变换。这就是齐次坐标的魅力。

P_c = [R | t] * P_w

4. 实战：从世界坐标到像素坐标

好了，理论讲完了，咱们来点实际的。假设你有一个三维点 P_w = [10, 5, 2]^T（单位：米），想把它投影到图像上，需要几步？

1. 世界 → 车辆：用车辆在世界坐标系中的位姿（R, t）做刚体变换
2. 车辆 → 相机：用相机在车辆坐标系中的外参（R, t）做刚体变换
3. 相机 → 图像：用相机内参矩阵 K 做投影

写成代码就是：

import numpy as np

# 假设已知参数
R_w2v = np.eye(3)  # 世界到车辆的旋转
t_w2v = np.array([0, 0, 0])  # 世界到车辆的平移
R_v2c = np.eye(3)  # 车辆到相机的旋转
t_v2c = np.array([0.5, 0, 1.0])  # 车辆到相机的平移
K = np.array([[500, 0, 320],
              [0, 500, 240],
              [0, 0, 1]])  # 相机内参

# 世界坐标系下的点
P_w = np.array([10, 5, 2, 1])  # 齐次坐标

# 世界 -> 车辆
T_w2v = np.vstack([np.hstack([R_w2v, t_w2v.reshape(3,1)]), [0,0,0,1]])
P_v = T_w2v @ P_w

# 车辆 -> 相机
T_v2c = np.vstack([np.hstack([R_v2c, t_v2c.reshape(3,1)]), [0,0,0,1]])
P_c = T_v2c @ P_v

# 相机 -> 图像（归一化 + 内参投影）
P_c_norm = P_c[:3] / P_c[2]  # 归一化到 Z=1 平面
pixel = K @ P_c_norm

print(f"像素坐标: u={pixel[0]:.1f}, v={pixel[1]:.1f}")

5. 避坑指南

做坐标系变换时，有几个坑我踩过，分享给你：

• 坐标系方向搞反：比如把右手系当成左手系，结果旋转矩阵算出来是转置的。我建议每次拿到新传感器，先打印一个已知点验证一下。
• 齐次坐标忘记归一化：从相机坐标系投影到图像坐标系时，一定要先除以 Z 分量。我见过有人直接拿 [X, Y, Z] 乘内参矩阵，结果像素坐标完全不对。
• 外参和内参混用：外参是传感器之间的相对位姿，内参是传感器自身的参数。两者不能混淆。我习惯在代码里用不同的变量名区分，比如 extrinsic_matrix 和 intrinsic_matrix。

好了，这一章的内容就到这里。坐标系和空间变换是传感器标定的基础，后面我们会反复用到这些知识。记住一句话：坐标系搞清楚了，标定就成功了一半。