3、刚体变换与齐次坐标：旋转矩阵、平移向量、齐次坐标矩阵、刚体变换的数学表达

好，咱们进入第三章。说实话，这一章是整个手眼标定的数学地基。你后面能不能把标定做明白，很大程度上取决于你对刚体变换和齐次坐标的理解深度。

我见过不少工程师，拿着 HALCON 的算子一顿调，标定结果出来看着挺像回事，但一换场景就崩。为什么？说白了，就是没搞懂坐标系之间到底是怎么转的、怎么移的。今天咱们就把这个根儿刨清楚。

3.1 什么是刚体变换？

先问个问题：你手里拿个手机，从桌上拿到眼前，手机本身变形了吗？没有。它还是那个形状、那个大小。这种只改变位置和姿态、不改变物体形状和尺寸的变换，就叫刚体变换。

在机器视觉里，刚体变换就是描述一个坐标系相对于另一个坐标系的位置和朝向。它由两部分组成：

• 旋转 —— 描述朝向变化
• 平移 —— 描述位置变化

你可以这么理解：旋转是“怎么转过去的”，平移是“走到哪去了”。

3.2 旋转矩阵 —— 朝向的数学表达

旋转矩阵是个 3×3 的方阵。它有个很重要的性质：正交矩阵。什么意思？就是它的逆等于它的转置。这个性质在实际计算中非常有用，后面讲 HALCON 算子时会用到。

咱们先看绕三个坐标轴的基本旋转矩阵：

旋转轴	旋转矩阵（角度 θ）	说明
绕 X 轴	[1, 0, 0; 0, cosθ, -sinθ; 0, sinθ, cosθ]	X 坐标不变
绕 Y 轴	[cosθ, 0, sinθ; 0, 1, 0; -sinθ, 0, cosθ]	Y 坐标不变
绕 Z 轴	[cosθ, -sinθ, 0; sinθ, cosθ, 0; 0, 0, 1]	Z 坐标不变

3.3 平移向量 —— 位置的偏移

平移向量就是个 3×1 的列向量，表示坐标系原点之间的偏移量。

比如：

t = [tx, ty, tz]^T

表示新坐标系的原点相对于原坐标系，在 X 方向偏移了 tx，Y 方向偏移了 ty，Z 方向偏移了 tz。

这里有个容易踩的坑：平移向量是在哪个坐标系下表达的？

答案是：在原坐标系下表达的。也就是说，你站在原坐标系里，看新坐标系的原点在哪。

3.4 齐次坐标矩阵 —— 把旋转和平移统一起来

你想想看，如果每次变换都要先做矩阵乘法再做向量加法，写代码多麻烦？而且多个变换串联时，公式会变得非常冗长。

齐次坐标就是来解决这个问题的。它把 3D 点扩展成 4D 向量：

P_hom = [x, y, z, 1]^T

然后，旋转和平移就可以合并成一个 4×4 的齐次变换矩阵：

H = [R, t;
     0, 1]

其中 R 是 3×3 旋转矩阵，t 是 3×1 平移向量，最后一行是 [0, 0, 0, 1]。

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

P'_hom = H · P_hom

3.5 刚体变换的数学表达 —— 从公式到代码

咱们用个具体例子串一下。假设有个点 P = [1, 2, 3]^T，先绕 Z 轴转 30°，再沿 X 轴平移 5 个单位。

第一步：构造旋转矩阵

Rz(30°) = [cos30, -sin30, 0;
           sin30,  cos30, 0;
           0,      0,     1]

        = [0.866, -0.5, 0;
           0.5,   0.866, 0;
           0,     0,     1]

第二步：构造齐次变换矩阵

H = [0.866, -0.5, 0, 5;
     0.5,   0.866, 0, 0;
     0,     0,     1, 0;
     0,     0,     0, 1]

第三步：变换点

P'_hom = H · [1, 2, 3, 1]^T
       = [0.866*1 + (-0.5)*2 + 0*3 + 5*1,
          0.5*1 + 0.866*2 + 0*3 + 0*1,
          0*1 + 0*2 + 1*3 + 0*1,
          1]
       = [5.866, 2.232, 3, 1]^T

所以变换后的 3D 坐标是 [5.866, 2.232, 3]^T。

3.6 知识结构图

下面这张图帮你理清本章的知识脉络：

3.7 本章小结

刚体变换是手眼标定的数学基础。你需要记住几个关键点：

• 旋转矩阵是正交矩阵，描述朝向
• 平移向量描述位置偏移，注意方向
• 齐次坐标矩阵把旋转和平移统一成 4×4 矩阵
• 变换顺序很重要：先旋转后平移，矩阵乘法从右到左