ICP算法概述以及使用SVD进行算法推导

本文转载于B站UP主
摆烂世家的视频ICP算法概述及使用SVD推导（组会录像）
作者：苏浩田
（注：如有侵权，私信我下立马删咯）

1 ICP 算法

  三维点云拼接技术在不同场合亦被称为重定位、配准或拼合技术，其实质是把不同的坐标系下测得的数据点云进行坐标变换，问题的关键是坐标变换参数（旋转矩阵）和（平移矢量）的求取。目前国内外的拼接技术一般分两步：粗拼接和精确拼接。
  粗拼接大致将不同坐标系下点云对准到同一坐标系下，一般粗拼接很难满足精度要求，需在粗拼接的基础上使用迭代算法进行精确拼接，使点云之间的拼接误差达到最小。
  目前国内外最常用的精确拼接方法为迭代最近点（Iterative Closest Point, ICP）算法，本文基于目前ICP及其改进算法的发展现状，将其分为４个主要阶段：

（１）对原始点云数据进行采样；

（２）确定初始对应点集；

（３）去除错误对应点对；

（４）坐标变换的求解。

1.1 ICP 算法的问题概述

ICP 算法对待拼接的 2 片点云, 首先根据一定的准则确立对应点集$P=\{p_1,p_2,...,p_n\}$与$Q=\{q_1,q_2,...,q_n\}$,其中对应点对的个数为n。然后通过最小二乘法迭代计算最优的坐标变换, 即旋转矩阵和平移矢量，即问题可描述为
$$\begin{array}{c}(R,\mathbf{t})=\underset{R\in SO(d),\mathbf{t}\in {\mathbb{R}}^d}{\mathrm{argmin}}\sum _{i=1}^n{w}_i{\Vert R{\mathbf{p}}_i+\mathbf{t}-{\mathbf{q}}_i\Vert }^2\end{array}$$

ICP 算法计算简便直观且可使拼接具有较好的精度, 但是算法的运行速度以及向全局最优的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。各种粗拼接技术可为 ICP 算法提供较好的初始位置, 所以迭代过程中确立正确对应点集以避免迭代陷入局部极值成为各种改进算法的关键, 决定了算法的收敛速度与最终的拼接精度。

图 1 ICP算法示意图

1.2 ICP 算法步骤

1.2.1 对原始点云数据进行采样

  目前常用的采样方法有均匀采样、随机采样和法矢采样等。其中均匀采样与随机采样简便易行, 但由于未考虑待拼接物体的形貌等因素, 采样时可能会丢失物体的关键特征, 故限制了其应用场合。法矢采样以待拼物体表面点的法矢分布为基础, 使每个法矢方向上都有差不多个数的点, 此方法可尽可能地保留点云的细微特征。

1.2.2 确定初始对应点集

  根据确立对应点集的方法，可将ICP及其各种改进算法分为 3 类：点到点(point-to-point)、点到投影(point-to-projection)和点到面(point-to-surface)。

（ 1 ）点到点

  此方法通过直接搜索一片点云中的点在另一片点云中的最近点来确定对应点集。这种以最近点为标准的方法虽然计算简便直观,但其所确立的对应点集中存在大量错误对应点对,算法迭代易陷入局部极值。

（ 2 ）点到投影

  对于点云上一点 ,其对应点为过点云的视点方向$O_Q$在点云上的投影。由于省去了搜索对应点的步骤,此方法可极大地缩短计算时间,但其缺点是无法达到较高的拼接精度。

（ 3 ）点到平面

  点到面算法在三类方法中精度最高,这种方法将点点距离用点面距离代替,迭代次数少,且不易陷入局部极值。应用最为广泛的点到面算法为点到切平面算法。

1.2.3 去除错误对应点对

  采用上述方法确立的对应点集中会存在错误对应点对(或称为不可靠、不兼容点对),这种情况在算法迭代初期尤为明显。严格来说,使用ICP算法所确立的对应点集中可能没有一对是真正的“正确”点对,为此,需要引入 1 种评价标准或约束以去除错误对应点对,这是许多ICP改进算法的关键,也是国内外诸多学者研究的重点。

（ 1 ）基于曲面几何特征的约束方法

  此类方法基于的原理是:在不同视角下,被测物体同一位置的曲面几何特征应该保持不变。此类约束中常用的几何特征有曲率、法矢等。使用此类约束方法确立的对应点对一般正确率较高,故可达到较高的精度,但此类方法要求被测物体有较明显的特征,且由于存在提取曲面特征的过程,一般计算较为耗时,特别是点云点数较多时。

代表算法有：

