一、ICP算法简介

ICP算法可以用于匹配两个点云，得到两个点云之间的位姿（位移矩阵T和旋转矩阵R）转换关系。

二、ICP算法推导

对于两组点云： $$\begin{gathered} X=x_1,x_2,...x_n \\ P=p_1,p_2,...p_n \end{gathered}$$ 求解位移矩阵T和旋转矩阵R，使得下式最小： $$E(R,T)=\frac{1}{N}\begin{array}{c}{\sum }_{i=1}^N||x_i-R{p}_i-T|{|}^2\end{array}(1)$$ 计算点云质心 $$\begin{gathered} u_x=\frac{1}{N_x}\begin{array}{c}{\sum }_{i=1}^{N_x}{x}_i\end{array} \\ {u}_p=\frac{1}{N_p}\begin{array}{c}{\sum }_{i=1}^{N_p}{p}_i\end{array} \end{gathered}$$ 对式(1)进行变换： $$E(R,T)=\frac{1}{N}\begin{array}{c}{\sum }_{i=1}^N||x_i-u_x-R(p_i-u_p)+u_x-R{u}_p-T|{|}^2\end{array}(2)$$ 展开得到： $$E(R,T)=\frac{1}{N}\begin{array}{c}{\sum }_{i=1}^N||x_i-u_x-R(p_i-u_p)|{|}^2+||u_x-R{u}_p-T|{|}^2+2(x_i-u_x-R(p_i-u_p))(u_x-R{u}_p-T)\end{array}(3)$$ 式子(3)的第三项： $$\begin{gathered} \begin{array}{c}{\sum }_{i=1}^N(x_i-u_x-R(p_i-u_p))(u_x-R{u}_p-T)\end{array} \\ =(u_x-R{u}_p-T)\begin{array}{c}{\sum }_{i=1}^N(x_i-u_x-R(p_i-u_p))\end{array} \\ =(u_x-R{u}_p-T)(\begin{array}{c}{\sum }_{i=1}^N{x}_i\end{array}-N{u}_x-R(\begin{array}{c}{\sum }_{i=1}^N{p}_i\end{array}-N{u}_p)) \\ =0 \end{gathered}$$ 因此式(3)简化为： $$E(R,T)=\frac{1}{N}\begin{array}{c}{\sum }_{i=1}^N||x_i-u_x-R(p_i-u_p)|{|}^2+||u_x-R{u}_p-T|{|}^2\end{array}(4)$$ 平移矩阵T只与式(4)第二项有关，可令： $$\frac{1}{N}\begin{array}{c}{\sum }_{i=1}^N||u_x-R{u}_p-T|{|}^2\end{array}=0$$ 则可以计算出平移矩阵T： $$T=u_x-R{u}_p$$ 则式(4)可以进一步简化： $$\begin{gathered} E(R,T)=\frac{1}{N}\begin{array}{c}{\sum }_{i=1}^N||x_i-u_x-R(p_i-u_p)|{|}^2\end{array} \\ =\frac{1}{N}\begin{array}{c}{\sum }_{i=1}^N||x_i-R{p}_i|{|}^2\end{array} \\ =\frac{1}{N}\begin{array}{c}{\sum }_{i=1}^N{x_i}^T{x}_i+{p_i}^T{R}^{T}R{p}_i-2{x_i}^{T}R{p}_i\end{array} \end{gathered}$$ 第一项为常数；因为旋转矩阵R为正交矩阵，所以RTR为单位矩阵，则第二项也是常数。则E的最小值只与第三项有关。 则最后简化为取下式的最大值： $$\begin{array}{c}{\sum }_{i=1}^N{x_i}^{T}R{p}_i\end{array}=\begin{array}{c}{\sum }_{i=1}^{N}Trace(R{x}_i{p_i}^T)\end{array}=Trace(RH)(5)$$ 其中： $$H=\begin{array}{c}{\sum }_{i=1}^N{x}_i{p_i}^T\end{array}$$ 定理： 若A为正定对称矩阵，则对于任意的正交矩阵B，都有： $$Trace(A)\ge Trace(BA)$$ 因此需要构建正定对称矩阵，对H进行SVD分解得到： $$H=U\Lambda V^T$$ 令一个变量X为： $$X=V{U}^T$$

则X是正交矩阵，容易证明： $$X{X}^T=V{U}^T(V{U}^T{)}^T=V{U}^{T}U{T}^T=E$$

得到： $$XH=V{U}^{T}U\Lambda V^T=V\Lambda V^T$$ 则XH为正定矩阵，因此可以应用定理得到： $$Trace(XH)\ge Trace(BXH)$$ 可知，H乘以X时式(5)取最大值，所以： $$R=X=V{U}^T$$

三、举例验证

由上述推导可得到结论： $$\begin{gathered} R=V{U}^T \\ T=u_x-R{u}_p \end{gathered}$$ 其中V和U由H的SVD分解得到。 下面我举一个简单的例子进行验证： 对于两组点云： $$\begin{gathered} X=x_1(4,1),x_2(6,3),x_3(5,5) \\ P=p_1(0,3),p_2(-2,5),p_3(-4,4) \end{gathered}$$ 各自质心： $$\begin{gathered} u_x=(5,3) \\ {u}_p=(-2,4) \end{gathered}$$ 则H为： $$H=\begin{array}{c}{\sum }_{i=1}^N{x}_i{p_i}^T\end{array}=\left[\begin{array}{cc}-2& 2\\ -8& 2\end{array}\right]$$ 对H进行SVD分解： $$H=U\Lambda V^T=\left[\begin{array}{cc}-0.2898& -0.9571\\ -0.9571& 0.2898\end{array}\right]\left[\begin{array}{cc}8.6056& 0\\ 0& 1.3944\end{array}\right]\left[\begin{array}{cc}0.9571& -0.2898\\ -0.2898& -0.9571\end{array}\right]$$ 则可以求出旋转矩阵R和平移矩阵T： $$\begin{gathered} R=\left[\begin{array}{cc}0& 1\\ -1& 0\end{array}\right] \\ T=\left[\begin{array}{c}1\\ 1\end{array}\right] \end{gathered}$$