CE7453 Numerical Algorithms 期末高分超详细攻略（第八章：3D Data Registration）

本章内容：3D数据配准、ICP算法、最优旋转/平移、SVD分解、实际应用
适用对象：零基础/考前冲刺/快速查漏补缺
内容特色：详细原理、公式推导、算法流程、例题全解、英文关键词

1. 基本概念与背景

• 3D数据配准（3D Data Registration）：将多个3D点云或模型对齐到同一坐标系，常用于3D重建、医学成像、机器人导航等。
• 典型问题：给定两组点云，求最优刚性变换（旋转+平移）使它们尽量重合。

2. ICP算法（Iterative Closest Point）

2.1 原理

• 迭代式地将源点云与目标点云配准
• 每次迭代包括“最近点匹配”和“最优变换估计”两步

2.2 算法详细流程

1. 初始化：设源点集 $A$，目标点集 $B$，初始变换 $T_0$
2. 最近点匹配：对 $A$ 中每个点 $a_i$，在 $B$ 中找最近点 $b_j$
3. 估计最优变换：求刚性变换 $T$（旋转$R$+平移$t$），最小化配对点的均方误差 $$\underset{R,t}{\min }\sum _i\Vert R{a}_i+t-b_i{\Vert }^2$$
4. 应用变换：用 $T$ 更新 $A$
5. 收敛判定：若误差变化小于阈值，停止；否则回到第2步

2.3 最优旋转/平移的SVD解法

• 设 $A$ 和 $B$ 已配对，先去中心化
• 计算协方差矩阵 $H={\sum }_i(a_i-\bar{a})(b_i-\bar{b}{)}^T$
• 对 $H$ 做SVD分解 $H=U\Sigma V^T$
• 最优旋转 $R=V{U}^T$，最优平移 $t=\bar{b}-R\bar{a}$

3. 典型例题

例题1：已知 $A=\{(0,0),(1,0)\}$，$B=\{(1,1),(2,1)\}$，求将 $A$ 配准到 $B$ 的最优刚性变换

解答：

1. 计算质心：
   • $A$ 的质心：$\bar{a}=\left(\frac{0+1}{2},\frac{0+0}{2}\right)=(0.5,0)$
   • $B$ 的质心：$\bar{b}=\left(\frac{1+2}{2},\frac{1+1}{2}\right)=(1.5,1)$
2. 去中心化：
   • $A$ 去中心化后：$\{(0-0.5,0-0),(1-0.5,0-0)\}=\{(-0.5,0),(0.5,0)\}$
   • $B$ 去中心化后：$\{(1-1.5,1-1),(2-1.5,1-1)\}=\{(-0.5,0),(0.5,0)\}$
3. 计算协方差矩阵 $H$：
   • $H={\sum }_i(a_i-\bar{a})(b_i-\bar{b}{)}^T$
   • $H=(-0.5)\cdot (-0.5)+(0.5)\cdot (0.5)=0.25+0.25=0.5$ （对每个维度分别计算）
   • 完整矩阵 $H=\left[\begin{array}{cc}0.5& 0\\ 0& 0\end{array}\right]$
4. SVD分解：
   • $H=U\Sigma V^T$，得到 $U=I$，$V=I$（因为 $H$ 是对角矩阵）
   • 旋转矩阵 $R=V{U}^T=I$（单位矩阵）
5. 计算平移向量：
   • $t=\bar{b}-R\bar{a}=(1.5,1)-(0.5,0)=(1,1)$
6. 结果：
   • 最优旋转矩阵 $R$ 为单位矩阵（无旋转）
   • 最优平移向量 $t=(1,1)$

验证：将 $A$ 的点应用变换后，$(0,0)\to (1,1)$，$(1,0)\to (2,1)$，与 $B$ 完全重合。

例题2：给定点集 $A=[(0,0,0),(1,0,0)]$ 和 $B=[(0,1,0),(1,1,0)]$，求将 $A$ 配准到 $B$ 的最优刚性变换

解答步骤：

1. 计算质心：
   • $A$ 的质心：$\bar{a}=\left(\frac{0+1}{2},\frac{0+0}{2},\frac{0+0}{2}\right)=(0.5,0,0)$
   • $B$ 的质心：$\bar{b}=\left(\frac{0+1}{2},\frac{1+1}{2},\frac{0+0}{2}\right)=(0.5,1,0)$
2. 去中心化：
   • $A$ 去中心化后：$\{(0-0.5,0-0,0-0),(1-0.5,0-0,0-0)\}=\{(-0.5,0,0),(0.5,0,0)\}$
   • $B$ 去中心化后：$\{(0-0.5,1-1,0-0),(1-0.5,1-1,0-0)\}=\{(-0.5,0,0),(0.5,0,0)\}$