• ICCP(Iterative Closest Compatible Point) 此算法中使用候选对应点间的曲率和法矢的夹角判断点对的兼容性。
• ICPIF (Iterative Closest Points using Invariant Features) 此算法中使用被测物体的欧氏空间不变量(曲率、矩不变量、球谐函数不变量等) 确立对应点对。
• GP-ICPR( Geometric Primitive ICP with the RANSAC) 此算法中利用曲面上 1 点的曲率变化与对应点间的法矢的夹角确立对应点对。
• ICL( Iterative Closest Line)、ICT( Iterative Closest Triangle Patch)…

（ 2 ）基于刚性运动一致性的约束方法

  此类方法直接利用了刚性运动应保持被测物体重叠区域对应点不变的原理。此类约束常用的一致性准则有对应点间的距离、对应点对间的相邻关系等。此类方法简便易行,计算效率较高,但确立的对应点集错误率较高。

代表算法有：

• ICRP(Iterative Closest Reciprocal Point) 对于点云上 1 点,其在上的最近点为,找到在上的最近点′,若‖−′‖>(为给定的阈值),则点对(,)被去除。

• PICP(Picky ICP) 此算法对使用最近点对点方法找到的对应点集中的点对(,) ,若‖−‖>,则点对(,)被去除。其中为所有对应点对间距离的均值,为给定的阈值。

• …

1.2.4 坐标变换的求解

  ICP 算法对最终确立的对应点集,一般采用最小二乘法迭代求解 2 片点云间的最优坐标变换。对于旋转矩阵和平移矢量的求值,通常可采用以下 4 种方法:单位四元数法、奇异值分解法(SVD)、正交矩阵法和对偶四元数法。

1 .2.5 算法具体步骤

  步骤 1. 点云预处理，即第一阶段对原始点云数据进行采样；

  步骤 2. 确定初始对应点集、；

  步骤 3. 去除不合理的对应点对；

  步骤 4. 坐标变换的求解。即计算平移向量和旋转矩阵，获得点集计算旋转后的点集′。通过与′计算平均距离平方。以平均距离平方之差绝对值作为迭代判断数值；若该数值小于设定阈值，则算法结束。否则，返回步骤 2 。

图 2 ICP算法流程图

2 使用 SVD 方法求解 ICP 问题

2.1 问题回顾

  我们通过 1.2.2和 1.2.3获得了对应点集其中$P=\{p_1,p_2,...,p_n\}$与$Q=\{q_1,q_2,...,q_n\}$,其中对应点对的个数为 n。然后通过最小二乘法迭代计算最优的坐标变换, 即旋转矩阵和平移矢量，即问题可描述为
$$\begin{array}{c}(R,\mathbf{t})=\underset{R\in SO(d),\mathbf{t}\in {\mathbb{R}}^d}{\mathrm{argmin}}\sum _{i=1}^n{w}_i{\Vert R{\mathbf{p}}_i+\mathbf{t}-{\mathbf{q}}_i\Vert }^2\end{array}\text{\hspace{1em}(1)}$$
  其中$w_i$代表每个点的权重。和是我们所要求的旋转矩阵和平移向量。

2.2 计算平移

  我们分两步求解旋转和平移，首先求解。固定，我们要优化的误差函数（目标函数）如下：
$$\begin{array}{c}F(t)=\sum _{i=1}^n{w}_i{\Vert \left(R{\mathbf{p}}_i+\mathbf{t}\right)-{\mathbf{q}}_i\Vert }^2\end{array}$$
  为求最小值我们可以对求偏导得：
$$\begin{array}{c}\frac{\partial F}{\partial \mathbf{t}}=\sum _{i=1}^{n}2w_i\left(R{\mathbf{p}}_i+\mathbf{t}-{\mathbf{q}}_i\right)=2\mathbf{t}\left(\sum _{i=1}^n{w}_i\right)+2R\left(\sum _{i=1}^n{w}_i{\mathbf{p}}_i\right)-2\left(\sum _{i=1}^n{w}_i{\mathbf{q}}_i\right)=0\end{array}\text{\hspace{1em}(2)}$$
  我们记：
$$\begin{array}{c}\overline{\mathbf{p}}=\frac{\sum _{i=1}^n{w}_i{\mathbf{p}}_i}{\sum _{i=1}^n{w}_i},\overline{\mathbf{q}}=\frac{\sum _{i=1}^n{w}_i{\mathbf{q}}_i}{\sum _{i=1}^n{w}_i}\end{array}\text{\hspace{1em}(3)}$$
  也就是加权平均的质心，将其带回(2)式中得：
