第二章：坐标系与投影基础

做BEV感知，说白了就是解决一个问题：怎么把相机看到的2D图像，映射到车顶俯瞰的3D空间里。

这背后绕不开坐标系和投影变换。我刚开始接触这个领域时，觉得不就是几个坐标系来回倒腾嘛，结果第一次做实车标定，投影出来的点全飘到天上去了……嗯，从那以后我再也不敢小看这部分基础。

2.1 四个核心坐标系

BEV感知里，我们打交道的主要有四个坐标系。我习惯把它们分成两组来理解：

• 传感器侧：像素坐标系、相机坐标系
• 车体侧：车身坐标系、世界坐标系

2.1.1 像素坐标系 (u, v)

这就是你看到的图像本身。原点在左上角，u轴向右，v轴向下。单位是像素。

举个例子：一张1920×1080的图像，像素坐标(960, 540)就是正中心。

2.1.2 相机坐标系 (Xc, Yc, Zc)

相机坐标系的原点在相机光心。Zc轴指向相机前方（也就是镜头朝向），Xc向右，Yc向下（跟图像坐标系对齐）。

这里有个关键点：相机坐标系是三维的，像素坐标系是二维的。从3D到2D，就是投影变换要干的事。

2.1.3 车身坐标系 (Xv, Yv, Zv)

车身坐标系的原点通常在后轴中心（或者车辆质心）。Xv指向车头，Yv指向左侧，Zv指向上方。

为什么用后轴中心？因为车辆运动学建模时，后轴中心是理想的参考点。我见过一些新手直接把GPS天线位置当车身原点，结果做轨迹规划时偏差很大。

2.1.4 世界坐标系 (Xw, Yw, Zw)

世界坐标系是全局参考系。在自动驾驶里，通常用UTM坐标系或者局部ENU坐标系。原点可以是任意选定的固定点，比如地图上的某个地标。

你想想看，车在跑，世界坐标系是不动的。车身坐标系跟着车动。所以从车身到世界的变换，本质上就是车辆的位姿（位置+朝向）。

2.2 内参与外参

搞清楚了坐标系，接下来就是怎么在它们之间来回切换。这里有两个核心概念：内参和外参。

2.2.1 内参矩阵 (Intrinsic Matrix)

内参描述的是相机本身的属性。说白了，就是光线穿过镜头后，怎么落到传感器上的。

内参矩阵K长这样：

K = [fx,  0, cx
      0, fy, cy
      0,  0,  1]

• fx, fy：焦距（以像素为单位）。fx = 焦距(mm) × 像素密度(pixel/mm)
• cx, cy：光心在像素坐标系中的位置。通常接近图像中心。

我记得第一次自己标定相机内参时，用OpenCV的棋盘格标定法，跑了20张图，结果fx和fy差了3个像素。后来发现是棋盘格打印时纸张有轻微变形。所以建议：标定板一定要贴平在硬板上。

2.2.2 外参矩阵 (Extrinsic Matrix)

外参描述的是相机相对于车身（或世界）的位置和朝向。它是一个4×4的齐次变换矩阵：

T = [R, t
     0, 1]

• R：3×3旋转矩阵，描述朝向
• t：3×1平移向量，描述位置

举个例子：如果相机安装在车顶前方，t就表示相机光心相对于车身原点的偏移量。R则表示相机坐标系相对于车身坐标系的旋转。

2.3 投影变换公式推导

好了，现在我们把所有零件凑起来。从世界坐标系的一个3D点，到像素坐标系的一个2D点，整个过程是这样的：

2.3.1 完整投影链

世界点 Pw → 车身点 Pv → 相机点 Pc → 像素点 p

每一步对应一个变换：

1. 世界 → 车身：Pv = T_world_to_vehicle × Pw
2. 车身 → 相机：Pc = T_vehicle_to_camera × Pv
3. 相机 → 像素：p = K × Pc（归一化后取前两维）

合并起来就是：

p = K × T_vehicle_to_camera × T_world_to_vehicle × Pw

2.3.2 关键公式推导

从相机坐标系到像素坐标系这一步，是投影的核心。假设相机坐标系下有一个点 (Xc, Yc, Zc)：

u = fx × (Xc / Zc) + cx
v = fy × (Yc / Zc) + cy

