标题：A Method for Registration of 3-D Shapes
作者： J. Besl, Neil D. McKay
目录：

• Ⅰ Introduction
• Ⅱ Literature Review
• Ⅲ Mathematical Preliminaries
  • A. Point to Parametric Entity Distance
  • B. Point to Implicit Entity Distance
  • C. Corresponding Point Set Registration
• Ⅳ The Iterative Closest Point Algorithm
  • A. ICP Algorithm Statement
  • B. Convergence Theorem
  • C. An Accelerated ICP Algorithm
  • D. Alternative Minimization Approaches
• Ⅴ The Set of Initial Registrations
  • A. Initial States for Global Matching
  • B. A Counter Example
  • C. Local Shape Matching
• Ⅵ. Experimental Results
  • A. Point Set Matching
  • B. Curve Matching
  • C. Surface Matching
    • 1 A Bezier Surface Patch
    • 2 The NRCC African Mask
    • 3 Terrain Data
• Ⅶ. Conclusions
• Ⅷ. Future Directions

文章目录

• 一种用于3D形状配准的方法
• • 1. 引言
  • 2. 文章预览
  • 3. 数学预备
  • • 3.1 点到参数化实体距离
    • 3.2 点到隐式实体的距离
    • 3.3 对应点集配准
  • 4. 迭代最近点算法
  • • 4.1 ICP算法描述
    • 4.2 收敛定理
    • 4.3 加速的ICP算法
    • 4.4 可选的最小化方法
  • 5. 初始配准集
  • • 5.1 全局匹配的初始状态
    • 5.2 反例
    • 5.3 局部形状匹配
  • 6. 实验结果
  • • 6.1 点集匹配
    • 6.2 曲线匹配
    • 6.3 表面匹配
    • • 6.3.1 贝塞尔曲面块
      • 6.3.2 NRCC 非洲人脸
      • 6.3.3 地形数据
  • 7. 结论
  • 8. 未来方向

一种用于3D形状配准的方法

摘要——本文描述了一种通用的、独立于表达的方法，用于准确且高效计算的 3D 形状（包括*曲线和曲面）配准。该方法基于迭代最近点（iterative closest point, ICP）算法处理整个6*度，ICP算法只需要一个找到几何实体上离给定点最近的点的过程。ICP算法总是单调地收敛到均方距离度量的最小值，且经验显示在前面的迭代中收敛速率很快。因此对于具有一定“形状复杂性”级别的特定类别的物体，给定足够的初始旋转和平移的集合，是可以通过测试每个初始配准来全局地在整个6*度上最小化均方距离度量。例如，一个给定形状的“模型”和一个表达模型形状主要部分的感知的“数据”形状，能够以允许给定的模型复杂度，通过测试一个初始平移和一组相对较小的旋转，在几分钟内配准起来。该方法的一个重要应用是将来自不固定刚性物体的感知数据与理想几何模型的先验进行配准，来做形状检查。我们描述的方法也能够被用来决定基本问题，如，不同几何表达的一致（形状等价）以及估计未知对应的点集间的运动。实验结果展示了配准算法在点集，曲线和曲面上的能力。

关键词——*曲线匹配，*曲面匹配，运动估计，位姿估计，四元数，3D配准

1. 引言

对*曲线，表面和点集的全局和局部形状匹配度量在【3】中有提到，它试图形式化和统一计算机视觉中关键问题的描述：给定一个在传感器坐标系下的3D数据，这描述了一个数据形状与模型形状的对应，并且给定一个以不同几何形状表达的模型坐标系中的模型形状，估计最优的旋转和平移，从而对齐，或者配准的模型形状与数据形状，这是通过最小化形状间的距离实现的，且因此能够通过均方误差度量来确定形状间的等价。许多应用的感兴趣的关键问题是：将来自距离图像的分段区域与在计算机辅助设计（computer-aided-design(CAD)）模型上的B样条表面进行匹配是否可行？本文提出了一个*表面匹配问题的解决方案，正如在【3】和【5】中定义的作为一个简单、通用且统一方法的特殊情况，它可以泛化到 n n n 维并为 1）未知对应的点集匹配问题；2）*曲线匹配问题；提供解决方案。算法不需要额外的特征，不需要曲线或表面的导数，且除了去除统计的异常值，不需要对3D数据进行预处理。

    我们提出的方法的主要应用是将来自未固定的刚性物体的数字化数据与理想的几何模型先验进行匹配，来做形状检查。在浅景深范围内使用高精度非接触式测量设备来检测形状时，不同感测点的不确定度变化不大。因此，为简单起见并与基于数千个数字化点的检测应用相关，不考虑点之间不同的不确定性的情况。类似地，统计离群值的去除被认为是一个预处理步骤，可能是最好的实现，也不会被处理。在检查应用的背景下，高精度非接触性测量设备不生成坏的数据的假设是合理的，因为一些传感器有能力拒绝高度不确定的测量值。

    我们提出的形状配准算法可以在以下的几何数据的表达中被使用：

1）点集
2）线段集（折线）；
3）隐式曲线：$\vec{g}(x, y, z) = 0$
4）参数化曲线：(x(u), y(u), z(u))
5）三角集合（刻面）
6）隐式表面：$g(x, y, z) = 0$
7）参数化表面：(x(u, v), y(u, v), z(u, v))

这覆盖了要使用3D形状配准方法的大部分应用。其他表达是通过提供评估给定形状上与给定数字化点最近的点的过程来处理的。

    本文结构如下：首先回顾一些文献中的相关论文。其次会提到的几何表示涵盖了计算形状上到给定点的最近点的数学预备知识。然后，描述了迭代最近点（ICP）算法，以及关于它单调收敛性的定理的证明。最后，基于点集，曲线，表面的实验结果展现了ICP配准算法的能力。

