第二章 坐标系与变换：世界坐标系、机器人坐标系、齐次变换矩阵、四元数与旋转矩阵

做SLAM这么多年，我见过太多新手在坐标系上栽跟头。说句实话，坐标系搞不清楚，后面所有定位算法都是空中楼阁。今天咱们就把这块硬骨头啃下来。

2.1 世界坐标系与机器人坐标系

先问个问题：你站在房间里，怎么描述你手机的位置？

你可能会说「手机在桌上，离我右手边半米」。这个描述里其实藏着两个坐标系：一个是房间的固定坐标系（世界坐标系），一个是以你身体为原点的坐标系（机器人坐标系）。

世界坐标系，说白了就是整个场景的绝对参考系。在SLAM里，我们通常把第一帧相机的位置设为世界坐标系原点。这个坐标系一旦定下来，就不动了。

机器人坐标系，是跟着机器人走的。我习惯叫它「body frame」，原点在机器人中心，x轴朝前，y轴朝左，z轴朝上。嗯，这里要注意，不同厂家的定义可能不一样，做融合的时候一定要看清楚。

我在项目中遇到过一件事：有个同事把激光雷达的数据直接当世界坐标用，结果建出来的地图全是歪的。排查了半天，才发现是忘了做坐标系变换。从那以后，我每次拿到传感器数据，第一件事就是确认它是在哪个坐标系下表达的。

2.2 齐次变换矩阵

齐次变换矩阵，说白了就是用一个4x4的矩阵，同时搞定旋转和平移。你想想看，如果分开做旋转和平移，公式写起来多麻烦？

它的形式长这样：

T = [ R  t ]
    [ 0  1 ]

其中R是3x3的旋转矩阵，t是3x1的平移向量。最后一行是[0,0,0,1]，这是齐次坐标的固定格式。

为什么用齐次坐标？因为可以把连续的变换写成矩阵乘法。比如从机器人坐标系到世界坐标系的变换：

p_world = T * p_robot

如果你有多个变换，直接乘起来就行：

T_world_to_camera = T_world_to_robot * T_robot_to_camera

2.3 旋转矩阵

旋转矩阵是3x3的正交矩阵，行列式为+1。它描述了一个坐标系相对于另一个坐标系的朝向。

举个例子，绕z轴旋转θ角：

R_z(θ) = [ cosθ  -sinθ  0 ]
         [ sinθ   cosθ  0 ]
         [ 0      0     1 ]

绕x轴和y轴的类似，我就不写了。你记住一个规律：绕哪个轴转，哪个轴对应的行和列就不变。

2.4 四元数

四元数，很多人一听就头大。其实它没那么可怕。说白了，四元数就是用四个数来表示旋转：一个实部加三个虚部。

q = w + xi + yj + zk

其中w是实部，x,y,z是虚部。约束条件是w² + x² + y² + z² = 1。

为什么不用旋转矩阵？因为旋转矩阵有9个参数，但只有3个自由度，存在冗余。做优化的时候，这9个参数还要满足正交约束，很麻烦。四元数只有4个参数，一个归一化约束，用起来方便多了。

我个人的经验：在代码里，我几乎只用四元数做旋转的插值和优化。只有在最后可视化的时候，才转成旋转矩阵给OpenGL用。

2.5 四元数与旋转矩阵的互转

这个转换公式，我建议你直接背下来，或者存成代码片段。每次手推太浪费时间了。

四元数转旋转矩阵：

R = [ 1-2(y²+z²)   2(xy-wz)   2(xz+wy) ]
    [ 2(xy+wz)     1-2(x²+z²) 2(yz-wx) ]
    [ 2(xz-wy)     2(yz+wx)   1-2(x²+y²) ]

旋转矩阵转四元数：

w = sqrt(1 + R[0][0] + R[1][1] + R[2][2]) / 2
x = (R[2][1] - R[1][2]) / (4 * w)
y = (R[0][2] - R[2][0]) / (4 * w)
z = (R[1][0] - R[0][1]) / (4 * w)

2.6 本章知识体系

下面这张图，是我自己总结的坐标系与变换的核心逻辑。你看一遍，应该能理清思路。

2.7 实战建议

说了这么多理论，最后给几条实在的建议：

• 统一坐标系定义：项目一开始，就把所有坐标系的定义写清楚。x轴朝哪，y轴朝哪，z轴朝哪。别等到代码写了一半才发现定义不一致。
• 用库，别手写：Eigen、Sophus、Ceres这些库都封装好了变换操作。我见过有人手写四元数乘法，结果符号搞反了，调了三天bug。何必呢？
• 可视化验证：每次做完坐标系变换，都可视化出来看一眼。RViz或者Matlab都行。眼睛一看，就知道对不对。
• 单元测试：写一个简单的测试用例，比如把点从机器人坐标系变换到世界坐标系，再变换回来，看是不是回到原点。这个测试能帮你发现90%的坐标系错误。