ICP配准算法介绍

ICP算法

ICP（Iterative CLosest Point）迭代最近点算法是一种点云匹配算法。

我们通过RGB-D相机得到一组点云$P = \{p_1, p_2, ..., p_n\}$，相机经过位姿变换后拍摄得到第二组点云$Q=\{q_1, q_2, ..., q_n\}$。这两组点云的坐标对应移动前和移动后以相机为坐标原点的两套坐标系。

需要解决的问题是计算相机的旋转$R$与平移$t$，在无误差的情况下，从$P$转换到$Q$的公式为

$$ q_i = Rp_i + t $$

但是由于噪声以及错误匹配等问题，上式不总是成立，于是最小化目标函数变成了

$$ \frac12 \sum^{n}_{i=1}||q_i - Rp_i - t||^2 $$

常用的求解$R$与$t$的方法有SVD与非线性优化

SVD（奇异值分解）

先定义两组点云的质心：

$$ \boldsymbol{\mu}_{p}=\frac{1}{n} \sum_{i=1}^{n} \mathbf{p}_{i}, \quad \boldsymbol{\mu}_{q}=\frac{1}{n} \sum_{i=1}^{n} \mathbf{q}_{i} $$

对最小化目标函数进行展开：

$$ \begin{aligned} & \frac{1}{2} \sum_{i=1}^{n}\left\|\mathbf{q}_{i}-\mathbf{R}_{\mathbf{p}_{i}}-\mathbf{t}\right\|^{2} \\=& \frac{1}{2} \sum_{i=1}^{n}\left\|\mathbf{q}_{i}-\mathbf{R}_{\mathbf{p}_{i}}-\mathbf{t}-\left(\boldsymbol{\mu}_{q}-\mathbf{R} \boldsymbol{\mu}_{p}\right)+\left(\boldsymbol{\mu}_{q}-\mathbf{R} \boldsymbol{\mu}_{p}\right)\right\|^{2} \\=& \frac{1}{2} \sum_{i=1}^{n}\left\|\left(\mathbf{q}_{i}-\boldsymbol{\mu}_{q}-\mathbf{R}\left(\mathbf{p}_{i}-\boldsymbol{\mu}_{p}\right)\right)+\left(\boldsymbol{\mu}_{q}-\mathbf{R} \boldsymbol{\mu}_{p}-\mathbf{t}\right)\right\|^{2} \\=& \frac{1}{2} \sum_{i=1}^{n}\left(\left\|\mathbf{q}_{i}-\boldsymbol{\mu}_{q}-\mathbf{R}\left(\mathbf{p}_{i}-\boldsymbol{\mu}_{p}\right)\right\|^{2}+\left\|\boldsymbol{\mu}_{q}-\mathbf{R} \boldsymbol{\mu}_{p}-\mathbf{t}\right\|^{2}+2\left(\mathbf{q}_{i}-\boldsymbol{\mu}_{q}-\mathbf{R}\left(\mathbf{p}_{i}-\boldsymbol{\mu}_{p}\right)\right)^{T}\left(\boldsymbol{\mu}_{q}-\mathbf{R} \boldsymbol{\mu}_{p}-\mathbf{t}\right)\right) \end{aligned} $$

将最后一项变形

$$ \begin{aligned} & \sum_{i=1}^{n}\left(\mathbf{q}_{i}-\boldsymbol{\mu}_{q}-\mathbf{R}\left(\mathbf{p}_{i}-\boldsymbol{\mu}_{p}\right)\right)^{T}\left(\boldsymbol{\mu}_{q}-\mathbf{R} \boldsymbol{\mu}_{p}-\mathbf{t}\right) \\=&\left(\boldsymbol{\mu}_{q}-\mathbf{R} \boldsymbol{\mu}_{p}-\mathbf{t}\right)^{T} \sum_{i=1}^{n}\left(\mathbf{q}_{i}-\boldsymbol{\mu}_{q}-\mathbf{R}\left(\mathbf{p}_{i}-\boldsymbol{\mu}_{p}\right)\right) \\=&\left(\boldsymbol{\mu}_{q}-\mathbf{R} \boldsymbol{\mu}_{p}-\mathbf{t}\right)^{T}\left(n \boldsymbol{\mu}_{q}-n \boldsymbol{\mu}_{q}-\mathbf{R}\left(n \boldsymbol{\mu}_{p}-n \boldsymbol{\mu}_{p}\right)\right) \\=& \mathbf{0} \end{aligned} $$

设$$
\mathbf{p}_{i}^{\prime}=\mathbf{p}_{i}-\boldsymbol{\mu}_{p}, \mathbf{q}_{i}^{\prime}=\mathbf{q}_{i}-\boldsymbol{\mu}_{q}
$$，目标函数可以简化为：

$$ \frac{1}{2} \sum_{i=1}^{n}\left(\left\|\mathbf{q}_{i}^{\prime}-\mathbf{R} \mathbf{p}_{i}^{\prime}\right\|^{2}+\left\|\boldsymbol{\mu}_{q}-\mathbf{R} \boldsymbol{\mu}_{p}-\mathbf{t}\right\|^{2}\right) $$

最优解就变成了寻找最优解$R^*$与$t^*$

$$ \begin{array}{l}{\text { 1. } \mathbf{R}^{*}=\underset{\mathbf{R}}{\operatorname{argmin}} \frac{1}{2} \sum_{i=1}^{n}\left\|\mathbf{q}_{i}^{\prime}-\mathbf{R} \mathbf{p}_{i}^{\prime}\right\|^{2}} \\ {\text { 2. } \mathbf{t}^{*}=\boldsymbol{\mu}_{q}-\mathbf{R} \boldsymbol{\mu}_{p}}\end{array} $$