2. 文章预览

    在 3D *形状的配准（姿势估计、对齐、运动估计）领域,发表的工作相对较少。大部分解决全局形状匹配或配准的现有文献，已经解决了有限的形状类别，即，1）多面体模型，2)分段（超​​）二次模型【2】【27】，3）未知对应（关系）的点集。读者可以参考【6】和【14】了解1985年以前这些领域的工作。对于以下未讨论的其他近期相关工作的样本，见【8，10，12，13，19，20，24，26，34，35，37，39，44，46，48，53，58，59】。

    从历史上看，使用 3-D 数据的*形状匹配最早是由 Faugeras 和他在 INRIA 的团队完成的【18】，他们在 1980 年代初期展示了与雷诺汽车零件（转向节）的有效匹配。这项工作在计算机视觉社区中推广了使用四元数进行相应 3-D 点集的最小二乘配准。奇异值分解（SVD）算法的替代使用【23，1，49】在这个时间范围内并不广为人知。这项工作的主要限制是它依赖于在*形状中可能存在的相当大平面区域。

    Schwartz 和 Sharir【50】在1985年年底开发了一个不需要特征提取的*形状空间曲线匹配问题的解决方案。他们使用非四元数方法来计算最小二乘旋转矩阵。该方法在合理的质量曲线数据上工作很好，但在有很大噪声的曲线（数据）上有困难，因为该方法使用曲线的弧长采样来获得对应的点集。

    Haralick等【28】使用稳健方法结合最小二乘 SVD 配准方法解决了 3-D 点集姿态估计问题，这为最小二乘四元数或 SVD 点集匹配提供了稳健的统计替代方案。该算法能够处理统计异常值并且在理论上，只要正交矩阵的行列式严格为正数，就可以替换我们基于四元数的算法。最近的会议论文集【47】涵盖了关于该主题的新的贡献。

    Horn【31】衍生出了最小二乘四元数匹配的 Faugeras’s 方法【18】的可替代的公式，它使用 4 × 4 4 \times 4 4×4 矩阵最大特征值代替最小特征值。Horn【30】和 Brou【11】也开发出了扩展高斯图像（extended Gaussian image, EGI）的方法，该方法允许基于表面法线直方图的凸和受限非凸形形状集的匹配。

    Taubin【55】在隐式代数非平面3-D曲线和曲面估计（应用于无需特征提取的位置估计）领域做了一些有趣的工作。他描述了一个使用近似距离度量高达10度的使用隐式代数近似数据点的方法（he describes a method of approximating data points with implicit algebraic forms up to the tenth degree using an approximate distance metric）。全局形状（没被遮挡的形状）可以根据广义特征值进行识别，并且可以恢复配准变换。该方法被证明可用于完整的平面曲线和空间曲线形状，但尚不清楚其有效性能否很好地推广到更复杂的表面，例如地形数据或人脸。Taubin 曾表示近似距离拟合的数值方法倾向于在十度以上分解（tend to break down above the tenth degree）。他后来【56】通过研究基于广义形状多项式的形状匹配，扩展了他在形状描述方面的工作。这表明了一些有趣的理论结果，但是在复杂表面上的实际使用仍有待证明。

    Szeliski【54】描述了一种从稀疏范围数据中估计运动的方法，没有点之间的对应关系，也没有特征提取。他的主要目标是创建一种方法来估计观察者在同一地形的两个距离图像帧之间的运动。给定一帧的点集，他应用平滑假设来创建点的平滑样条近似。然后，应用传统的最速下降算法来旋转并平移第二个数据集，使得它最小化点和表面之间的协方差加权的 z z z 差异的总和。他的方法是基于规则的 x y xy xy 网格结构，且没有计算真实的3-D点到表面的距离。最速下降方法是一个比我们下面提出的ICP算法到达局部极小值更慢的选项。Szeliski 使用最优贝叶斯数学，让他能够从模拟测距仪中降低更远距离的噪声值，在数据点中的不确定性从前景到背景变化很大。对于浅景深的高精度传感器，点之间的不确定性变化要小几个数量级，并且不太受关注。Szeliski 提供了合成地形数据和块（block）的实验结果。地形数据运动测试是一个沿着一个轴的简单的平移：一个1-D的相关问题。他的块测试涉及到6*度，但是块是一个非常简单的形状。总之，这项工作提出了一些有趣的创新点，但是实验结果在应用中没有说服力。

    Horn 和 Harris【33】也解决了在给定相同地形的有序数字化距离图像帧的情况下，估计观察者的精确的刚体运动的问题。他们描述了一个射程率约束方程和海拔率约束方程。结果是一种非迭代最小二乘法，只要数据帧之间的运动相对较小，就可以提供六*度运动估计。这个方法比 Szeliski 提出的方法要快得多，但是尚不清楚这种方法是否可以推广到形状的任意旋转和平移。

    Kamgar-Parsi等【36】也描述了一种无需提取特征的多重叠距离图像配准方法。该方法使用*工作地非常好。使用 2.5-D 距离数据的水平集（level sets），但基本上仅限于平面中的三个*度，因为这项工作旨在拼凑地形地图数据。

    Li【38】解决了任意旋转和平移的*形式表面匹配问题。他的方法为数据和模型形状形成了基本表面区域的属性关系图，然后使用不精确的方法执行图匹配，该方法允许属性和图邻接关系的可变性。这似乎是一个合理的方法，但它依赖于基于导数的数量的提取（relies on extraction of derivative-based quantities）。实验结果显示为咖啡杯和雷诺汽车零件；见Wong等【60】其它的相关工作使用用于3-D匹配的属性图。

    Gilbert 和 Foo【21】和 Gilbert等【22】的工作是相关的，它解决了两个目标形状之间距离的计算。此类方法可以作为下面描述的类似形状匹配技术的基础。然而，他们的方法的主要不便之处在于，必须将对象形状分解为凸的亚体（convex subbodies），这个问题通常对于任意 CAD 模型或数字化 3-D 数据来说并非微不足道。

3. 数学预备

    在这一章中，我们描述了在上面列出的各种几何表达上计算到给定点最近的点的方法。首先，覆盖基本几何实体，然后是参数实体，最后是隐式实体。读者可以参考 Mortenson【42】下面的一些项目以获取更多信息。

    两点 r ⃗ 1 = ( x 1 , y 1 , z 1 ) \vec{r}_1 = (x_1, y_1, z_1) r 1​=(x1​,y1​,z1​) 和 r ⃗ 2 = ( x 2 , y 2 , z 2 ) \vec{r}_2 = (x_2, y_2, z_2) r 2​=(x2​,y2​,z2​) 之间的欧式距离 d ( r ⃗ 1 , r ⃗ 2 ) = ∥ r ⃗ 1 , r ⃗ 2 ∥ = ( x 2 − x 1 ) 2 + ( y 2 − y 1 ) 2 + ( z 2 − z 1 ) 2 d(\vec{r}_1, \vec{r}_2) = \|\vec{r}_1, \vec{r}_2\| = \sqrt{(x_2-x_1)^2 + (y_2-y_1)^2 + (z_2-z_1)^2} d(r 1​,r 2​)=∥r 1​,r 2​∥=(x2​−x1​)2+(y2​−y1​)2+(z2​−z1​)2 ​。令 A = a ⃗ i ,    ( i = 1 , . . . , N a ) A ={\vec{a}_i},~~(i = 1, ... , N_a) A=a i​,  (i=1,...,Na​) 表示具有 N a N_a Na​ 个点的点集。定义点 p ⃗ \vec{p} p ​ 和点集 A A A 之间的距离

d ( p ⃗ , A ) = m i n i ∈ { 1 , . . . , N a } d ( p ⃗ , a ⃗ i )   (1) d(\vec{p}, A) = \underset{i \in \{1,...,N_a\}}{min} d(\vec{p}, \vec{a}_i)~\tag{1} d(p ​,A)=i∈{1,...,Na​}min​d(p ​,a i​) (1)

A A A 的最近点 a ⃗ j \vec{a}_j a j​ 满足等式 d ( p ⃗ , a ⃗ j ) = d ( p ⃗ , A ) d(\vec{p}, \vec{a}_j) = d(\vec{p}, A) d(p ​,a j​)=d(p ​,A)。

    令 l l l 为连接两个点 r ⃗ 1 \vec{r}_1 r 1​ 和 r ⃗ 2 \vec{r}_2 r 2​的线段。定义点 r ⃗ \vec{r} r 与线段 l l l 之间的距离

d ( p ⃗ , l ) = m i n u + v = 1 ∥ u r ⃗ 1 + v r ⃗ 2 − p ⃗ ∥   (2) d(\vec{p}, l) = \underset{u+v=1}{min} \|u\vec{r}_1 + v\vec{r}_2 - \vec{p}\|~\tag{2} d(p ​,l)=u+v=1min​∥ur 1​+vr 2​−p ​∥ (2)

其中 u ∈ [ 0 , 1 ]   , v ∈ [ 0 , 1 ] u \in [0, 1]~, v \in [0, 1] u∈[0,1] ,v∈[0,1]。需要的闭式计算很直接。令 L = { l i } ,   ( i = 1 , . . . , N l ) L = \{l_i\},~(i = 1,...,N_l) L={li​}, (i=1,...,Nl​) 为 N l N_l Nl​ 条线段 l i l_i li​ 的集合。定义点 p ⃗ \vec{p} p ​ 和线段集 L L L 的距离

d ( p ⃗ , L ) = m i n i ∈ { 1 , . . . , N l } d ( p ⃗ , l i )   (3) d(\vec{p}, L) = \underset{i \in \{1, ... , N_l\}}{min} d(\vec{p}, l_i)~\tag{3} d(p ​,L)=i∈{1,...,Nl​}min​d(p ​,li​) (3)

    令 t t t 为由三个点 r ⃗ 1 = ( x 1 , y 1 , z 1 ) ,   r 2 = ( x 2 , y 2 , z 2 )   , r 3 = ( x 3 , y 3 , z 3 ) \vec{r}_1 = (x_1, y_1, z_1),~r_2 = (x_2, y_2, z_2)~,r_3 = (x_3, y_3, z_3) r 1​=(x1​,y1​,z1​), r2​=(x2​,y2​,z2​) ,r3​=(x3​,y3​,z3​) 定义的三角形，定义点 p 到三角形 t t t 间的距离