$$\begin{array}{c}\mathbf{t}=\overline{\mathbf{q}}-R\overline{\mathbf{p}}\end{array}\text{\hspace{1em}(4)}$$
  这样我们可以知道，平移向量可由加权后的质心得到。我们再将带回到目标函数中得：
$$\begin{array}{rl}cF(t)& =\sum _{i=1}^n{w}_i{\Vert \left(R{\mathbf{p}}_i+\mathbf{t}\right)-{\mathbf{q}}_i\Vert }^2=\sum _{i=1}^n{w}_i{\Vert R{\mathbf{p}}_i+\overline{\mathbf{q}}-R\overline{\mathbf{p}}-{\mathbf{q}}_i\Vert }^2=\\ & =\sum _{i=1}^n{w}_i{\Vert R\left({\mathbf{p}}_i-\overline{\mathbf{p}}\right)-\left({\mathbf{q}}_i-\overline{\mathbf{q}}\right)\Vert }^2\end{array}\text{\hspace{1em}(5)}$$
  再记：
$$\begin{array}{r}{\mathbf{x}}_i:={\mathbf{p}}_i-\overline{\mathbf{p}},{\mathbf{y}}_i:={\mathbf{q}}_i-\overline{\mathbf{q}}\end{array}\text{\hspace{1em}(6)}$$
  我们可以直接求质心后对每个点进行归一化再求解。有了之后，我们要求解的如下：
$$\begin{array}{c}R=\underset{R\in \mathrm{SO}(d)}{\mathrm{argmin}}\sum _{i=1}^n{w}_i{\Vert R{\mathbf{x}}_i-{\mathbf{y}}_i\Vert }^2\end{array}\text{\hspace{1em}(7)}$$

2.3 计算旋转

  首先简化下(7)式，先看权重后面的部分，则公式推导如下：
$$\begin{array}{rl}{\Vert R{\mathbf{x}}_i-{\mathbf{y}}_i\Vert }^2& ={\left(R{\mathbf{x}}_i-{\mathbf{y}}_i\right)}^T\left(R{\mathbf{x}}_i-{\mathbf{y}}_i\right)=\left({\mathbf{x}}_i^T{R}^T-{\mathbf{y}}_i^T\right)\left(R{\mathbf{x}}_i-{\mathbf{y}}_i\right)=\\ & ={\mathbf{x}}_i^T{R}^{T}R{\mathbf{x}}_i-{\mathbf{y}}_i^{T}R{\mathbf{x}}_i-{\mathbf{x}}_i^T{R}^T{\mathbf{y}}_i+{\mathbf{y}}_i^T{\mathbf{y}}_i=\\ & ={\mathbf{x}}_i^T{\mathbf{x}}_i-{\mathbf{y}}_i^{T}R{\mathbf{x}}_i-{\mathbf{x}}_i^T{R}^T{\mathbf{y}}_i+{\mathbf{y}}_i^T{\mathbf{y}}_i\end{array}\text{\hspace{1em}(8)}$$
  其次，由于$x_i^T{R}^T{y}_i$是一个标量，对于标量，我们有性质$a=a^T$因此有：
$$\begin{array}{rlr}{\mathbf{x}}_i{}^T{R}^T{\mathbf{y}}_i& ={\left({\mathbf{x}}_i{}^T{R}^T{\mathbf{y}}_i\right)}^T& ={\mathbf{y}}_i{}^{T}R{\mathbf{x}}_i\end{array}\text{\hspace{1em}(9)}$$
  因次，我们可以继续将(8)式简化：
$${\Vert R{\mathbf{x}}_i-{\mathbf{y}}_i\Vert }^2={\mathbf{x}}_i^T{\mathbf{x}}_i-2{\mathbf{y}}_i^{T}R{\mathbf{x}}_i+{\mathbf{y}}_i^T{\mathbf{y}}_i\text{\hspace{1em}(10)}$$
  代回(7)式得：
$$\begin{array}{l}R=\underset{R\in SO(d)}{\mathrm{argmin}}{\sum }_{i=1}^n{w}_i{\Vert R{\mathbf{x}}_i-{\mathbf{y}}_i\Vert }^2=\underset{R\in SO(d)}{\mathrm{argmin}}{\sum }_{i=1}^n{w}_i\left({\mathbf{x}}_i{}^T{\mathbf{x}}_i-2{\mathbf{y}}_i{}^{T}R{\mathbf{x}}_i+{\mathbf{y}}_i{}^T{\mathbf{y}}_i\right)=\\ =\underset{R\in SO(d)}{\mathrm{argmin}}\left({\sum }_{i=1}^n{w}_i{\mathbf{x}}_i^T{\mathbf{x}}_i-2{\sum }_{i=1}^n{w}_i{\mathbf{y}}_i{}^{T}R{\mathbf{x}}_i+{\sum }_{i=1}^n{w}_i{\mathbf{y}}_i{}^T{\mathbf{y}}_i\right)=\\ =\underset{R\in SO(d)}{\mathrm{argmin}}\left(-2{\sum }_{i=1}^n{w}_i{\mathbf{y}}_i{}^{T}R{\mathbf{x}}_i\right)(11)\\ =\underset{R\in \mathrm{So}(d)}{\mathrm{argmax}}{\sum }_{i=1}^n{w}_i{\mathbf{y}}_i^{T}R{\mathbf{x}}_i(12)\end{array}$$
  根据矩阵迹的性质，我们有以下结论：