为什么除以Zc？因为透视投影——远处的物体看起来更小。你想想看，一个10米外的行人，在图像上可能只有几十个像素；而1米外的行人，能占满半个屏幕。

写成齐次坐标形式更简洁：

[u]   [fx,  0, cx, 0]   [Xc]
[v] = [ 0, fy, cy, 0] × [Yc]
[1]   [ 0,  0,  1, 0]   [Zc]
                         [ 1]

然后归一化：u = u / Zc, v = v / Zc。

2.3.3 逆投影：从像素到3D射线

BEV感知里，我们经常需要做逆操作——给定一个像素点，反推它在3D空间中的可能位置。

但注意：单个像素点无法唯一确定3D位置，只能确定一条射线。

Xc = (u - cx) × Zc / fx
Yc = (v - cy) × Zc / fy

这里Zc是未知的。所以我们需要额外的信息来约束Zc，比如：

• 深度估计（单目深度网络）
• 地面假设（假设点在路面上）
• 多目立体匹配

2.4 实操：坐标系转换代码

下面给出一段Python代码，演示完整的投影流程。我习惯用NumPy做矩阵运算，清晰又高效。

import numpy as np

def project_world_to_pixel(Pw, K, T_vehicle_to_camera, T_world_to_vehicle):
    """
    将世界坐标系下的3D点投影到像素坐标系
    
    参数:
        Pw: 世界坐标系下的点，形状 (3,) 或 (N, 3)
        K: 内参矩阵，形状 (3, 3)
        T_vehicle_to_camera: 车身→相机变换矩阵，形状 (4, 4)
        T_world_to_vehicle: 世界→车身变换矩阵，形状 (4, 4)
    
    返回:
        p: 像素坐标，形状 (N, 2)
    """
    # 确保输入是齐次坐标
    if Pw.ndim == 1:
        Pw = Pw.reshape(1, -1)
    
    N = Pw.shape[0]
    Pw_h = np.hstack([Pw, np.ones((N, 1))])  # (N, 4)
    
    # 世界 → 车身 → 相机
    Pc_h = (T_vehicle_to_camera @ T_world_to_vehicle @ Pw_h.T).T  # (N, 4)
    
    # 归一化
    Pc = Pc_h[:, :3] / Pc_h[:, 2:3]  # (N, 3)
    
    # 相机 → 像素
    p_h = (K @ Pc.T).T  # (N, 3)
    p = p_h[:, :2] / p_h[:, 2:3]  # (N, 2)
    
    return p

# 示例：内参矩阵（假设640×480图像，焦距约500像素）
K = np.array([[500,   0, 320],
              [  0, 500, 240],
              [  0,   0,   1]])

# 外参：相机安装在车顶，向前看
# 假设相机在车身坐标系下位于 (0, 0, 1.5) 米，无旋转
T_vehicle_to_camera = np.array([
    [1, 0, 0, 0],
    [0, 1, 0, 0],
    [0, 0, 1, 1.5],
    [0, 0, 0, 1]
])

# 世界到车身：假设车辆位于世界原点，朝向正X
T_world_to_vehicle = np.eye(4)

# 测试：世界坐标系下 (10, 0, 0) 的点（前方10米）
Pw = np.array([10, 0, 0])
p = project_world_to_pixel(Pw, K, T_vehicle_to_camera, T_world_to_vehicle)
print(f"像素坐标: ({p[0,0]:.1f}, {p[0,1]:.1f})")
# 输出应该接近 (320, 240)，因为点在正前方，投影到图像中心

2.5 常见坑与避坑指南

这部分我踩过的坑比较多，挑几个典型的说说：

2.6 小结

这一章我们聊了：

• 四个核心坐标系：像素、相机、车身、世界
• 内参和外参的含义与矩阵形式
• 从3D世界到2D图像的完整投影链
• 逆投影的思路与局限性
• 实际代码实现和常见坑

说实话，坐标系和投影这部分，看起来简单，但真正做项目时最容易出问题。我建议你学完这章后，自己动手写一遍投影代码，然后用真实数据验证一下。只有亲手踩过坑，才能真正理解。

下一章，我们会进入BEV感知的核心——视角转换与特征映射。到时候会用到这章的所有知识，所以务必把基础打牢。

---

公众号:蓝海资料掘金营，微信deep3321