d ( p ⃗ , t ) = m i n u + v + w = 1 ∥ u r ⃗ 1 + v r ⃗ 2 + w r ⃗ 3 − p ⃗ ∥   (4) d(\vec{p}, t) = \underset{u+v+w=1}{min} \|u\vec{r}_1 + v\vec{r}_2 + w\vec{r}_3 - \vec{p}\|~\tag{4} d(p ​,t)=u+v+w=1min​∥ur 1​+vr 2​+wr 3​−p ​∥ (4)

其中， u ∈ [ 0 , 1 ] ,   v ∈ [ 0 , 1 ] ,   w ∈ [ 0 , 1 ] u \in [0,1],~v \in [0, 1],~w \in [0, 1] u∈[0,1], v∈[0,1], w∈[0,1] 需要的闭式计算也很直接。令 T = { t i } ,   ( i = 1 , . . . , N t ) T = \{t_i\},~(i = 1, ... , N_t) T={ti​}, (i=1,...,Nt​) 为 N t N_t Nt​ 个三角形 t i t_i ti​ 的集合。定义点 p ⃗ \vec{p} p ​ 和三角形集 T T T 的距离

d ( p ⃗ , T ) = m i n i ∈ { 1 , . . . , N t } d ( p ⃗ , t i )   (5) d(\vec{p}, T) = \underset{i \in \{1, ... , N_t\}}{min} d(\vec{p}, t_i)~\tag{5} d(p ​,T)=i∈{1,...,Nt​}min​d(p ​,ti​) (5)

y i y_i yi​ 到三角形集上的最近点满足等式 d ( p ⃗ , y ⃗ i ) = d ( p ⃗ , T ) d(\vec{p}, \vec{y}_i) = d(\vec{p}, T) d(p ​,y ​i​)=d(p ​,T)

3.1 点到参数化实体距离

在这一节中，参数曲线和参数表面到被视为单个的参数实体 r ⃗ ( u ⃗ ) \vec{r}(\vec{u}) r (u ) 其中 u ⃗ = u ∈ R 1 \vec{u} = u \in R^1 u =u∈R1 应替换为参数曲线，而 u ⃗ = ( u , v ) ∈ R 2 \vec{u} = (u, v) \in R^2 u =(u,v)∈R2 应替换为参数曲面（R表示实线）。区间内曲线的求值域，但曲面的求值域可以是平面中的任意闭合连接区域。关于参数实体的更多信息，如 贝塞尔和B样条曲线和曲面，见【9，15-17，42，52】

    从一个给定点到一个参数实体的距离 E E E 为

d ( p ⃗ , E ) = m i n r ⃗ ( u ⃗ ) ∈ E d ( p ⃗ , r ⃗ ( u ⃗ ) )   (6) d(\vec{p}, E) = \underset{\vec{r}(\vec{u}) \in E}{min} d(\vec{p}, \vec{r}(\vec{u}))~\tag{6} d(p ​,E)=r (u )∈Emin​d(p ​,r (u )) (6)

计算距离的计算不是闭式的，而是相对复杂的。下面描述了一个计算点到曲线和点到曲面距离的方法。一旦实现了单个实体的距离度量，参数化实体集再次变得简单明了。令 F = { E i } ,   ( i = 1 , . . . , N e ) F = \{E_i\},~(i = 1, ... ,N_e) F={Ei​}, (i=1,...,Ne​) 表示 N e N_e Ne​ 个由 E i E_i Ei​ 表示的参数实体。定位点 p ⃗ \vec{p} p ​ 到参数实体集 F F F 的距离

d ( p ⃗ , F ) = m i n i ∈ { 1 , . . . , N e } d ( p ⃗ , E ⃗ i )   (7) d(\vec{p}, F) = \underset{i \in \{1, ... ,N_e\}}{min} d(\vec{p}, \vec{E}_i)~\tag{7} d(p ​,F)=i∈{1,...,Ne​}min​d(p ​,E i​) (7)

在参数实体集 F F F 上的最近点 y ⃗ i \vec{y}_i y ​i​ 满足等式 d ( p ⃗ , y ⃗ j ) = d ( p ⃗ , F ) d(\vec{p}, \vec{y}_j) = d(\vec{p}, F) d(p ​,y ​j​)=d(p ​,F)

    计算从一个点到一个参数实体的距离的第一步是创建一个基于单纯形的近似（线段或三角形）。对于一个参数空间曲线 C = { r ⃗ ( u ) } C = \{\vec{r}(u)\} C={r (u)}，可以计算折线 L ( C , δ ) L(C, \delta) L(C,δ) 使得分段线性近似不会偏离空间曲线超过预先指定的距离 δ \delta δ。通过用参数曲线对应的 u u u 参数值标记折线的每个点，可以得到离线段集最近的点的参数值 u a u_a ua​ 的估计。

    同样的，对于一个参数表面 S = { r ⃗ ( u , v ) } S = \{\vec{r}(u, v)\} S={r (u,v)}，可以计算三角集 T ( S , δ ) T(S, \delta) T(S,δ) 使得分段三角形近似不会偏离表面超过预先指定的距离 δ \delta δ。通过用参数化曲面相应的 ( u , v ) (u, v) (u,v) 参数值标记每个三角形顶点，可以从三角形集合中获得最近点的参数值的估计值 ( u a , v a ) (u_a, v_a) (ua​,va​)。作为这些曲线和曲面过程的结果，可以假设初始值 u ⃗ a \vec{u}_a u a​ 是已知的，这样 r ⃗ ( u ⃗ a ) \vec{r}(\vec{u}_a) r (u a​) 非常接近到参数实体上的最近点。

    当可靠的起点 u ⃗ a \vec{u}_a u a​ 已知时，点到参数实体距离的问题是采用纯牛顿最小化方法的理想选择。要优化的标量目标函数为

f ( u ⃗ ) = ∥ r ⃗ ( u ⃗ ) − p ⃗ ∥ 2   (8) f(\vec{u}) = \|\vec{r}(\vec{u}) - \vec{p}\|^2~\tag{8} f(u )=∥r (u )−p ​∥2 (8)

令 ∇ = [ ∂ / ∂ u ⃗ ] t \nabla = [\partial / \partial \vec{u}]^t ∇=[∂/∂u ]t 为向量微分梯度算子（其中 t t t 表示向量的转置）。 f f f 在 ∇ f = 0 \nabla f = 0 ∇f=0 处有最小值。当参数实体是表面时，2D梯度向量为 ∇ f = [ f u , f v ] t \nabla f = [f_u, f_v]^t ∇f=[fu​,fv​]t，2D Hessian 矩阵为

∇ ∇ t ( f ) = [ f u u f u v f u v f v v ]   (9) \nabla\nabla^t(f) = \left[\begin{matrix} f_{uu} & f_{uv} \\ f_{uv} & f_{vv}\end{matrix}\right]~\tag{9} ∇∇t(f)=[fuu​fuv​​fuv​fvv​​] (9)

其中目标函数的偏导数为

f u ( u ⃗ ) = 2 r ⃗ u t ( u ⃗ ) ( r ⃗ ( u ⃗ ) − p ⃗ )   (10) f_u(\vec{u}) = 2\vec{r}_u^t(\vec{u})(\vec{r}(\vec{u}) - \vec{p})~\tag{10} fu​(u )=2r ut​(u )(r (u )−p ​) (10)

f v ( u ⃗ ) = 2 r ⃗ v t ( u ⃗ ) ( r ⃗ ( u ⃗ ) − p ⃗ )   (11) f_v(\vec{u}) = 2\vec{r}_v^t(\vec{u})(\vec{r}(\vec{u}) - \vec{p})~\tag{11} fv​(u )=2r vt​(u )(r (u )−p ​) (11)

