第2章坐标系与空间变换

第2章：坐标系与空间变换

各位同学，今天我们来聊聊BEV感知里最基础、也最容易踩坑的一环——坐标系与空间变换。

说实话，我刚入行那会儿，觉得坐标系不就是几个箭头嘛，有什么好学的？结果第一次做多传感器融合项目，摄像头和激光雷达的数据怎么都对不齐，折腾了两天才发现是坐标系定义搞反了。嗯，从那以后，我再也不敢小看这个环节了。

2.1 四大坐标系，你分得清吗？

在自动驾驶里，我们天天跟四个坐标系打交道。你想想看，摄像头看到的是像素，激光雷达扫到的是点云，车辆自己有个位置，地图又有自己的坐标——这四者怎么统一？

说白了，就是通过坐标变换，把不同传感器的数据都映射到同一个参考系里。

2.1.1 相机坐标系

相机坐标系，是以相机光心为原点，Z轴指向拍摄方向，X轴向右，Y轴向下。为什么Y轴向下？因为图像坐标系的原点在左上角，Y轴朝下。

我个人习惯把相机坐标系想象成「相机看世界的方式」——它看到的是一个锥形的视野，近大远小。

重要概念：相机坐标系是3D的，而图像是2D的。从3D到2D，我们丢失了深度信息。这也是为什么单目BEV感知那么难——你缺了一个维度。

2.1.2 车辆坐标系

车辆坐标系，也叫车身坐标系。通常以车辆后轴中心为原点，X轴指向车头，Y轴指向左侧，Z轴向上。

这里有个坑：不同车厂的定义可能不一样。有的用前轴中心，有的用质心。我在项目中遇到过，两个团队用的车辆坐标系定义不同，融合结果直接偏了半米。所以，拿到数据第一件事——确认坐标系定义。

2.1.3 世界坐标系

世界坐标系，就是全局的参考系。在自动驾驶里，通常用UTM坐标系或者自定义的局部坐标系。

世界坐标系是固定的，车辆在动，但世界不动。我们做BEV感知，最终输出的障碍物位置，就是要用世界坐标系来表达。

2.1.4 图像坐标系与像素坐标系

这两个容易搞混。图像坐标系以图像中心为原点，单位是毫米；像素坐标系以左上角为原点，单位是像素。

它们之间就差一个平移和缩放。说白了，就是物理尺寸到像素的映射。

2.2 内外参矩阵：相机的「身份证」

相机参数分两种：内参和外参。内参是相机自己的属性，外参是相机在空间中的位置。

2.2.1 内参矩阵

内参矩阵K，描述了从相机坐标系到像素坐标系的映射。它长这样：

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

fx、fy是焦距（像素单位），cx、cy是主点偏移。为什么会有偏移？因为镜头中心和感光元件中心不一定完全对齐。

我记得有一次标定，发现cx、cy偏得离谱，检查了半天才发现是镜头装歪了。嗯，硬件问题也会反映在参数里。

2.2.2 外参矩阵

外参矩阵[R|t]，描述了相机坐标系到车辆坐标系的变换。R是旋转矩阵，t是平移向量。

外参是动态的——车在动，相机相对于车的位置不变，但相对于世界在变。所以，外参通常分为两部分：

静态外参：相机相对于车辆的固定安装位置
动态外参：车辆相对于世界的位置（由定位模块提供）

我的经验：外参标定是BEV感知的「地基」。地基没打好，后面所有工作都是白费。我建议每次装车后都做一次外参标定，哪怕只是微调。

2.3 坐标变换的数学基础

坐标变换，说白了就是矩阵乘法。一个3D点从相机坐标系变换到像素坐标系，只需要三步：

世界坐标系 → 相机坐标系（外参变换）
相机坐标系 → 归一化平面（透视投影）
归一化平面 → 像素坐标系（内参变换）

写成公式就是：

pixel = K * [R|t] * world_point

等等，这里有个细节。K是3x3，[R|t]是3x4，world_point是4x1（齐次坐标）。所以结果是3x1的齐次坐标，再归一化就得到像素坐标。

2.3.1 齐次坐标

为什么要用齐次坐标？因为平移变换在普通坐标里是加法，在齐次坐标里变成了乘法。这样，旋转和平移就可以统一成一个矩阵。

