2. 坐标系与坐标变换：世界坐标系、车辆坐标系、传感器坐标系及其变换关系

做感知的同学，天天跟坐标系打交道。说白了，我们就是在不同坐标系之间来回倒腾数据。

我个人习惯把坐标系理解成「视角」。你站在路边看车是一个视角，车上的摄像头看路是另一个视角。感知算法的核心任务之一，就是把这些不同视角的信息统一起来。

2.1 三大坐标系，先搞清楚谁是谁

自动驾驶里常用的坐标系有三个：世界坐标系、车辆坐标系、传感器坐标系。嗯，咱们一个一个说。

2.1.1 世界坐标系

世界坐标系是全局的、固定的参考系。你可以把它想象成地球上的经纬度系统。一旦定义好，它就不动了。

作用：描述车辆、行人、道路标志等所有物体的绝对位置。
常见定义：通常采用东北天坐标系（ENU），即X轴指向东，Y轴指向北，Z轴指向天。
单位：米（m）。

我的经验：在高精地图里，世界坐标系的原点通常选在某个固定地标上。我在做园区项目时，就直接把停车场入口设为原点，省去了很多换算麻烦。

2.1.2 车辆坐标系

车辆坐标系是跟着车走的。车动，坐标系就动。它描述的是「相对于车」的位置。

作用：描述车辆自身的运动状态，以及周围物体相对于车辆的位置。
常见定义：ISO 8855标准。X轴指向车辆前方，Y轴指向车辆左侧，Z轴指向车辆上方。原点通常在后轴中心或车辆质心。
单位：米（m），角度用弧度（rad）。

注意：不同车厂对车辆坐标系定义可能不同。有的把原点放在前轴中心，有的放在后轴。我曾经因为没确认这个细节，导致融合模块的偏差整整调了两天。

2.1.3 传感器坐标系

每个传感器都有自己的小坐标系。摄像头、激光雷达、毫米波雷达，各说各话。

摄像头坐标系：原点在光心，Z轴指向镜头前方，X轴向右，Y轴向下（图像坐标系常用）。
激光雷达坐标系：原点在激光雷达中心，通常X轴指向车辆前方，Y轴指向左侧，Z轴向上。
毫米波雷达坐标系：类似激光雷达，但角度分辨率较低。

你想想看，如果每个传感器都用自己的坐标系汇报数据，那融合模块根本没法干活。所以，必须做坐标变换。

2.2 坐标变换，其实就是矩阵乘法

坐标变换听起来高大上，说白了就是旋转加平移。数学上用一个4x4的齐次变换矩阵搞定。

一个点从坐标系A变换到坐标系B，公式长这样：

P_B = T_A2B * P_A

其中，T_A2B 是变换矩阵，包含了旋转矩阵R和平移向量t。

T = | R   t |
    | 0   1 |

旋转矩阵R是3x3的，描述两个坐标系之间的朝向关系。平移向量t是3x1的，描述原点之间的偏移。

  核心要点：变换矩阵是可逆的。从A到B的变换矩阵的逆，就是从B到A的变换矩阵。这个性质在实际工程中非常有用。

2.2.1 传感器到车辆坐标系的变换

这是最常用的变换。每个传感器安装在车上的位置和朝向都是固定的，所以这个变换矩阵是固定的，叫做外参。

举个例子，激光雷达装在车顶，它的坐标系原点在车顶中心，X轴指向车头。要把它变换到车辆坐标系，需要：

测量激光雷达相对于车辆坐标系原点的偏移量（平移）。
测量激光雷达的安装角度（旋转）。
组合成变换矩阵。

# 伪代码示例：激光雷达到车辆坐标系的变换
T_lidar2vehicle = get_transform(translation=[0.0, 0.0, 1.5],  # 车顶高度1.5米
                                rotation=[0.0, 0.0, 0.0])    # 朝向与车辆一致

# 将激光雷达点云变换到车辆坐标系
points_vehicle = T_lidar2vehicle * points_lidar

2.2.2 车辆到世界坐标系的变换

这个变换是动态的。车在跑，车辆坐标系的原点和朝向一直在变。变换矩阵由车辆的定位模块提供，通常是GPS/IMU组合导航的结果。

车辆坐标系到世界坐标系的变换，其实就是车辆的位姿（位置+朝向）。

# 伪代码示例：车辆到世界坐标系的变换
# 假设定位模块给出了车辆的位姿
vehicle_pose = get_vehicle_pose()  # 返回位置和朝向

T_vehicle2world = build_transform(translation=vehicle_pose.position,
                                  rotation=vehicle_pose.orientation)

# 将车辆坐标系下的障碍物变换到世界坐标系
obstacle_world = T_vehicle2world * obstacle_vehicle

2.3 一条完整的数据流

咱们走一遍完整流程。假设激光雷达检测到一个障碍物：

传感器坐标系：激光雷达报告障碍物在 (10, 2, 0) 米处（相对于激光雷达）。
车辆坐标系：乘以 T_lidar2vehicle，得到障碍物在 (10, 2, 1.5) 米处（相对于车辆）。
世界坐标系：乘以 T_vehicle2world，得到障碍物在全球坐标系下的绝对位置。

你看，每一步都是矩阵乘法。只要变换矩阵标定得准，数据就不会打架。

避坑指南：我曾经遇到过一个诡异的问题——障碍物在车辆坐标系下明明在正前方，到了世界坐标系却跑到左边去了。查了半天，发现是车辆朝向的初始值给错了。所以，定位模块的初始对准非常关键。

2.4 坐标系变换的工程实现

在实际代码里，我们通常用现成的库来处理坐标变换。比如ROS里的tf库，或者Eigen、Sophus等数学库。

我个人习惯用齐次坐标，因为可以统一处理旋转和平移，代码更简洁。

// C++ 示例：使用Eigen库进行坐标变换
#include <Eigen/Dense>

// 定义变换矩阵
Eigen::Matrix4d T_lidar2vehicle;
T_lidar2vehicle << 1, 0, 0, 0,
                    0, 1, 0, 0,
                    0, 0, 1, 1.5,
                    0, 0, 0, 1;

// 激光雷达坐标系下的点
Eigen::Vector4d point_lidar(10, 2, 0, 1);

// 变换到车辆坐标系
Eigen::Vector4d point_vehicle = T_lidar2vehicle * point_lidar;

2.5 本章知识体系

下面这张图总结了坐标系与坐标变换的核心逻辑：

说白了，坐标变换就是搭桥。把传感器看到的局部信息，一步步映射到全局世界中去。这个桥搭得稳不稳，直接决定了感知系统的精度。

嗯，关于坐标系和坐标变换，就先聊到这儿。记住一点：标定是基础，变换是手段，统一坐标系才是目的。

  一句话总结：坐标系是感知系统的语言，坐标变换是翻译器。搞不懂坐标系，感知系统就是鸡同鸭讲。

📚 课程目录

2. 坐标系与坐标变换：世界坐标系、车辆坐标系、传感器坐标系及其变换关系

2.1 三大坐标系，先搞清楚谁是谁

2.1.1 世界坐标系

2.1.2 车辆坐标系

2.1.3 传感器坐标系

2.2 坐标变换，其实就是矩阵乘法

2.2.1 传感器到车辆坐标系的变换

2.2.2 车辆到世界坐标系的变换

2.3 一条完整的数据流

2.4 坐标系变换的工程实现

2.5 本章知识体系