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=USigma 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 Sigmaright) \ &=arg max _{R} operatorname{trace}left(R^{prime} Sigmaright) \ &=Vleft(arg max _{R^{prime}} operatorname{trace}left(R^{prime} Sigmaright)right) U^{T} \ &=Vleft(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)

Last modification:December 13th, 2019 at 07:56 pm
恰饭环节