你想想看，如果不用齐次坐标，你要先旋转再平移，写两行代码。用了齐次坐标，一行矩阵乘法搞定。多清爽。

2.3.2 旋转矩阵与欧拉角

旋转矩阵R是正交矩阵，行列式为1。它有9个元素，但自由度只有3。所以，我们通常用欧拉角（roll、pitch、yaw）来表示旋转。

但欧拉角有个问题——万向锁。当pitch为±90度时，roll和yaw会耦合。我在做俯仰角很大的车载相机标定时遇到过这个问题，后来改用四元数才解决。

避坑指南：我曾经在代码里直接用欧拉角做插值，结果旋转路径绕了一大圈。后来才意识到，欧拉角插值要用四元数SLERP。记住：欧拉角适合人看，四元数适合机器算。

2.4 BEV视角下的坐标变换

做BEV感知，核心就是把相机视角的图像，变换到鸟瞰视角。这个过程叫IPM（Inverse Perspective Mapping，逆透视映射）。

IPM的原理很简单：假设地面是平的，那么相机坐标系下的地面点，可以通过单应性矩阵映射到BEV坐标系。

但这里有个前提——地面必须是平的。如果路面有坡度，IPM就会变形。我在高速项目上遇到过，匝道有坡度，BEV图像直接扭曲了。后来我们加了地面高程模型才解决。

2.4.1 单应性矩阵

单应性矩阵H，描述了同一平面在两个相机视角下的映射关系。在BEV里，我们用它把相机图像映射到地面平面。

H = K * (R - t * n^T / d) * K_inv

其中n是地面法向量，d是相机到地面的距离。这个公式看着复杂，但说白了就是：知道相机位置和地面方程，就能算出映射关系。

2.4.2 前向投影 vs 反向投影

做IPM时，有两种方式：

前向投影：把每个像素投影到BEV网格上。速度快，但会有空洞。
反向投影：对BEV网格的每个点，反查它在原图中的位置。质量好，但计算量大。

我个人习惯用反向投影，虽然慢一点，但图像质量好。现在GPU算力强了，这点计算量不算什么。

2.5 实战：写一个坐标变换工具

光说不练假把式。我们来写一个简单的坐标变换函数：

def transform_point(point, R, t, K):
    """
    将3D点从世界坐标系变换到像素坐标系
    point: [x, y, z] 世界坐标
    R: 3x3 旋转矩阵
    t: 3x1 平移向量
    K: 3x3 内参矩阵
    """
    # 世界坐标转相机坐标
    cam_point = R @ point + t.flatten()
    
    # 相机坐标转归一化平面
    norm_point = cam_point[:2] / cam_point[2]
    
    # 归一化平面转像素坐标
    pixel = K @ np.append(norm_point, 1)
    pixel = pixel[:2] / pixel[2]
    
    return pixel.astype(int)

这段代码看着简单，但我在项目里见过无数人写错。最常见的问题：忘记归一化，或者把齐次坐标和普通坐标搞混。

调试技巧：写坐标变换代码时，先用手算一个简单点验证。比如原点(0,0,0)变换后应该落在哪里。如果结果不对，八成是矩阵维度搞错了。

2.6 总结

坐标系与空间变换，是BEV感知的「第一性原理」。你想想看，所有传感器数据最终都要统一到同一个坐标系下，这一步做不好，后面再牛的算法也是白搭。

我见过太多团队，算法模型调得飞起，结果发现是坐标变换有bug。嗯，这种坑踩过一次就记住了。

下一章，我们会聊BEV感知的核心——视角转换与特征对齐。到时候你会发现，今天学的这些坐标变换知识，全都会用上。

核心要点回顾：

四大坐标系：相机、车辆、世界、图像/像素
内参矩阵：相机自身的属性，焦距+主点
外参矩阵：相机在空间中的位置，旋转+平移
齐次坐标：把旋转和平移统一成矩阵乘法
IPM：从相机视角到鸟瞰视角的映射，假设地面是平的

好了，今天就到这里。有什么问题，欢迎在课程群里讨论。记住：坐标系搞清楚了，BEV感知就成功了一半。

📚 课程目录