3、坐标系统与变换：世界坐标系、传感器坐标系、刚体变换与齐次坐标
做多传感器融合，说白了就是在跟坐标系打交道。
我刚开始接触这个领域时，觉得算法才是核心。后来踩了几个坑才明白——坐标系搞错了，再牛的算法也是白搭。你想想看，激光雷达说目标在左边，摄像头说在右边，到底信谁？
嗯，这节课我们就来把坐标系这件事彻底讲清楚。
3.1 世界坐标系：我们共同的“锚点”
世界坐标系，就是所有传感器数据的“共同语言”。
每个传感器都有自己的小世界，但最终都要统一到一个大世界里。这个大世界，就是世界坐标系。
核心要点：世界坐标系是固定的，不随任何传感器移动。通常我们选地面上的某个点作为原点，比如停车场的第一根柱子，或者实验室的墙角。
我个人习惯用右手坐标系：X轴指向东，Y轴指向北，Z轴指向天。当然，不同项目有不同约定。我记得有一次做车载项目，客户坚持用Z轴向下——嗯，那段时间我经常搞混俯仰角的正负方向。
世界坐标系的作用就三个：
- 统一基准：所有传感器的数据最终都投影到这里
- 路径规划：机器人的运动轨迹是在世界坐标系下描述的
- 地图构建：SLAM建出来的地图，就是世界坐标系下的点云
3.2 传感器坐标系：每个传感器的“小世界”
每个传感器都有自己的坐标系。激光雷达的坐标系原点在激光头中心，摄像头的坐标系原点在光心，IMU的坐标系原点在芯片中心。
这些坐标系会随着传感器移动而移动。你想想看，车在转弯时，激光雷达坐标系也在跟着转。
常见的传感器坐标系：
| 传感器类型 | 原点位置 | 坐标轴定义 |
|---|---|---|
| 激光雷达 | 激光发射中心 | X向前，Y向左，Z向上 |
| 摄像头 | 光心 | Z向前，X向右，Y向下（图像坐标系） |
| IMU | 芯片几何中心 | X向前，Y向左，Z向上 |
| GPS/RTK | 天线相位中心 | ENU（东-北-天） |
小技巧：我建议在项目一开始就把所有传感器的坐标系画出来，贴在墙上。我曾经因为搞混了摄像头Z轴方向（到底是向前还是向下），导致融合结果偏了半米，查了两天才找到原因。
3.3 刚体变换：把数据从一个坐标系搬到另一个
刚体变换，说白了就是旋转加平移。物体本身不变，只是换个角度看它。
数学上，一个刚体变换可以写成：
P' = R * P + T其中：
- P：在原坐标系下的坐标
- P'：在新坐标系下的坐标
- R：旋转矩阵（3x3）
- T：平移向量（3x1）
旋转矩阵有9个元素，但自由度只有3个（绕X、Y、Z轴的旋转角）。所以实际项目中，我们常用欧拉角或四元数来表示旋转。
避坑指南：我曾经在项目中直接用欧拉角做插值，结果出现了万向锁。从那以后，我只要涉及旋转插值，一律用四元数。记住：欧拉角适合给人看，四元数适合给机器算。
3.4 齐次坐标：把旋转和平移统一起来
你有没有觉得上面的公式有点麻烦？先乘R，再加T，写代码时还得分开处理。
齐次坐标就是来解决这个问题的。它给三维坐标加了一个维度，变成四维：
P_homogeneous = [x, y, z, 1]^T这样，刚体变换就变成了一个矩阵乘法：
P' = M * P其中M是4x4的变换矩阵：
M = [R T]
[0 1]你看，旋转和平移被塞进了一个矩阵里。连续变换时，只需要把矩阵乘起来就行。
为什么用齐次坐标？
- 统一了旋转和平移的表示
- 可以方便地组合多个变换
- 在GPU上计算效率更高
3.5 实战：激光雷达到世界坐标系的变换
我们来看一个实际例子。假设你有一辆自动驾驶车，激光雷达装在车顶。
要把激光雷达的点云投影到世界坐标系，需要两步：
- 激光雷达到车体坐标系：这是固定的外参，标定一次就行
- 车体坐标系到世界坐标系：这取决于车辆的当前位置和姿态（由GPS/IMU提供）
代码实现大概是这样的：
// 激光雷达点云变换到世界坐标系
Eigen::Matrix4f T_lidar_to_vehicle; // 外参标定结果
Eigen::Matrix4f T_vehicle_to_world; // 由GPS/IMU实时计算
for (auto& point : point_cloud) {
Eigen::Vector4f p_lidar(point.x, point.y, point.z, 1.0);
Eigen::Vector4f p_world = T_vehicle_to_world * T_lidar_to_vehicle * p_lidar;
// 现在p_world就是世界坐标系下的坐标
}个人经验：我建议把变换矩阵的乘法顺序写清楚，加个注释。我曾经因为搞反了乘法顺序，把点云投影到了地下两米的位置。嗯，那次调试花了我一整个下午。
3.6 知识体系总览
下面这张图展示了本章的核心逻辑：
这张图把本章的核心逻辑串起来了。你看，世界坐标系和传感器坐标系之间，靠的就是刚体变换。而齐次坐标，是描述刚体变换最优雅的方式。
最后提醒一句：坐标系变换的精度，直接决定了融合效果的上限。我见过太多项目，算法调得再好，最后发现是外参标定偏了0.1度——嗯，那感觉就像盖房子地基没打平。
做多传感器融合，坐标系就是你的地基。打好这个基础，后面的路就好走了。