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

各位同学，今天我们来聊聊目标跟踪里最基础、也最绕不开的一个话题——刚体变换。

说白了，刚体变换就是「物体在空间里动了一下，但形状没变」。你想想看，一辆车从路口开到下一个路口，它的大小、轮子之间的距离、车身的轮廓，这些都没变吧？变了的那叫「变形」，不是刚体。在目标跟踪里，我们处理的绝大多数目标——行人、车辆、无人机——都可以近似看作刚体。

我个人习惯把刚体变换拆成两个动作：旋转和平移。先转一下，再挪个位置，完事。就这么简单。

旋转矩阵：方向怎么描述？

先聊旋转。假设一个点在三维空间里，坐标是 (x, y, z)。它绕某个轴转了个角度，新坐标怎么算？

旋转矩阵就是干这个的。它是一个 3×3 的矩阵，每一列代表旋转后的新坐标轴在原坐标系下的投影。

举个例子，绕 Z 轴旋转 θ 角度的旋转矩阵长这样：

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

绕 X 轴和 Y 轴的类似，我就不全写了。你记住一个规律：绕哪个轴转，哪个轴对应的行和列就不动，其他四个元素是 cos 和 sin 的组合。

旋转矩阵有几个重要性质：

• 它是正交矩阵，即 R^T = R^-1
• 行列式为 +1（纯旋转，不含反射）
• 多个旋转可以复合，顺序很重要——矩阵乘法不交换

我在项目中遇到过一个问题：两个旋转矩阵相乘，顺序搞反了，结果目标在画面里直接「瞬移」到了另一边。排查了半天才发现是 R1 * R2 写成了 R2 * R1。嗯，这种坑踩过一次就记住了。

平移向量：位置怎么挪？

旋转搞定了，接下来是平移。平移更简单，就是一个三维向量 t = [t_x, t_y, t_z]^T，表示在三个方向上分别移动多少。

完整的刚体变换公式就是：

p' = R * p + t

其中 p 是原始坐标，p' 是变换后的坐标。

你可能会问：为什么旋转用矩阵，平移用向量？能不能统一成一个东西？

能。这就是接下来要说的齐次坐标。

齐次坐标：把旋转和平移塞进一个矩阵

齐次坐标的核心思想很简单：给三维坐标多加一个维度，变成四维。原来 (x, y, z) 变成 (x, y, z, 1)。

这样一来，旋转和平移就可以合并成一个 4×4 的变换矩阵：

T = [
    [R, t],
    [0, 1]
]

具体写出来就是：

T = [
    [r11, r12, r13, tx],
    [r21, r22, r23, ty],
    [r31, r32, r33, tz],
    [0,   0,   0,   1 ]
]

变换操作就变成了一个矩阵乘法：

p'_hom = T * p_hom

为什么要这么折腾？有两个好处：

• 统一形式：所有变换都是矩阵乘法，代码里可以统一处理
• 方便复合：多个变换可以预先乘成一个矩阵，减少计算量

欧氏变换：刚体变换的正式名称

旋转 + 平移，合起来就叫欧氏变换。它保持两点之间的距离不变，也保持角度不变。说白了，就是物体在空间里「刚性地」移动。

欧氏变换的集合构成一个群，叫 SE(3)（Special Euclidean Group）。旋转部分构成 SO(3)（Special Orthogonal Group）。这些数学概念你了解一下就行，实际写代码时，我们更关心怎么用。

在目标跟踪中，欧氏变换的应用场景太多了：

• 相机坐标系与世界坐标系之间的转换
• 相邻两帧之间目标的位姿变化
• 多传感器融合时的坐标对齐

我举个例子。在视觉SLAM里，我们经常需要把相机坐标系下的点转换到世界坐标系。假设相机位姿是 T_cw（从世界到相机），那么一个世界点 P_w 在相机坐标系下的坐标就是：

P_c = T_cw * P_w

反过来，如果你知道相机坐标系下的点，想求它在世界坐标系下的位置，就用逆变换：

P_w = T_cw^(-1) * P_c

因为 T 是欧氏变换矩阵，它的逆很好求——旋转部分取转置，平移部分取负再乘转置：

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

这个公式我建议你记下来。写代码时经常用到，手写推导也快。

本章知识体系

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

从图中你可以看到，旋转矩阵和平移向量是刚体变换的两大基石，齐次坐标把它们统一成 4×4 的欧氏变换矩阵，最后应用到各种实际场景中。

好了，这一章的内容就到这里。刚体变换是目标跟踪的数学基础，后面的章节会反复用到。你先把旋转矩阵、平移向量、齐次坐标这几个概念理清楚，代码写起来就顺了。