1. 坐标系与刚体变换基础

做多传感器融合，第一件事是什么？

不是调算法，不是写代码。是先把坐标系搞清楚。

我见过太多项目，标定结果看起来挺好，一融合就崩。查到最后，往往是坐标系定义不一致。说白了，你以为是X轴朝前，他以为是Y轴朝前，那还怎么玩？

先说说世界坐标系。你可以把它理解成一个"绝对参考系"。不管你的车怎么动，不管你的激光雷达怎么转，世界坐标系始终不变。

我个人习惯把世界坐标系的原点设在起始位置。比如无人车从车库出发，我就把车库门口设为原点。X轴指向正东，Y轴指向正北，Z轴朝天。这样后面做轨迹回放、地图构建都方便。

传感器坐标系就不一样了。每个传感器都有自己的"小世界"：

核心原则：所有传感器数据最终都要统一到同一个坐标系下。通常是车体坐标系或世界坐标系。

刚体变换，说白了就是"怎么把一个点从一个坐标系搬到另一个坐标系"。它包含两部分：旋转和平移。

旋转矩阵是个3x3的矩阵。它描述了坐标轴的旋转关系。比如相机看到的一个点，怎么用激光雷达的坐标表示？

// 旋转矩阵示例：绕Z轴旋转θ角度
R_z(θ) = [
  [cosθ, -sinθ, 0],
  [sinθ,  cosθ, 0],
  [0,      0,    1]
]

我记得刚入行时，总搞不清旋转矩阵是左乘还是右乘。后来想通了：左乘是旋转坐标系，右乘是旋转向量。标定中我们用的是左乘，因为我们要把传感器坐标系下的点变换到另一个坐标系。

平移向量就简单了。就是两个坐标系原点之间的偏移。比如激光雷达装在车顶，相机装在车头，它们之间有个固定的位移。

完整的刚体变换公式：

P_w = R * P_s + t

其中P_w是世界坐标系下的点，P_s是传感器坐标系下的点，R是旋转矩阵，t是平移向量。

我的经验：标定时，平移向量往往比旋转矩阵更容易收敛。如果标定结果一直不理想，先检查旋转矩阵的初值给对了没有。

为什么要引入齐次坐标？

你想想看，上面的公式P_w = R * P_s + t，既有乘法又有加法。如果连续做多次变换，公式会变得非常冗长。齐次坐标就是来解决这个问题的。

齐次坐标给三维点加了一个维度：

// 普通坐标 (x, y, z) -> 齐次坐标 (x, y, z, 1)
// 方向向量 (dx, dy, dz) -> 齐次坐标 (dx, dy, dz, 0)

注意看：点的最后一个分量是1，方向向量的最后一个分量是0。这个设计很巧妙——方向向量平移不变。

有了齐次坐标，刚体变换就变成了一个4x4矩阵：

T = [
  [R(0,0), R(0,1), R(0,2), t_x],
  [R(1,0), R(1,1), R(1,2), t_y],
  [R(2,0), R(2,1), R(2,2), t_z],
  [0,      0,      0,      1   ]
]

这样，多次变换就变成了矩阵乘法：

P_w = T_wc * P_c
// 其中T_wc是从相机坐标系到世界坐标系的变换矩阵

我曾经踩过的坑：变换矩阵的逆矩阵不是简单的转置。旋转部分可以转置（因为旋转矩阵是正交矩阵），但平移部分不能直接转置。正确的逆变换是：

T_inv = [
  [R^T,  -R^T * t],
  [0,    1       ]
]

下面这张图总结了本章的核心内容。我建议你把它存下来，后面做标定时会经常用到。

讲个真实案例。有一次我帮客户标定一个多传感器系统，有3个相机、2个激光雷达、1个IMU。每个传感器都有自己的坐标系定义。客户给的标定板数据，有的用毫米，有的用米。

我当时做了这么几件事：

我的建议：在代码里用专门的类来管理坐标系变换。不要到处写R和t。我习惯用一个TransformTree类，每次变换都从树里查。这样改一个传感器的外参，所有依赖它的变换自动更新。

嗯，坐标系和刚体变换就讲这么多。这些概念看起来简单，但实际项目中80%的标定问题都出在这里。后面讲具体标定方法时，你会发现所有操作都是在求解和验证这些R和t。

本章要点回顾：

可可精品资料库