3、坐标系与变换：世界坐标系、相机坐标系、图像坐标系、像素坐标系、激光雷达坐标系，刚体变换与欧拉角

做传感器融合，说白了就是跟坐标系打交道。你想想看，激光雷达说「前方5米有个障碍物」，相机说「图像右下角有个红色物体」，这两句话用的坐标系完全不一样。怎么让它们对上话？这就是本章要解决的问题。

我个人习惯把坐标系变换比作「翻译官」。每个传感器都有自己的语言，我们需要一个通用的翻译规则。嗯，这个规则就是刚体变换和欧拉角。

3.1 五大坐标系，一个都不能少

在自动驾驶中，我们主要跟五个坐标系打交道。我刚开始做项目时，经常搞混它们的关系，后来画了一张图才彻底理清。

3.1.1 世界坐标系（World Coordinate System）

这是全局参考系。你可以把它想象成「上帝视角」。通常我们以车辆启动位置为原点，或者用GPS/IMU定义。它用 (X_w, Y_w, Z_w) 表示。

• 作用：统一所有传感器的位置信息
• 特点：固定不动，不随车辆移动而改变
• 我在项目中遇到过：有一次标定结果总是不对，排查了半天才发现是世界坐标系的原点定义跟IMU不一致。从那以后，我每次都会先确认「原点在哪」。

3.1.2 相机坐标系（Camera Coordinate System）

以相机光心为原点，Z轴指向镜头前方，X轴向右，Y轴向下。这是相机「看到」的世界。

• 表示：(X_c, Y_c, Z_c)
• 注意：相机坐标系是右手系，但Y轴方向跟常规右手系相反

3.1.3 图像坐标系（Image Coordinate System）

这是相机内部的物理成像平面。原点在图像中心，单位是毫米。用 (x, y) 表示。

• 关系：通过小孔成像模型，从相机坐标系投影得到
• 公式：x = f * X_c / Z_c，y = f * Y_c / Z_c

3.1.4 像素坐标系（Pixel Coordinate System）

这就是我们最终看到的图像。原点在左上角，u轴向右，v轴向下。单位是像素。

• 转换：从图像坐标系到像素坐标系，需要知道像素尺寸和主点偏移
• 公式：u = x / dx + u₀，v = y / dy + v₀

3.1.5 激光雷达坐标系（LiDAR Coordinate System）

以激光雷达中心为原点，通常X轴指向车辆前方，Y轴向左，Z轴向上。这是点云数据的「母语」。

• 表示：(X_l, Y_l, Z_l)
• 特点：点云数据直接就是三维坐标，不需要投影

3.2 坐标系变换的数学本质

坐标系变换，说白了就是「旋转+平移」。你想想看，把一个点从激光雷达坐标系挪到世界坐标系，无非就是先转个角度，再挪个位置。

3.2.1 刚体变换（Rigid Transformation）

刚体变换不改变物体的形状和大小。它由两部分组成：

• 旋转矩阵 R：3x3的正交矩阵，描述旋转
• 平移向量 t：3x1的向量，描述平移

数学表达式：P' = R * P + t

其中 P 是原始坐标系下的点，P' 是目标坐标系下的点。

3.2.2 欧拉角（Euler Angles）

欧拉角是描述旋转的另一种方式。它用三个角度表示：绕X轴旋转（roll）、绕Y轴旋转（pitch）、绕Z轴旋转（yaw）。

角度	符号	描述	自动驾驶中的常见含义
横滚角	roll (φ)	绕X轴旋转	车辆侧倾
俯仰角	pitch (θ)	绕Y轴旋转	车辆抬头/低头
偏航角	yaw (ψ)	绕Z轴旋转	车辆转向

欧拉角转旋转矩阵的公式（以ZYX顺序为例）：

R = Rz(yaw) * Ry(pitch) * Rx(roll)

其中：
Rz(yaw) = [[cosψ, -sinψ, 0],
           [sinψ,  cosψ, 0],
           [0,      0,    1]]

Ry(pitch) = [[cosθ,  0, sinθ],
             [0,     1, 0],
             [-sinθ, 0, cosθ]]

Rx(roll) = [[1, 0,    0],
            [0, cosφ, -sinφ],
            [0, sinφ,  cosφ]]

3.3 从激光雷达到像素：完整的变换链路

现在我们把所有知识串起来。假设你有一个激光雷达点 P_l，想把它投影到像素坐标 (u, v)。

1. 激光雷达 → 世界坐标系：P_w = R_l2w * P_l + t_l2w
2. 世界坐标系 → 相机坐标系：P_c = R_w2c * P_w + t_w2c
3. 相机坐标系 → 图像坐标系：x = f * X_c / Z_c，y = f * Y_c / Z_c
4. 图像坐标系 → 像素坐标系：u = x / dx + u₀，v = y / dy + v₀

实际项目中，我们通常把前两步合并成一个外参矩阵，后两步合并成内参矩阵。这样只需要一次矩阵乘法：

# 伪代码示例
def project_lidar_to_pixel(pt_lidar, extrinsic, intrinsic):
    # pt_lidar: [x, y, z, 1] 齐次坐标
    # extrinsic: 4x4 外参矩阵 (激光雷达到相机)
    # intrinsic: 3x3 内参矩阵
    
    # 变换到相机坐标系
    pt_cam = extrinsic @ pt_lidar
    
    # 投影到像素
    u = intrinsic[0,0] * pt_cam[0] / pt_cam[2] + intrinsic[0,2]
    v = intrinsic[1,1] * pt_cam[1] / pt_cam[2] + intrinsic[1,2]
    
    return u, v

3.4 本章知识体系

下面这张图展示了五个坐标系之间的变换关系，以及刚体变换和欧拉角在其中的作用。你可以把它当作一个「导航图」，随时回来查阅。

这张图里，你从激光雷达坐标系出发，经过外参变换到世界坐标系，再经过另一组外参到相机坐标系，然后通过内参投影到图像和像素坐标系。刚体变换和欧拉角就是连接这些坐标系的「桥梁」。

好了，坐标系和变换就讲到这里。记住一句话：所有传感器融合的问题，最终都是坐标系变换的问题。把这个基础打牢，后面的联合标定就是水到渠成的事。

可可精品资料库