点云处理-ICP算法推导

一、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||{\hat{x}}_i-R{\hat{p}}_i|{|}^2\end{array} \\ =\frac{1}{N}\begin{array}{c}{\sum }_{i=1}^N{{\hat{x}}_i}^T{\hat{x}}_i+{{\hat{p}}_i}^T{R}^{T}R{\hat{p}}_i-2{{\hat{x}}_i}^{T}R{\hat{p}}_i\end{array} \end{gathered}$$
第一项为常数；因为旋转矩阵R为正交矩阵，所以RTR为单位矩阵，则第二项也是常数。则E的最小值只与第三项有关。
则最后简化为取下式的最大值：
$$\begin{array}{c}{\sum }_{i=1}^N{{\hat{x}}_i}^{T}R{\hat{p}}_i\end{array}=\begin{array}{c}{\sum }_{i=1}^{N}Trace(R{\hat{x}}_i{{\hat{p}}_i}^T)\end{array}=Trace(RH)(5)$$
其中：
$$H=\begin{array}{c}{\sum }_{i=1}^N{\hat{x}}_i{{\hat{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{\hat{x}}_i{{\hat{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}$$