$$\sum _{i=1}^n{w}_i{\mathbf{y}}_i{}^{T}R{\mathbf{x}}_i=\mathrm{tr}\left(W{Y}^{T}RX\right)\text{\hspace{1em}(13)}$$
  其中 W = ( w 1 w 2 ⋱ w n ) , Y T = ( − y 1 T − − y 2 T − ⋮ − y n T − ) , X = ( ∣ ∣ ∣ x 1 x 2 ⋯ x n ∣ ∣ ∣ ) W=\left(\begin{array}{cccc} w_{1} & & & \\ & w_{2} & & \\ & & \ddots & \\ & & & w_{n} \end{array}\right), Y^{T}=\left(\begin{array}{c} -\boldsymbol{y}_{1}^{T}- \\ -\boldsymbol{y}_{2}^{T}- \\ \vdots \\ -\boldsymbol{y}_{n}^{T}- \end{array}\right), X=\left(\begin{array}{cccc} \mid & \mid & & \mid \\ \boldsymbol{x}_{1} & \boldsymbol{x}_{2} & \cdots & \boldsymbol{x}_{n} \\ \mid & \mid & \mid \end{array}\right) W= ​w1​​w2​​⋱​wn​​ ​,YT= ​−y1T​−−y2T​−⋮−ynT​−​ ​,X= ​∣x1​∣​∣x2​∣​⋯∣​∣xn​​ ​
  推导：
W Y T R X = ( w 1 w 2 ⋱ w n ) ( − y 1 T − − y 2 T − ⋮ − y n T − ) ( R ) ( ∣ ∣ ∣ x 1 x 2 ⋯ x n ∣ ∣ ∣ ) = W Y^{T} R X=\left(\begin{array}{cccc} w_{1} & & & \\ & w_{2} & & \\ & & \ddots & \\ & & & w_{n} \end{array}\right)\left(\begin{array}{c} -\boldsymbol{y}_{1}^{T}- \\ -\boldsymbol{y}_{2}^{T}- \\ \vdots \\ -\boldsymbol{y}_{n}^{T}- \end{array}\right)(R)\left(\begin{array}{cccc} \mid & \mid & & \mid \\ \boldsymbol{x}_{1} & \boldsymbol{x}_{2} & \cdots & \boldsymbol{x}_{n} \\ \mid & \mid & & \mid \end{array}\right)= WYTRX= ​w1​​w2​​⋱​wn​​ ​ ​−y1T​−−y2T​−⋮−ynT​−​ ​(R) ​∣x1​∣​∣x2​∣​⋯​∣xn​∣​ ​=
= ( − w 1 y 1 T − − w 2 y 2 T − ⋮ − w n y n T − ) ( ∣ ∣ ∣ R x 1 R x 2 ⋯ R x n ∣ ∣ ∣ ) = = ( w 1 y 1 T R x 1 w 2 y 2 T R x 2 ⋱ w n y n T R x n ) \begin{array}{l} =\left(\begin{array}{c} -w_{1} \boldsymbol{y}_{1}^{T}- \\ -w_{2} \boldsymbol{y}_{2}^{T}- \\ \vdots \\ -w_{n} \boldsymbol{y}_{n}^{T}- \end{array}\right)\left(\begin{array}{cccc} \mid & \mid & & \mid \\ R \boldsymbol{x}_{1} & R \boldsymbol{x}_{2} & \cdots & R \boldsymbol{x}_{n} \\ \mid & \mid & & \mid \end{array}\right)= \\ =\left(\begin{array}{llll} w_{1} \boldsymbol{y}_{1}{ }^{T} R \boldsymbol{x}_{1} & & \\ & w_{2} \boldsymbol{y}_{2}^{T} R \boldsymbol{x}_{2} & & \\ & & \ddots & \\ & & w_{n} \boldsymbol{y}_{n}{ }^{T} R \boldsymbol{x}_{n} \end{array}\right) \end{array} = ​−w1​y1T​−−w2​y2T​−⋮−wn​ynT​−​ ​ ​∣Rx1​∣​∣Rx2​∣​⋯​∣Rxn​∣​ ​== ​w1​y1​TRx1​​w2​y2T​Rx2​​⋱wn​yn​TRxn​​​ ​​
  所以