f u u ( u ⃗ ) = 2 r ⃗ u u t ( u ⃗ ) ( r ⃗ ( u ⃗ ) − p ⃗ ) + 2 r ⃗ u t ( u ⃗ ) r ⃗ u ( u ⃗ )   (12) f_{uu}(\vec{u}) = 2\vec{r}_{uu}^t(\vec{u})(\vec{r}(\vec{u}) - \vec{p}) + 2\vec{r}_u^t(\vec{u}) \vec{r}_u(\vec{u})~\tag{12} fuu​(u )=2r uut​(u )(r (u )−p ​)+2r ut​(u )r u​(u ) (12)

f v v ( u ⃗ ) = 2 r ⃗ v v t ( u ⃗ ) ( r ⃗ ( u ⃗ ) − p ⃗ ) + 2 r ⃗ v t ( u ⃗ ) r ⃗ v ( u ⃗ )   (13) f_{vv}(\vec{u}) = 2\vec{r}_{vv}^t(\vec{u})(\vec{r}(\vec{u}) - \vec{p}) + 2\vec{r}_v^t(\vec{u}) \vec{r}_v(\vec{u})~\tag{13} fvv​(u )=2r vvt​(u )(r (u )−p ​)+2r vt​(u )r v​(u ) (13)

f u v ( u ⃗ ) = 2 r ⃗ u v t ( u ⃗ ) ( r ⃗ ( u ⃗ ) − p ⃗ ) + 2 r ⃗ u t ( u ⃗ ) r ⃗ v ( u ⃗ )   (14) f_{uv}(\vec{u}) = 2\vec{r}_{uv}^t(\vec{u})(\vec{r}(\vec{u}) - \vec{p}) + 2\vec{r}_u^t(\vec{u}) \vec{r}_v(\vec{u})~\tag{14} fuv​(u )=2r uvt​(u )(r (u )−p ​)+2r ut​(u )r v​(u ) (14)

在曲线情况下，只需要计算 f u f_u fu​ 和 f u u f_{uu} fuu​。对任意实体的牛顿更新公式为

u ⃗ k + 1 = u ⃗ k − [ ∇ ∇ t ( f ) ( u ⃗ k ) ] − 1 ∇ f ( u ⃗ k )   (15) \vec{u}_{k+1} = \vec{u}_k - [\nabla \nabla^t (f)(\vec{u}_k)]^{-1} \nabla f(\vec{u}_k)~\tag{15} u k+1​=u k​−[∇∇t(f)(u k​)]−1∇f(u k​) (15)

其中 u ⃗ 0 = u ⃗ a \vec{u}_0 = \vec{u}_a u 0​=u a​。上述起点选择方法基于具有相当小的 δ \delta δ 的简单近似，用牛顿方法计算最近点通常在1~5次迭代就能收敛，典型的是3次。与找到好的起点相比，牛顿方法的计算成本非常低。

3.2 点到隐式实体的距离

隐式几何定义为可能为向量值的多元函数 g ⃗ ( r ⃗ ) = 0 \vec{g}(\vec{r}) = 0 g ​(r )=0 的零集。从给定的点 p ⃗ \vec{p} p ​ 到隐式实体的距离为

d ( p ⃗ , I ) = m i n g ⃗ ( r ⃗ = 0 ) d ( p ⃗ , r ⃗ ) = m i n g ⃗ ( r ⃗ ) = 0 ∥ r ⃗ − p ⃗ ∥   (16) d(\vec{p}, I) = \underset{\vec{g}(\vec{r} = 0)}{min} d(\vec{p}, \vec{r}) = \underset{\vec{g}(\vec{r}) = 0}{min} \|\vec{r} - \vec{p}\|~\tag{16} d(p ​,I)=g ​(r =0)min​d(p ​,r )=g ​(r )=0min​∥r −p ​∥ (16)

求这个距离的计算也是闭式的，并且相对复杂的。下面给出点到曲线和点到曲面的距离的方法。一旦实现了单个实体的距离度量，隐式实体集就简单明了。令 J = { I k } ,   ( k = 1 , . . . , N I ) J = \{I_k\},~(k = 1, ..., N_I) J={Ik​}, (k=1,...,NI​) 表示由 I k I_k Ik​ 表示的 N I N_I NI​ 个参数实体。定义点 p ⃗ \vec{p} p ​ 到隐式实体集的距离

d ( p ⃗ , J ) m i n k ∈ { 1 , . . . , N I }   d ( p ⃗ , I k ⃗ )   (17) d(\vec{p}, J) \underset{k \in \{1, ...,N_I\}}{min}~d(\vec{p}, \vec{I_k})~\tag{17} d(p ​,J)k∈{1,...,NI​}min​ d(p ​,Ik​ ​) (17)

隐式实体 I j I_j Ij​ 上的最近点 y ⃗ i \vec{y}_i y ​i​ 满足等式 d ( p ⃗ , y ⃗ j ) = d ( p ⃗ , J ) d(\vec{p}, \vec{y}_j) = d(\vec{p}, J) d(p ​,y ​j​)=d(p ​,J)。

    计算从一个点到一个隐式实体的距离的第一步是创建一个基于单纯形的近似（线段或三角形）,就像对参数化实体【7】所做的那样。计算点到线集或者点到三角形集的距离得到一个近似的最近点 r ⃗ a \vec{r}_a r a​，可以用它来计算确切的距离。

    隐式实体距离问题与无约束优化就足够的参数实体情况完全不同。为了找到由 g ⃗ ( r ⃗ ) = 0 \vec{g}(\vec{r}) = 0 g ​(r )=0 定义的隐式实体上到给定点 p ⃗ \vec{p} p ​ 的最近点，必须解决约束优化问题以最小化受非线性约束的二次目标函数

M i n i m i z e   f ( r ⃗ ) = ∥ r ⃗ − p ⃗ ∥ 2    w h e r e    g ⃗ ( r ⃗ ) = 0   (18) \mathrm{Minimize} ~f(\vec{r}) = \|\vec{r} - \vec{p}\|^2~~\mathrm{where}~~ \vec{g}(\vec{r}) = 0~\tag{18} Minimize f(r )=∥r −p ​∥2  where  g ​(r )=0 (18)

解决这个问题的一种方法是形成方程的增广拉格朗日乘子方程【40】：

∇ f ( r ⃗ ) + λ ⃗ t ∇ g ⃗ ( r ⃗ ) = 0 g ⃗ ( r ⃗ ) = 0   (19) \nabla f(\vec{r}) + \vec{\lambda}^t \nabla \vec{g}(\vec{r}) = 0 \\ \vec{g}(\vec{r}) = 0 ~\tag{19} ∇f(r )+λ t∇g ​(r )=0g ​(r )=0 (19)

其中 ∂ = [ ∂ / ∂ r ⃗ ] t \partial = [\partial / \partial \vec{r}]^t ∂=[∂/∂r ]t 并通过数值方法求解这个非线性方程组。对于这个非线性方程组的方程和未知数数量，平面曲线3个，曲面4个，隐式定义的空间曲线5个。即使没有一个好的起点，延续方法(continuation methods)【41】也可以用来解决代数实体的这个问题，但是一个好的起点将允许使用更快的方法，例如多维牛顿的求根方法。从数值的角度看，参数化的方法要更容易处理些。从应用的角度看，没有工业 CAD 系统以隐式形式存储*曲面或曲线。因此，在我们实现的系统中，通过特殊情况数学(via special case mathematics)（如，球）或通过参数化形式，处理隐式的感兴趣表面。当然，如果有应用需要在隐式形式中处理*形式的隐式实体【51】，可以实现上述算法。

当 g ( r ⃗ 0 ) g(\vec{r}_0) g(r 0​) 趋于0时，Taubin【55】使用近似距离算法，该算法暗含了一个为表面和平面曲线简单更新的公式：