求$R^*$的最优解过程如下

$$ \begin{aligned} R^{*} &=\underset{R}{\arg \min } \sum_{i=1}^{n}\left\|y_{i}-y_{o}-R\left(x_{i}-x_{o}\right)\right\|^{2} \\ &=\underset{R}{\arg \min } \sum_{i=1}^{n}\left\|y_{i}^{\prime}-R x_{i}^{\prime}\right\|^{2} \\ &=\underset{R}{\arg \min } \sum_{i=1}^{n}\left(y_{i}^{T} y_{i}^{\prime}+x_{i}^{T} R^{T} R x_{i}^{\prime}-2 y_{i}^{T} R x_{i}^{\prime}\right) \\ &=\underset{R}{\arg \min } \sum_{i=1}^{n}\left(-2 y_{i}^{T} R x_{i}^{\prime}\right) \\ &=\underset{R}{\arg \max } \sum_{i=1}^{n}\left(y_{i}^{T} R x_{i}^{\prime}\right) \end{aligned} $$

下面用到了迹（Trace）的性质$trace(AB)=trace(BA)$，并令$$
H=\sum_{i=1}^{n} x_{i}^{\prime} y_{i}^{T}

$$ $$

\begin{array}{l}{=\underset{R}{\arg \max } \sum_{i=1}^{n}\left(\operatorname{trace}\left(y_{i}^{T} R x_{i}^{\prime}\right)\right)} \ {=\underset{R}{\arg \max } \sum_{i=1}^{n}\left(\operatorname{trace}\left(R x_{i}^{\prime} y_{i}^{\prime T}\right)\right)} \ {=\underset{R}{\arg \max } \operatorname{trace}\left(\sum_{i=1}^{n} R x_{i}^{\prime} y_{i}^{\prime T}\right)} \ {=\underset{R}{\arg \max } \operatorname{trace}\left(R \sum_{i=1}^{n} x_{i}^{\prime} y_{i}^{\prime T}\right)} \ {=\underset{R}{\arg \max } \operatorname{trace}(R H)} \ {=\underset{R}{\arg \max } \operatorname{trace}(R H)}\end{array}

$$ 然后对$H$进行SVD分解 $$

H=U\Sigma V^T

$$ 其中$U$与$V$都是正交矩阵，$$ \Sigma=\left[\begin{array}{ccc}{\sigma_{1}} & {0} & {0} \\ {0} & {\sigma_{2}} & {0} \\ {0} & {0} & {\sigma_{3}}\end{array}\right]$$，$\sigma_1,\sigma_2,\sigma_3$为$H$的三个奇异值。继续推导误差函数，令$R'=V^TRU$，求出$R'$的argmax后再变换回$R$。 $$

\begin{aligned} R^{*} &=\underset{R}{\arg \max } \operatorname{trace}(R H) \ &=\underset{R}{\arg \max } \operatorname{trace}\left(R U \Sigma V^{T}\right) \ &=\arg \max _{R} \operatorname{trace}\left(V^{T} R U \Sigma\right) \ &=\arg \max _{R} \operatorname{trace}\left(R^{\prime} \Sigma\right) \ &=V\left(\arg \max _{R^{\prime}} \operatorname{trace}\left(R^{\prime} \Sigma\right)\right) U^{T} \ &=V\left(\arg \max _{R^{\prime}} r_{11}^{\prime} \sigma_{1}+r_{22}^{\prime} \sigma_{2}+r_{33}^{\prime} \sigma_{3}\right) U^{T} \ &=V U^{T} \end{aligned}

$$ 其中 $$

R^{\prime}=\left[\begin{array}{ccc}{r_{11}^{\prime}} & {r_{12}^{\prime}} & {r_{13}^{\prime}} \ {r_{21}^{\prime}} & {r_{22}^{\prime}} & {r_{23}^{\prime}} \ {r_{31}^{\prime}} & {r_{32}^{\prime}} & {r_{33}^{\prime}}\end{array}\right]

$$ 因为$R,U,V$都是正交矩阵，所以$R'$也是正交矩阵。由正交矩阵的性质可知$-1<r'_{11},r'_{22},r'_{33}\leq1$。又因为奇异值$\sigma_1,\sigma_2,\sigma_3 $大于0（这是一个重要性质）。所以要使得$r_{11}^{\prime} \sigma_{1}+r_{22}^{\prime} \sigma_{2}+r_{33}^{\prime} \sigma_{3}$最大，就要使得每一项都最大。当$r'_{11}=r'_{22}=r'_{33}=1$时，上式最大，且最大值为$ \sigma_{1}+\sigma_{2}+\sigma_{3}$。得到$R'=I$。然后得到$R$。最后得到$R$后要进行进一步的验证，验证是否$|R|=1$。如果$|R|=-1$那么$R$代表镜像对称，求解失败。出现这种现象的原因一般为点云共面、共线或者外点噪声过大导致的。 ## 参考资料 [Least-Squares Fitting of Two 3-D Point Sets](https://link.zhihu.com/?target=http%3A//www.math.pku.edu.cn/teachers/yaoy/Fall2011/arun.pdf) K. S. Arun, T. S. Huang, and S. D. Blostein [SVD求解3D-3D位姿估计](https://zhuanlan.zhihu.com/p/51362089) [迭代最近点（Iterative Closest Point, ICP）算法介绍](https://zhuanlan.zhihu.com/p/35893884)

ICP配准算法介绍