$$\mathrm{tr}\left(W{Y}^{T}RX\right)=\sum _{i=1}^n{w}_i{\mathbf{y}}_i^{T}R{\mathbf{x}}_i$$
  推导完毕。

  这里之所以表示成迹的形式，是因为矩阵的迹有许多性质可以利用。我们利用迹的对称性，有：

$$\mathrm{tr}\left(W{Y}^{T}RX\right)=\mathrm{tr}\left(\left(W{Y}^T\right)(RX)\right)=\mathrm{tr}\left(RXW{Y}^T\right)\text{\hspace{1em}(14)}$$
  定义协方差矩阵$S=XW{Y}^T$，对进行SVD分解：
$$S=U\Sigma V^T\text{\hspace{1em}(15)}$$
  将上述式子代入(14)，再利用迹的对称性得：
$$\mathrm{tr}\left(RXW{Y}^T\right)=\mathrm{tr}(RS)=\mathrm{tr}\left(RU\Sigma V^T\right)=\mathrm{tr}\left(\Sigma V^{T}RU\right)\text{\hspace{1em}(16)}$$
  因为其中、和均为正交矩阵。我们令$M=V^{T}RU$，因此$M=V^{T}RU$也是正交矩阵。根据正交矩阵定义，其行、列向量均为正交的单位向量，因此对于矩阵$M$中每个列向量$m_j$均有$m_j^T{m}_j=1$，由此可推出中的每一个元素的绝对值均有$|m_{ij}|\le 1$ 。推导如下
$$1={\mathbf{m}}_j^T{\mathbf{m}}_j=\sum _{i=1,j=1}^d{m}_{ij}^2\Rightarrow m_{ij}^2\le 1\Rightarrow \left|m_{ij}\right|\le 1$$
  根据SVD分解的性质 Σ = ( σ 1 σ 2 ⋱ σ d ) \Sigma=\left(\begin{array}{llll} \sigma_{1} & & & \\ & \sigma_{2} & & \\ & & \ddots & \\ & & & \sigma_{d} \end{array}\right) Σ= ​σ1​​σ2​​⋱​σd​​ ​，其中对角线元素1, 2, ⋯ , ≥ 0 ，因此有：
tr ⁡ ( Σ V T R U ) = tr ⁡ ( Σ M ) = tr ⁡ ( ( σ 1 σ 2 ⋱ σ d ) ( m 11 m 12 ⋯ m 1 d m 21 m 22 ⋯ m 2 d ⋮ ⋮ ⋱ ⋮ m d 1 m d 2 ⋯ m d d ) ) = ∑ i = 1 d σ i m i i ≤ ∑ i = 1 d σ i (17) \begin{aligned} \operatorname{tr}\left(\Sigma V^{T} R U\right) & =\operatorname{tr}(\Sigma M)=\operatorname{tr}\left(\left(\begin{array}{cccc} \sigma_{1} & & & \\ & \sigma_{2} & & \\ & & \ddots & \\ & & \sigma_{d} \end{array}\right)\left(\begin{array}{cccc} m_{11} & m_{12} & \cdots & m_{1 d} \\ m_{21} & m_{22} & \cdots & m_{2 d} \\ \vdots & \vdots & \ddots & \vdots \\ m_{d 1} & m_{d 2} & \cdots & m_{d d} \end{array}\right)\right) \\ & =\sum_{i=1}^{d} \sigma_{i} m_{i i} \leq \sum_{i=1}^{d} \sigma_{i} \end{aligned}\tag{17} tr(ΣVTRU)​=tr(ΣM)=tr ​ ​σ1​​σ2​​⋱σd​​​ ​ ​m11​m21​⋮md1​​m12​m22​⋮md2​​⋯⋯⋱⋯​m1d​m2d​⋮mdd​​ ​ ​=i=1∑d​σi​mii​≤i=1∑d​σi​​(17)
  上式等号成立也就是取得最大值的唯一条件就是每一个$m_{ii}=1$，由于$M$为正交矩阵，其行列向量均为正交的单位向量，因次$M$只能是单位矩阵，即$M=I$，得：
$$I=M=V^{T}RU\Rightarrow V=RU\Rightarrow R=V{U}^T\text{\hspace{1em}(18)}$$
  这就是ICP算法中求解计算$R=VU$的计算过程