r ⃗ k + 1 = r ⃗ k − ∇ g ( r ⃗ k ) g ( r ⃗ k ) ∥ ∇ g ( r ⃗ k ) ∥ 2   (20) \vec{r}_{k+1} = \vec{r}_k - \frac{\nabla g(\vec{r}_k) g(\vec{r}_k)}{\|\nabla g(\vec{r}_k)\|^2} ~\tag{20} r k+1​=r k​−∥∇g(r k​)∥2∇g(r k​)g(r k​)​ (20)

只有在起点 r ⃗ 0 \vec{r}_0 r 0​ 处方向为 ∇ g ( r ⃗ ) \nabla g(\vec{r}) ∇g(r ) 的射线与隐式实体的交点处的法线向量具有相同方向时，此方法才准确。这通常不对，而且通常点离隐式实体越远，近似通常越糟糕。因此，如果需要精确的距离结果，则该结果不可用。

3.3 对应点集配准

所有的最近点（最小距离）算法都以推广到 n n n 维的形式被提到。并对产生最小二乘旋转和平移的另一个必要步骤进行了回顾。为了我们的目的，在二维和三维中，基于四元数的算法优于奇异值分解 (SVD) 方法，因为不需要反射(reflections)。然而，基于两点分布的互协方差矩阵的 SVD 方法确实很容易推广到 n n n 维，并且将是我们在任何 n > 3 n > 3 n>3 的 n n n 维应用中选择的方法。下面描述了 Horn【31】的基本解决方案，尽管该方法与 Faugeras【18】等价。我们的总结强调了 SVD 互协方差矩阵的作用，这是其他工作中未讨论的重要关系。

    单位四元数是一个四维向量 q ⃗ R = [ q 0   q 1   q 2   q 3 ] t \vec{q}_R = [q_0~ q_1~ q_2~ q_3]^t q ​R​=[q0​ q1​ q2​ q3​]t，其中 q 0 ≥ 0 ,    q 0 2 + q 1 2 + q 2 2 + q 3 2 = 1 q_0 \ge 0,~~q_0^2 + q_1^2 + q_2^2 + q_3^2 = 1 q0​≥0,  q02​+q12​+q22​+q32​=1。

R = [ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 1 q 2 + q 0 q 3 ) q 0 2 + q 2 2 − q 1 2 − q 3 2 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 2 q 3 + q 0 q 1 ) q 0 2 + q 3 2 − q 1 2 − q 2 2 ]   (21) \mathbf{R} = \left[\begin{matrix}q_0^2+q_1^2-q_2^2-q_3^2 & 2(q_1q_2 - q_0q_3) & 2(q_1q_3 + q_0q_2) \\ 2(q_1q_2+q_0q_3) & q_0^2+q_2^2-q_1^2-q_3^2 & 2(q_2q_3 - q_0q_1) \\ 2(q_1q_3-q_0q_2) & 2(q_2q_3 + q_0q_1) & q_0^2+q_3^2-q_1^2-q_2^2 \end{matrix}\right]~\tag{21} R=⎣⎡​q02​+q12​−q22​−q32​2(q1​q2​+q0​q3​)2(q1​q3​−q0​q2​)​2(q1​q2​−q0​q3​)q02​+q22​−q12​−q32​2(q2​q3​+q0​q1​)​2(q1​q3​+q0​q2​)2(q2​q3​−q0​q1​)q02​+q32​−q12​−q22​​⎦⎤​ (21)

这一页的底部讲到了通过单位四元数生成 3 × 3 3 \times 3 3×3 旋转矩阵。令 q ⃗ T = [ q 4   q 5   q 6 ] t \vec{q}_T = [q_4~ q_5~ q_6]^t q ​T​=[q4​ q5​ q6​]t 为平移向量。完整的配准状态向量 q ⃗ \vec{q} q ​ 表示为 q ⃗ = [ q ⃗ R ∣ q ⃗ T ] t \vec{q} = [\vec{q}_R | \vec{q}_T]^t q ​=[q ​R​∣q ​T​]t。令 P = { p ⃗ i } P = \{\vec{p}_i\} P={p ​i​} 为与模型点集 X = { x ⃗ i } X = \{\vec{x}_i\} X={x i​} 对齐的测量数据点集。其中 N x = N p N_x = N_p Nx​=Np​ 且每个点 p ⃗ i \vec{p}_i p ​i​ 与具有相同索引的点 x ⃗ i \vec{x}_i x i​对应。要最小化的均方目标函数为

f ( q ⃗ ) = 1 N p ∑ i = 1 N p ∥ x ⃗ i − R ( q ⃗ R ) p ⃗ i − q ⃗ T ∥ 2   (22) f(\vec{q}) = \frac{1}{N_p} \displaystyle\sum_{i = 1}^{N_p}\|\vec{x}_i - \mathbf{R}(\vec{q}_R) \vec{p}_i - \vec{q}_T\|^2~\tag{22} f(q ​)=Np​1​i=1∑Np​​∥x i​−R(q ​R​)p ​i​−q ​T​∥2 (22)

测量点集 P P P 的“质量中心” μ ⃗ p \vec{\mu}_p μ ​p​ 和 X X X 的质量中心 μ ⃗ x \vec{\mu}_x μ ​x​ 为

μ ⃗ p = 1 N p ∑ i = 1 N p p ⃗ i      a n d      μ ⃗ x = 1 N x ∑ i = 1 N x x ⃗ i   (23) \vec{\mu}_p = \frac{1}{N_p} \displaystyle\sum_{i = 1}^{N_p} \vec{p}_i ~~~~\mathrm{and}~~~~\vec{\mu}_x = \frac{1}{N_x} \displaystyle\sum_{i=1}^{N_x} \vec{x}_i~\tag{23} μ ​p​=Np​1​i=1∑Np​​p ​i​    and    μ ​x​=Nx​1​i=1∑Nx​​x i​ (23)

点集 P P P 和 X X X 的互x协方差矩阵 Σ p x \Sigma_{px} Σpx​ 为

Σ p x = 1 N p ∑ i = 1 N p [ ( p ⃗ i − μ ⃗ p ) ( x ⃗ i − μ ⃗ x ) t ] = 1 N p ∑ i = 1 N p [ p ⃗ i x ⃗ t ] − μ ⃗ p μ ⃗ x t   (24) \Sigma_{px} = \frac{1}{N_p} \displaystyle\sum_{i = 1}^{N_p}[(\vec{p}_i - \vec{\mu}_p)(\vec{x}_i - \vec{\mu}_x)^t] = \frac{1}{N_p} \displaystyle\sum_{i = 1}^{N_p}[\vec{p}_i \vec{x}^t] - \vec{\mu}_p \vec{\mu}_x^t~\tag{24} Σpx​=Np​1​i=1∑Np​​[(p ​i​−μ ​p​)(x i​−μ ​x​)t]=Np​1​i=1∑Np​​[p ​i​x t]−μ ​p​μ ​xt​ (24)

反对称矩阵 A i j = ( Σ p x − Σ p x T ) i j A_{ij} = (\Sigma_{px} - \Sigma_{px}^T)_{ij} Aij​=(Σpx​−ΣpxT​)ij​ 的循环分量用于形成列向量 Δ = [ A 23   A 31   A 12 ] T \Delta = [A_{23}~ A_{31}~ A_{12}]^T Δ=[A23​ A31​ A12​]T。然后该向量形成 4 × 4 4 \times 4 4×4 对称矩阵 Q ( Σ p x ) Q(\Sigma_{px}) Q(Σpx​)