3. 计算协方差矩阵 $H$：
   • $H={\sum }_i(a_i-\bar{a})(b_i-\bar{b}{)}^T$
   • 对于每个点对，计算外积并求和
   • $H=\left[\begin{array}{ccc}0.5& 0& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$
4. SVD分解：
   • $H=U\Sigma V^T$，得到 $U=I$，$V=I$
   • 旋转矩阵 $R=V{U}^T=I$（单位矩阵）
5. 计算平移向量：
   • $t=\bar{b}-R\bar{a}=(0.5,1,0)-(0.5,0,0)=(0,1,0)$
6. 结果：
   • 最优旋转矩阵 $R$ 为单位矩阵（无旋转）
   • 最优平移向量 $t=(0,1,0)$

验证：将 $A$ 的点应用变换后，$(0,0,0)\to (0,1,0)$，$(1,0,0)\to (1,1,0)$，与 $B$ 完全重合。

例题3（进阶）：给定点集 $A=[(0,0,0),(1,0,0),(0,1,0)]$ 和 $B=[(0,0,0),(0,1,0),(1,0,0)]$，求将 $A$ 配准到 $B$ 的最优刚性变换（涉及旋转）。

解答步骤：

1. 计算质心：
   • $A$ 的质心：$\bar{a}=\left(\frac{0+1+0}{3},\frac{0+0+1}{3},\frac{0+0+0}{3}\right)=\left(\frac{1}{3},\frac{1}{3},0\right)$
   • $B$ 的质心：$\bar{b}=\left(\frac{0+0+1}{3},\frac{0+1+0}{3},\frac{0+0+0}{3}\right)=\left(\frac{1}{3},\frac{1}{3},0\right)$
2. 去中心化：
   • $A$ 去中心化后：$\left\{\left(-\frac{1}{3},-\frac{1}{3},0\right),\left(\frac{2}{3},-\frac{1}{3},0\right),\left(-\frac{1}{3},\frac{2}{3},0\right)\right\}$
   • $B$ 去中心化后：$\left\{\left(-\frac{1}{3},-\frac{1}{3},0\right),\left(-\frac{1}{3},\frac{2}{3},0\right),\left(\frac{2}{3},-\frac{1}{3},0\right)\right\}$
3. 计算协方差矩阵 $H$：
   • $H={\sum }_i(a_i-\bar{a})(b_i-\bar{b}{)}^T$
   • 对应点对：$A_1\leftrightarrow B_1$，$A_2\leftrightarrow B_3$，$A_3\leftrightarrow B_2$
   • 计算每个点对的外积并求和，得到 $H=\left[\begin{array}{ccc}\frac{2}{9}& -\frac{2}{9}& 0\\ -\frac{2}{9}& \frac{2}{9}& 0\\ 0& 0& 0\end{array}\right]$
4. SVD分解：
   • 对 $H$ 进行 SVD 分解，得到 $U$ 和 $V$
   • $H$ 的特征值和特征向量计算后，$R=V{U}^T=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$（绕 z 轴旋转 90 度）
5. 计算平移向量：
   • $t=\bar{b}-R\bar{a}$
   • $R\bar{a}=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\frac{1}{3}\\ \frac{1}{3}\\ 0\end{array}\right]=\left[\begin{array}{c}-\frac{1}{3}\\ \frac{1}{3}\\ 0\end{array}\right]$
   • $t=\left(\frac{1}{3},\frac{1}{3},0\right)-\left(-\frac{1}{3},\frac{1}{3},0\right)=\left(\frac{2}{3},0,0\right)$
6. 结果：
   • 最优旋转矩阵 $R=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right]$
   • 最优平移向量 $t=\left(\frac{2}{3},0,0\right)$

验证：应用变换后，$A$ 的点将旋转并平移到与 $B$ 接近重合的位置（由于点对匹配，此处为近似解）。

4. 常见考点与易错点

• 最近点匹配的高效实现（如k-d树）
• SVD分解步骤与旋转矩阵构造
• ICP收敛性与初始位置敏感性
• 多组点云全局配准与误差累积
• 英文术语拼写与公式记忆

5. 英文关键词与表达

• 3D data registration, point cloud, rigid transformation, rotation, translation, iterative closest point (ICP), singular value decomposition (SVD), covariance matrix, alignment, convergence

如需更详细例题推导或某一算法的代码实现，可随时补充！