第3讲三维空间刚体运动

3.1 理论基础

刚体运动由旋转（Rotation）和平移（Translation）组成。

向量：空间中的实体，其坐标值取决于选取的坐标系（基底）。
坐标系：通常使用右手系（Right-hand system）。
外积与反对称矩阵：
- 在三维空间中，向量的外积 a × b 可以写成矩阵与向量的乘法 a^∧b。
- 符号：∧ (hat) 操作符将向量映射为反对称矩阵。
- 公式：
  - 设向量 a = [a₁, a₂, a₃]^T，则：

定义：描述两个坐标系基底之间的变换关系。
数学性质：
- 属于特殊正交群 SO(n)
- 对于三维空间 SO(3) = {R ∈ ℝ^3 × 3|RR^T = I, det (R) = 1}
- R 是正交矩阵： R⁻¹ = R^T RR^T = I det (R) = 1
缺点：
- 冗余性：用 9 个量描述 3 个自由度的旋转。
- 约束性：自身带有约束（正交且行列式为1），优化求解困难。

定义：用一个旋转轴 n 和旋转角 θ 来描述旋转。
表示：向量 w = θn（方向为轴，模长为角）。
旋转向量 → 旋转矩阵 (罗德里格斯公式) R = cos θI + (1 − cos θ)nn^T + sin θn^∧
旋转矩阵 → 旋转向量
- 求旋转角 θ：利用矩阵的迹（Trace，对角线元素之和）。 tr(R) = 1 + 2cos θ
- 求旋转轴 n： n 是矩阵 R 特征值 1 对应的特征向量，即满足： Rn = n
优点：紧凑（3个量），无约束。
缺点：具有奇异性（周期性问题）。

定义：将旋转分解为三次绕不同轴的旋转。
- Z - Yaw - 偏航
- Y - Pitch - 俯仰
- X - Roll - 滚转
优点：直观，易于人机交互理解。
缺点：万向锁 (Gimbal Lock)。
- 当俯仰角（Pitch）为 ±90^∘ 时，第三次旋转轴与第一次重合，丢失一个自由度。
- 不适合用于插值、迭代或 SLAM 的核心计算，仅用于显示。

定义：一种扩展的复数，q = q₀ + q₁i + q₂j + q₃k 。
向量形式记为 q = [s, v]，其中 s = q₀ 为实部，v = [q₁, q₂, q₃]^T 为虚部。
单位四元数：模长为 1 的四元数可表示三维旋转。
运算：
- 乘法：设 q_a = [s_a, v_a], q_b = [s_b, v_b] q_aq_b = [s_as_b − v_a^Tv_b, s_av_b + s_bv_a + v_a × v_b] 注意：四元数乘法不可交换。
- 共轭： q^* = [s, −v]
- 模长： ∥q∥² = s² + x² + y² + z²
- 逆： 若为单位四元数（描述旋转），∥q∥ = 1，则 q⁻¹ = q^*。
优点：紧凑（4个量），无奇异性（Gimbal Lock），计算效率高。
注意：q 和 −q 表示同一个旋转。
用四元数旋转一个点
- 设空间点 p = [x, y, z]^T，旋转由单位四元数 q 描述。
- 将点 p 扩展为虚四元数：p_quat = [0, x, y, z]。
- 旋转公式： p_quat^′ = qp_quatq⁻¹ (若 q 为单位四元数，即 qp_quatq^*)
- 取出 p_quat^′ 的虚部即为旋转后的坐标。
旋转向量 → 四元数
- 设旋转轴 n，角度 θ：
四元数 → 旋转矩阵
- 设 q = [q₀, q₁, q₂, q₃] (归一化后)，对应的 R 为：

欧氏变换：a^′ = Ra + t。
- 这不是线性变换，多次变换累加形式复杂。
齐次坐标：在三维向量末尾加 1，变为四维向量 [x, y, z, 1]^T。
变换矩阵 T：
数学性质：属于特殊欧氏群 SE(3)。
坐标变换
- 设点 P 在坐标系 2 的坐标为 p₂，变换到坐标系 1 的坐标 p₁：即非齐次形式：p₁ = R₁₂p₂ + t₁₂。
变换矩阵的逆
- 给定，其逆矩阵 T⁻¹ 为：
- 推导思路：若 a = Rb + t，反求 b： Rb = a − t 左右同乘 R^T (即 R⁻¹)： b = R^Ta − R^Tt 这就对应了 T⁻¹ 的结构。

三维空间中的变换除了刚体运动（欧氏变换）外，还有更一般的形式。它们的关系是包含关系： 欧氏 ⊂ 相似 ⊂ 仿射 ⊂ 射影。

以下均使用齐次坐标表示，设变换前的点为，变换后的点为。

在欧氏变换的基础上，允许物体进行均匀的缩放。

直观理解：刚体运动 + 均匀缩放（保持形状不变，只改变大小和位置）。
矩阵形式：
- s ∈ ℝ：缩放因子 (Scalar)。
- R ∈ SO(3)：旋转矩阵。
- t ∈ ℝ³：平移向量。
自由度 (DoF)：7
- 3 (旋转) + 3 (平移) + 1 (缩放)。
不变性 (Invariants)：
- 保角性：两条直线的夹角不变。
- 平行性：平行线变换后仍平行。
- 距离比：两段线段长度的比值不变（虽然绝对长度变了）。
应用：单目 SLAM 的尺度漂移（Sim3优化）。

仿射变换是线性变换（旋转、缩放、切变）加上平移的组合。

直观理解：把正方体压扁成平行六面体（立体的平行四边形）。它可以拉伸、剪切（shear），但不产生透视效果。
矩阵形式：
- A ∈ ℝ^3 × 3：只要是可逆矩阵即可，不必是正交矩阵。
- t ∈ ℝ³：平移。
自由度 (DoF)：12
- 9 (矩阵 A 的元素) + 3 (平移 t)。
不变性 (Invariants)：
- 平行性：平行线变换后依然平行（这是与射影变换最大的区别）。
- 体积比：物体体积的比值不变。
- 共线性：直线变换后还是直线。
注意：正交投影（Orthographic Projection）属于一种特殊的仿射变换（丢弃了Z轴信息）。

最一般的线性变换，也称为单应性变换 (Homography)。

直观理解：真实世界（3D）投影到照片（2D）的过程就是射影变换的一种降维形式。在3D-3D变换中，想象光线从一点发散投影，正方体可能变成不规则的四棱台（Frustum）。
矩阵形式：
- A ∈ ℝ^3 × 3。
- t ∈ ℝ³。
- a^T ∈ ℝ³：导致透视失真（近大远小）的关键部分。
- v ∈ ℝ。
自由度 (DoF)：15
- 矩阵原本有 4 × 4 = 16 个参数。
- 但是在齐次坐标下，T 和 kT (k ≠ 0) 表示同一个变换。我们需要扣除一个缩放因子的自由度，通常令右下角 v = 1 或矩阵模长为1，所以是 16 − 1 = 15。
不变性 (Invariants)：
- 共线性：直线变换后还是直线（但平行线可能会相交于“消隐点”）。
- 交比 (Cross-ratio)：直线上四个点的交比保持不变。
- 不保持：不保平行（两条平行线在照片里会相交），不保角度，不保距离比。

变换名称	自由度 (3D)	不变性质 (Invariant Properties)	几何形变举例
欧氏变换	6	长度、夹角、体积、平行性	刚体移动/旋转
相似变换	7	夹角、平行性、长度比	均匀放大/缩小
仿射变换	12	平行性、体积比	正方形 → 平行四边形
射影变换	15	共线性、交比	正方形 → 不规则四边形