Q ( Σ p x ) [ t r ( Σ p x ) Δ T Δ Σ p x + Σ p x T − t r ( Σ p x ) I 3 ]   (25) Q(\Sigma_{px})\left[\begin{matrix} \mathrm{tr}(\Sigma_{px}) & \Delta^T \\ \Delta & \Sigma_{px} + \Sigma_{px}^T - \mathrm{tr}(\Sigma_{px})\mathbf{I}_3 \end{matrix}\right]~\tag{25} Q(Σpx​)[tr(Σpx​)Δ​ΔTΣpx​+ΣpxT​−tr(Σpx​)I3​​] (25)

其中 I 3 \mathbf{I}_3 I3​ 是 3 × 3 3 \times 3 3×3 的单位矩阵。选择矩阵 Q ( Σ p x ) Q(\Sigma_{px}) Q(Σpx​) 最大特征值对应的单位特征向量 q ⃗ R = [ q 0   q 1   q 2   q 3 ] t \vec{q}_R = [q_0~q_1~q_2~q_3]^t q ​R​=[q0​ q1​ q2​ q3​]t 作为最优的旋转。优化平移向量为

q ⃗ T = μ ⃗ x − R ( q ⃗ R ) μ ⃗ p   (26) \vec{q}_T = \vec{\mu}_x - \mathbf{R}(\vec{q}_R) \vec{\mu}_p~\tag{26} q ​T​=μ ​x​−R(q ​R​)μ ​p​ (26)

该最小二乘四元数运算是 O ( N p ) O(N_p) O(Np​)，并表示为

( q ⃗ , d m s ) = Q ( P , X )   (27) (\vec{q}, d_{ms}) = \mathcal{Q}(P, X)~\tag{27} (q ​,dms​)=Q(P,X) (27)

其中 d m s d_{ms} dms​ 为均方点匹配误差。符号 q ⃗ ( R ) \vec{q}(R) q ​(R) 表示由配准向量 q ⃗ \vec{q} q ​ 对点集 P P P 转换后的结果。

4. 迭代最近点算法

现在已经概述了计算几何形状上到给定点的最近点和计算最小二乘配准向量的方法，ICP 算法可以用抽象几何形状 X X X 来描述，其内部表达必须已知才能执行算法，但不是本次讨论的关注点。因此，所有以下情况：1）点集，2）线段集合，3）参数曲线集合，4）隐式曲线集合，5）三角集合，6）参数曲面集合，和7）隐式曲面集合都适用于此。

    在算法的描述中，一个“数据”形状 P P P 被移动（配准，放置）使得与"模型"形状 X X X最好地对齐。为了我们的目的，如果数据已经不是点集的形式，那么则需要将它分解成点集。幸运的是，这很容易；被用作三角或线性集的点是顶点合终点，而且如果数据形状来自一个曲面或则曲线的形式，那么使用三角或直线近似（如上面所描述的那样）的顶点合端点。用 N p N_p Np​ 表示数据形状中点的数量。令 N x N_x Nx​ 为涉及的模型形状中的点，线段，三角形的数量。正如上面所描述的那样，在我们的系统中实现的曲线和表面最近点评估器，需要直线或三角形框架产生牛顿迭代的初始参数值；因此，值 N x N_x Nx​ 仍然与这些光滑实体相关，但是根据近似的精度的不同而不同。

    用 y ⃗ \vec{y} y ​ 表示 X X X 中的产生最小距离最近点，即 d ( p ⃗ , y ⃗ ) = d ( p ⃗ , X ) ,   y ⃗ ∈ X d(\vec{p}, \vec{y}) = d(\vec{p}, X),~\vec{y} \in X d(p ​,y ​)=d(p ​,X), y ​∈X。注意，计算最近点（的成本/复杂度）最糟糕的情况是 O ( N x ) O(N_x) O(Nx​)，预期成本为 log ⁡ ( N x ) \log(N_x) log(Nx​)。当对 P P P 中的每个点进行最近点计算（从 p ⃗ \vec{p} p ​ 到 X X X）时，该过程是最糟的情况 O ( N p N x ) O(N_pN_x) O(Np​Nx​)。令 Y Y Y 为最近点结果的集合，令 C \mathcal{C} C 为最近点算子：

Y = C ( P , X )   (29) Y = \mathcal{C}(P, X)~\tag{29} Y=C(P,X) (29)

给出所得到的对应点集 Y Y Y，描述最小二乘配准为：

( ) = Q ( P , Y )   (30) () = \mathcal{Q}(P, Y)~\tag{30} ()=Q(P,Y) (30)

然后，通过 P = q ⃗ ( P ) P = \vec{q}(P) P=q ​(P) 更新数据形状点集的位置。

4.1 ICP算法描述

ICP算法现在可以被描述为：

• 来自数据形状的 N p N_p Np​ 个点 { p ⃗ i } \{\vec{p}_i\} {p ​i​} 的点集 P P P 和模型形状 X X X（假设有 N x N_x Nx​ 个集合主元：点，线，三角形）。
• 通过设置 P 0 = P ,   q ⃗ 0 = [ 1 , 0 , 0 , 0 , 0 , 0 , 0 ] t    a n d k = 0 P_0 = P,~\vec{q}_0 = [1, 0, 0, 0, 0, 0, 0]^t~~\mathrm{and} k = 0 P0​=P, q ​0​=[1,0,0,0,0,0,0]t  andk=0 初始化迭代。配准向量是相对于初始数据集 P 0 P_0 P0​ 定义的，因此，最终的配准表达的是完整的变换【即求的结果是相对初始的变换，而不是相对上一次迭代的变换】。应用第1，2，3，4步直到收敛到容差为 τ \tau τ。括号中给出了每次操作的计算成本。
  • a. 计算最近点： Y k = C ( P k , X ) Y_k = \mathcal{C}(P_k, X) Yk​=C(Pk​,X)（成本(复杂的)：最坏O(N_pN_x)，平均O(N_p)）
  • b. 计算配准： ( q ⃗ k , d k ) = ( Q ) ( P 0 , Y k ) (\vec{q}_k, d_k) = \mathcal(Q)(P_0, Y_k) (q ​k​,dk​)=(Q)(P0​,Yk​) ）（成本：O(N_p)）
  • c. 应用配准： P k + 1 = q ⃗ k ( P 0 ) P_{k+1} = \vec{q}_k(P_0) Pk+1​=q ​k​(P0​) （成本：O(N_p)）
  • d. 当均方误差的变化小于预设的阈值（指定所需的配准精度） d k − d k + 1 < τ    ( τ > 0 ) d_k - d_{k+1} < \tau~~(\tau > 0) dk​−dk+1​<τ  (τ>0) 终止迭代。

如果要求无量纲的阈值，可以用 τ t r ( Σ x ) \tau\sqrt{tr(\Sigma_x)} τtr(Σx​) ​ 代替 τ \tau τ，其中模型形状的协方差的迹的平方根表明了模型形状粗略的大小。

4.2 收敛定理

    现在描述并证明ICP算法的收敛理论。关键思路是：1）最小二乘配准在每次迭代过程中一般性地减小对应点间的平均距离，然而 2）最近点的确定通常会分别减少每个点的距离。当然，各自距离的减小也减小了平均距离，因为一组更小的正数的平均也更小。我们在下面的证明中提供了详细的解释。

    定理：对于均方距离目标函数，迭代最近点算法总是单调地收敛到局部极小值。
    证明：给定 P k = { p ⃗ i k = q ⃗ k ( P 0 ) } P_k = \{\vec{p}_{ik} = \vec{q}_k(P_0)\} Pk​={p ​ik​=q ​k​(P0​)} 和 X X X，给定 X X X 的内部几何表达，如前面提到的那样，计算最近点 Y k = { y ⃗ i k } Y_k = \{\vec{y}_{ik}\} Yk​={y ​ik​} 的集合。定义对应的均方误差 e k e_k ek​

e k = 1 N p ∑ i = 1 N p ∥ y ⃗ i k − p ⃗ i k ∥ 2   (31) e_k = \frac{1}{N_p} \displaystyle\sum_{i = 1}^{N_p} \|\vec{y}_{ik} - \vec{p}_{ik}\|^2~\tag{31} ek​=Np​1​i=1∑Np​​∥y ​ik​−p ​ik​∥2 (31)

