2. 刚体变换：旋转矩阵、平移向量、齐次坐标与齐次变换矩阵

刚体变换，说白了就是物体在三维空间里「挪个位置、转个方向」，但形状和大小一点不变。你想想看，你手里拿个杯子，从桌面左边放到右边，再转个角度——杯子的长宽高没变，只是位置和朝向变了。这就是刚体变换。

在3D视觉里，我们天天跟它打交道。相机在动、物体在动、坐标系在变，怎么描述这些运动？核心就四个东西：旋转矩阵、平移向量、齐次坐标、齐次变换矩阵。

2.1 旋转矩阵：描述朝向的变化

先聊旋转矩阵。我个人习惯把它理解成「坐标系的三个轴在新坐标系下的投影」。假设世界坐标系是固定的，相机坐标系旋转了一个角度，那相机坐标系里的一个点，怎么换算到世界坐标系？

旋转矩阵 R 是一个 3×3 的正交矩阵，满足 RTR = I，行列式为 +1。它的每一列，其实就是原坐标系的基向量在目标坐标系下的坐标。

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

我在项目里遇到过一个问题：用欧拉角做旋转，结果出现了万向锁，姿态突然跳变。后来我全部改用旋转矩阵或四元数，再也没出过这种幺蛾子。所以我的建议是——能用矩阵就别用欧拉角，除非你只在俯仰角很小的场景下用。

旋转方式	优点	缺点
旋转矩阵	无奇异性，可组合	9个参数，冗余
欧拉角	直观，3个参数	万向锁，插值困难
四元数	无奇异性，易插值	不够直观

2.2 平移向量：描述位置的变化

旋转搞定了朝向，平移就简单了——就是坐标的偏移。平移向量 t 是一个 3×1 的向量，表示从原坐标系原点到目标坐标系原点的位移。

举个例子：相机在世界坐标系中的位置是 (1, 2, 3)，那平移向量就是 t = [1, 2, 3]T。一个点从相机坐标系转到世界坐标系，就是先旋转再平移：

P_w = R * P_c + t

这里有个坑，我曾经踩过：旋转和平移的顺序不能搞反。先平移再旋转，跟先旋转再平移，结果完全不同。刚体变换的标准顺序永远是「先旋转，后平移」。

2.3 齐次坐标：为什么要多一个维度？

你可能会问：旋转用矩阵乘，平移用向量加，两个操作不统一，能不能合并成一个？

能。答案就是齐次坐标。

齐次坐标把一个三维点 (x, y, z) 表示成四维向量 (x, y, z, 1)。多出来的那个 1，就是齐次因子。为什么是 1？因为这样平移就能写成矩阵乘法了。

// 三维点 (x, y, z) 的齐次坐标
P_h = [x, y, z, 1]^T

// 齐次坐标转回三维坐标：除以最后一个分量
P = [x/w, y/w, z/w]^T

齐次坐标还有一个好处：它可以表示无穷远点。当 w = 0 时，表示一个方向向量，而不是位置点。这在处理平行线、透视投影时特别有用。

2.4 齐次变换矩阵：刚体变换的终极形态

把旋转矩阵和平移向量塞进一个 4×4 矩阵，就是齐次变换矩阵 T：

T = [
  [R(3×3),  t(3×1)],
  [0 0 0,    1    ]
]

// 用齐次变换矩阵做变换
P_w_h = T * P_c_h

这个矩阵把旋转、平移、甚至缩放（虽然刚体变换不允许缩放）统一成一个矩阵乘法。连续多个变换，只需要把对应的 T 矩阵乘起来：

T_total = T_1 * T_2 * T_3 * ...

嗯，这里要注意：矩阵乘法不满足交换律，所以变换的顺序极其重要。我见过一个同事，把两个变换矩阵的顺序写反了，结果机械臂的末端位姿差了十万八千里，找了三天 bug。

2.5 知识结构图

下面这张图展示了刚体变换的核心逻辑：

_w = T · P_c 一个矩阵搞定刚体变换

2.6 实战中的常见问题

最后聊几个我实际项目中踩过的坑：

• 坐标系定义混乱：不同传感器用不同的坐标系（相机坐标系、IMU坐标系、世界坐标系），一定要先统一再计算。我曾经因为搞混了相机坐标系和图像坐标系，标定结果全错。
• 矩阵乘法顺序：T1 * T2 表示先做 T2 变换，再做 T1 变换。很多人写成 T2 * T1，结果完全反了。
• 齐次坐标忘记归一化：从齐次坐标转回三维坐标时，一定要除以 w 分量。我见过有人直接取前三个分量，结果坐标全偏了。
• 平移向量方向搞反：从 A 到 B 的变换，平移向量是 B 原点在 A 坐标系下的坐标。反过来用就错了。