3. 刚体变换与齐次矩阵：旋转矩阵、平移向量、齐次坐标、4x4齐次变换矩阵的数学原理与组合运算

在机器人手眼标定与点云配准中，核心任务是将一个坐标系下的点或刚体，精确地映射到另一个坐标系下。这种映射在三维空间中被称为刚体变换，它由旋转和平移两部分组成。本章将系统讲解其数学表达——从旋转矩阵、平移向量，到齐次坐标与4x4齐次变换矩阵，并深入探讨其组合运算规则。

3.1 旋转矩阵：三维空间中的姿态描述

旋转矩阵是一个3x3的正交矩阵（R^TR = I，且行列式为+1），用于描述一个坐标系相对于另一个坐标系的纯旋转关系。其每一列代表原坐标系基向量在目标坐标系中的投影。

绕三个坐标轴的基本旋转矩阵如下：

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

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

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

3.2 平移向量：三维空间中的位置偏移

平移向量是一个3x1的列向量 t = [t_x, t_y, t_z]^T，表示从一个坐标系原点到另一个坐标系原点的位移。它只改变点的位置，不改变点的方向。

对于一个三维点 p = [x, y, z]^T，平移变换后的新坐标为：

p' = p + t = [x + t_x, y + t_y, z + t_z]^T

3.3 齐次坐标：统一旋转与平移的数学工具

为了将旋转和平移统一为一种线性运算（矩阵乘法），我们引入齐次坐标。齐次坐标将一个三维点 [x, y, z]^T 表示为四维向量 [x, y, z, 1]^T。

使用齐次坐标的核心优势在于：

• 统一运算： 旋转和平移可以合并为一个4x4矩阵，通过一次乘法完成变换。
• 链式组合： 多个变换矩阵可以预先相乘，形成复合变换矩阵，避免逐点重复计算。
• 表示无穷远点： 当 w=0 时，齐次坐标表示方向向量，这在处理相机投影时非常有用。

3.4 4x4齐次变换矩阵：刚体变换的完整表达

一个完整的刚体变换（旋转 + 平移）可以用一个4x4齐次变换矩阵 T 表示：

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

其中：
R: 3x3 旋转矩阵
t: 3x1 平移向量
0: 1x3 零向量
1: 标量 1

展开形式为：

T = [[r11, r12, r13, t_x],
     [r21, r22, r23, t_y],
     [r31, r32, r33, t_z],
     [0,   0,   0,   1  ]]

对点 p_h = [x, y, z, 1]^T 应用变换：

p'_h = T * p_h

结果 p'_h 的前三个分量即为变换后的三维坐标。

T_复合 = T_2 * T_1

p' = T_复合 * p = T_2 * (T_1 * p)

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