应用 Q \mathcal{Q} Q 算子从对应中得到 q ⃗ k \vec{q}_k q ​k​ 和 d k d_k dk​：

d k = 1 N p ∑ i = 1 N p ∥ y ⃗ i k − R ( q ⃗ k R ) p ⃗ i 0 − q ⃗ k T ∥ 2   (32) d_k = \frac{1}{N_p} \displaystyle\sum_{i = 1}^{N_p} \|\vec{y}_{ik} - \mathbf{R}(\vec{q}_{kR}) \vec{p}_{i0} - \vec{q}_{kT} \|^2~\tag{32} dk​=Np​1​i=1∑Np​​∥y ​ik​−R(q ​kR​)p ​i0​−q ​kT​∥2 (32)

d k ≤ e k d_k \le e_k dk​≤ek​ 总是这样。假设 d k > e k d_k > e_k dk​>ek​。如果这样，那么在点集上的单位变换将会产生一个比最小二乘配准更小的均方误差，这样的情况是不可能的。接下来，将最小二乘配准应用到点集 P 0 P_0 P0​，得到点集 P k + 1 P_{k+1} Pk+1​。如果保持到点集 Y k Y_k Yk​ 之前的对应，那么均方误差任然是 d k d_k dk​，即

d k = 1 N p ∑ i = 1 N p ∥ y ⃗ i k − p ⃗ i , k + 1 ∥ 2   (33) d_k = \frac{1}{N_p} \displaystyle\sum_{i = 1}^{N_p} \|\vec{y}_{ik} - \vec{p}_{i,k+1}\|^2~\tag{33} dk​=Np​1​i=1∑Np​​∥y ​ik​−p ​i,k+1​∥2 (33)

然而，在连续应用最近点算子之后，得到了一个新的点集 Y k + 1 = C ( P k + 1 , X ) Y_{k+1} = \mathcal{C}(P_{k+1}, X) Yk+1​=C(Pk+1​,X)，显然

∥ y ⃗ i , k + 1 − p ⃗ i , k + 1 ∥ ≤ ∥ y ⃗ i k − p ⃗ i , k + 1 ∥    f o r e a c h i = 1 , . . . , N p   (34) \|\vec{y}_{i,k+1} - \vec{p}_{i,k+1}\| \le \|\vec{y}_{ik} - \vec{p}_{i,k+1}\| ~~\mathrm{for each} i = 1,...,N_p~\tag{34} ∥y ​i,k+1​−p ​i,k+1​∥≤∥y ​ik​−p ​i,k+1​∥  foreachi=1,...,Np​ (34)

因为点 y ⃗ i k \vec{y}_{ik} y ​ik​ 是通过 q ⃗ k \vec{q}_k q ​k​ 变换前的最近点，并且相对 p ⃗ i , k + 1 \vec{p}_{i,k+1} p ​i,k+1​ 位于一个新的距离处。如果 y ⃗ i , k + 1 \vec{y}_{i,k+1} y ​i,k+1​ 比 y ⃗ i , k \vec{y}_{i,k} y ​i,k​ 离 p ⃗ i , k + 1 \vec{p}_{i,k+1} p ​i,k+1​ 更远，那么这将直接地与最近点算子 KaTeX parse error: Undefined control sequence: \mathal at position 1: \̲m̲a̲t̲h̲a̲l̲{C} 的基本操作相矛盾。因此，均方误差 e k e_k ek​ 和 d k d_k dk​ 必须满足如下不等式：

0 ≤ d k + 1 ≤ e k + 1 ≤ d k ≤ e k    f o r   a l l   k   (35) 0 \le d_{k+1} \le e_{k+1} \le d_k \le e_k ~~\mathrm{for~all }~k~\tag{35} 0≤dk+1​≤ek+1​≤dk​≤ek​  for all k (35)

当然，下界出现了，因为均方误差不能为负。因为均方误差序列是非增的且有下界，上面提到的算法一定回单调地收敛到极小值。

    实验性地，我们发现在前面的几次迭代中收敛很快，当接近局部极小值时收敛慢。尽管以这样的慢节奏，在30~50次迭代的某处，得到优异的结果：模型形状大小的 d k ≈ 0.1 d_k \approx 0.1% dk​≈0.1。在使用下一章节描述的简单的附加操作，收敛会加速。

4.3 加速的ICP算法

加速的ICP算法使用多元无约束最小化【45】的基线搜索方法上的细微变化。随着迭代最近点算法的执行，得到了一系列的配准向量： q ⃗ 1 , q ⃗ 2 , q ⃗ 3 , q ⃗ 4 , q ⃗ 5 , q ⃗ 6 , . . . , \vec{q}_1,\vec{q}_2,\vec{q}_3,\vec{q}_4,\vec{q}_5,\vec{q}_6,..., q ​1​,q ​2​,q ​3​,q ​4​,q ​5​,q ​6​,...,，它们追踪从单位变换到局部最优形状匹配（变换）的配准状态空间中的轨迹。考虑由下式定义的差分向量序列

Δ q ⃗ k = q ⃗ k − q ⃗ k − 1   (36) \Delta \vec{q}_k = \vec{q}_k - \vec{q}_{k-1}~\tag{36} Δq ​k​=q ​k​−q ​k−1​ (36)

这定义了配准状态空间中的反向。定义两个最新方向间的7空间中的角度

θ k = cos ⁡ − 1 ( Δ q ⃗ k t Δ q ⃗ k − 1 ∥ Δ q ⃗ k ∥ ∥ Δ q ⃗ k − 1 ∥ )   (37) \theta_k = \cos^{-1} \Big(\frac{{\Delta \vec{q}_k}^t \Delta \vec{q}_{k-1}}{\|\Delta \vec{q}_k\| \|\Delta \vec{q}_{k-1}\|}\Big)~\tag{37} θk​=cos−1(∥Δq ​k​∥∥Δq ​k−1​∥Δq ​k​tΔq ​k−1​​) (37)

令 δ θ \delta \theta δθ 为一个足够小的角度容差（如，10°）。如果

θ k < δ θ    a n d    θ k − 1 < δ θ   (38) \theta_k < \delta \theta ~~\mathrm{and}~~ \theta_{k-1} < \delta \theta ~\tag{38} θk​<δθ  and  θk−1​<δθ (38)

那么，最新的三个配准状态向量： q ⃗ k ,   q ⃗ k − 1 , q ⃗ k − 2 \vec{q}_k,~\vec{q}_{k-1}, \vec{q}_{k-2} q ​k​, q ​k−1​,q ​k−2​ 则有一个很好的方向对齐。令 d k ,   d k − 1 ,   d k − 2 d_{k},~d_{k-1},~d_{k-2} dk​, dk−1​, dk−2​ 为关联的均方误差，并令 v k ,   v k − 1 ,   v k − 2 v_{k},~v_{k-1},~v_{k-2} vk​, vk−1​, vk−2​ 为关联的近似弧长参数值：

v k = 0 ,   v k − 1 = − ∥ Δ q ⃗ k ∥ ,    v k − 2 = − ∥ Δ q ⃗ k − 1 ∥ + v k − 1   (39) v_{k} = 0,~v_{k-1} = -\|\Delta \vec{q}_k\|,~~v_{k-2} = -\|\Delta \vec{q}_{k-1}\| + v_{k-1}~\tag{39} vk​=0, vk−1​=−∥Δq ​k​∥,  vk−2​=−∥Δq ​k−1​∥+vk−1​ (39)

该情况如图1所示。接下来，对最新的三个点进行线性近似以及抛物线插值：

d 1 ( v ) = c 1 v + b 1    d 2 ( v ) = a 2 v 2 + b 2 v + c 2   (40) d_1(v) = c_1 v + b_1~~d_2(v) = a_2 v^2 + b_2 v + c_2~\tag{40} d1​(v)=c1​v+b1​  d2​(v)=a2​v2+b2​v+c2​ (40)

这给了我们一个可能的线性更新，基于线的零交叉，以及可能的抛物线更新，基于抛物线的极值点：

v 1 = − b 1 / a 1 > 0     v 2 = − b 2 / 2 a 2   (41) v_1 = -b_1/a_1 > 0~~~ v_2 = -b_2 / 2a_2~\tag{41} v1​=−b1​/a1​>0   v2​=−b2​/2a2​ (41)

为了安全起见，我们采用最大允许值 v m a x v_{max} vmax​。使用接下来的逻辑执行尝试更新:
1）如果 0 < v 2 < v 1 < v m a x    o r    0 < v 2 < v m a x < v 1 0 < v_2 < v_1 < v_{max}~~\mathrm{or}~~0 < v_2 < v_{max} < v_1 0<v2​<v1​<vmax​  or  0<v2​<vmax​<v1​， 当在点集上进行更新时，使用基于抛物线的配准更新向量： q ⃗ k ′ = q ⃗ k + v 2 Δ q ⃗ k / ∥ Δ q ⃗ k ∥ \vec{q}'_k = \vec{q}_k + v_2 \Delta \vec{q}_k / \|\Delta \vec{q}_k\| q ​k′​=q ​k​+v2​Δq ​k​/∥Δq ​k​∥ 而不是通常的向量 q ⃗ k \vec{q}_k q ​k​
2）如果 0 < v 1 < v 2 < v m a x    o r    0 < v 1 < v m a x < v 2    o r    v 2 < 0    a n d    0 < v 1 < v m a x 0 < v_1 < v_2 < v_{max}~~\mathrm{or}~~0 < v_1 < v_{max} < v_2 ~~\mathrm{or}~~v_2 < 0~~\mathrm{and}~~0 < v_1 < v_{max} 0<v1​<v2​<vmax​  or  0<v1​<vmax​<v2​  or  v2​<0  and  0<v1​<vmax​， 使用基线更新配准向量 q ⃗ k ′ = q ⃗ k + v 1 Δ q ⃗ k / ∥ Δ q ⃗ k ∥ \vec{q}'_k = \vec{q}_k + v_1 \Delta \vec{q}_k / \|\Delta \vec{q}_k\| q ​k′​=q ​k​+v1​Δq ​k​/∥Δq ​k​∥ 而不是通常的向量 q ⃗ k \vec{q}_k q ​k​。
3）如果 v 1 > v m a x    a n d    v 2 > v m a x v_1 > v_{max}~~\mathrm{and}~~v_2 > v_{max} v1​>vmax​  and  v2​>vmax​，使用最大允许的更新 q ⃗ k ′ = q ⃗ k + v m a x Δ q ⃗ k / ∥ Δ q ⃗ k ∥ \vec{q}'_k = \vec{q}_k + v_{max} \Delta \vec{q}_k / \|\Delta \vec{q}_k\| q ​k′​=q ​k​+vmax​Δq ​k​/∥Δq ​k​∥ 而不是通常的向量 q ⃗ k \vec{q}_k q ​k​。

我们实验性地发现自适应地设置 v m a x = 25 ∥ Δ q ⃗ k ∥ v_{max} = 25\|\Delta \vec{q}_k\| vmax​=25∥Δq ​k​∥ 已经对更新提供了一个好的合理检查，允许迭代最近点算法以一个给定的角度精度，用很少的（迭代）步骤移动到局部极小值。对于给定的 τ \tau τ 值，超过 50 次基本 ICP 迭代的标称运行通常会加速到 15 或 20 次迭代。

    如果更新的配准向量以某种方式超过最小值从而产生更差的均方误差，这将有利于使用最后两步的新配准构建一个新的抛物线并移动到适当的最小值。这在我们的实验中已经没有必要了。为了严谨，如果导致了更坏的均方误差，可以忽视建议的更新。

    为了提供一个有质量的例子比较，我们记录了基本的和加速的ICP算法在相同的*曲面匹配测试的50次迭代中的配准值，RMS(root mean square) 误差，最大误差，角度变化，累积的弧长值。图2展示了基本的ICP算法的结果。注意所有曲线的平滑性。最重要特种是 cos ⁡ ( δ θ ) \cos(\delta \theta) cos(δθ) 的绘制，表明了除前几次迭代之外的所有更新的一致方向。相反，加速的ICP算法展示了理想的跳跃行为，如图 3 所示。此外，请注意，在第一个加速步骤后，大多数量都接近它们的最终值，在第二个加速步骤后非常接近。每当在 cos ⁡ ( δ θ ) \cos(\delta \theta) cos(δθ) 与迭代计数的图中出现 V形下降时，就会发生加速步骤。

图2，针对基本非加速 ICP 算法的迭代次数绘制的各种数量。

4.4 可选的最小化方法

    ICP算法与其他可能的替代方案相比，它允许我们从给定的起始点更快地移动到7个空间中的局部极小值。每次迭代只需要一个评估最接近点算子：最高成本的计算。任何没有使用向量梯度估计的优化方法，如 Powell’s 方向集方法（Powell’s direction set method）， Nelder-Mead 下山单纯形法（Nelder-Mead downhill simplex method）或者模拟退火（simulated annealling）需要几百到几万次的最近点评估。这些数字是基于对模拟最小二乘配准步骤的动作的测试，其中配准步骤涉及到一次ICP迭代，但使用Powell’s 方向集方法和来自【45】的Nelder-Mean方法。

    任意使用向量梯度的优化方法，如最速下降（steepest descent），共轭梯度（conjugate gradient）以及可变度量方案（variable metric schemes），对每次数值梯度评估都需要最少7次最近点评估。因此，这样的一个方法将不得不在3到4次迭代中收敛才能与加速ICP方法竞争。这种通用方法通常需要许多超过 3 次的迭代，即使在理想情况下，所需的最接近点评估的数量也超过 100。如果使用一个单纯的基于数值Hessian的牛顿方法，则数值梯度和Hessian计算需要在每次迭代中至少需要13次最近点评估，这意味着迭代必须在两次迭代中收敛才能与加速 ICP 算法竞争。如果初始点已经很好地在局部极小值的吸引区域，一个单纯的牛顿方法可能只需要3次迭代，但是牛顿的方法不能很好地处理初始迭代。

    每当在三次基本 ICP 步骤之后发生加速抛物线更新时，我们会以低于最速下降成本的方式获得近二次收敛。对于无法计算导数的函数来说，这是一项有趣的成就。注意，最速下降梯度方向不会被有意地计算出来；我们只是在遵循一致的方向时观察。

    使用通用优化方法涉及的其他问题如下：1）如果像【54】中那样使用任意的角度，必须正确处理 360° 的角度循环，且2）如果一个单位四元数变成一个非单位四元数，正如预期的那样在 4 个空间中采取任意方向步骤，四元数必须在一定程度上重新归一化。不幸的是，如果目标函数估计器在优化迭代过程中改变了状态向量中的值，这对大部分非线性优化算法来说会有一个坏的结果。

    总之，任意可以从它的初始值移动到其对应的局部极小值方法理论上都可以被应用到ICP算法中。例如，参考 Szeliski’s【54】的最速下降和三旋转角的工作。然而，上面的论点表明，人们很难找到一种平均只慢十倍的算法。ICP算法关键的优点是收敛很快且单调。没有高成本的最近点评估损失在比当前状态更差的均方误差的配准向量上。由于ICP的收敛定理，使得不必在多维空间中“四处走动”来确定移动的方向。

5. 初始配准集

5.1 全局匹配的初始状态

5.2 反例

5.3 局部形状匹配

6. 实验结果

6.1 点集匹配

6.2 曲线匹配

6.3 表面匹配

6.3.1 贝塞尔曲面块

6.3.2 NRCC 非洲人脸

6.3.3 地形数据

7. 结论

8. 未来方向