三维点云处理之点云配准

1. ICP

（1）定义

​ 对于两个点云source和target，求source经位姿变换后得到的点云与target尽可能重合的旋转$R$与平移$t$。由于实际中source和target两个点云中的点不一定完全对应，因此取二者能够相互关联的那些点（公共点）来参与位姿计算：
X = { x 1 , x 2 , ⋯   , x N x }            Y = { y 1 , y 2 , ⋯   , y N y }        N x = N y X = \{ x_1,x_2,\cdots,x_{N_x} \} \ \ \ \ \ \ \ \ \ \ Y = \{ y_1, y_2, \cdots, y_{N_y} \} \ \ \ \ \ \ N_x=N_y X={ x1​,x2​,⋯,xNx​​}          Y={ y1​,y2​,⋯,yNy​​}      Nx​=Ny​
目标：通过最小化损失函数计算旋转矩阵$R$和平移向量$t$
$$minE(R,t)=min\frac{1}{N_y}\sum _{i=1}^{N_y}||x_i-R{y}_i-t|{|}^2$$
流程

​ 获取两个点云之间的关联点一般是以二者距离最近的两个点为相互关联点，但若两初始点云相距较远，此时通过距离最近找到的关联点不一定是真正的关联点。在具有较好的初始位姿前提下，可通过迭代的方式，将当前迭代中变换后的点作为下一迭代的起始点，直到两关联点云达到预期的尽可能重合状态（较好的初始位姿是为了使经过迭代后两点云相互靠近，否则即使迭代多次后两点云仍不会达到预期重合状态）。其流程如下所示：

（2）ICP公式推导

​ 根据定义可知，目标函数如下：
$$\begin{gathered} E(R,t)=\frac{1}{N_y}\sum _{i=1}^{N_y}||x_i-R{y}_i-t-u_x+R{u}_y+u_x-R{u}_y|{|}^2 \\ =\frac{1}{N_y}\sum _{i=1}^{N_y}(||x_i-u_x-R(y_i-u_y)+(u_x-R{u}_y-t)|{|}^2) \\ =\frac{1}{N_y}\sum _{i=1}^{N_y}(||x_i-u_x-R(y_i-u_y)|{|}^2+||u_x-R{u}_y-t|{|}^2 \\ +2(x_i-u_x-R(y_i-u_y){)}^T(u_x-R{u}_y-t)) \\ =\frac{1}{N_y}\sum _{i=1}^{N_y}(||x_i-u_x-R(y_i-u_y)|{|}^2+||u_x-R{u}_y-t|{|}^2) \end{gathered}$$
​ 其中，$u_x$和$u_y$分别是点云X和Y的质心，即：
$$u_x=\frac{1}{N_x}\sum _{i=1}^{N_x}{x}_i{u}_y=\frac{1}{N_y}\sum _{i=1}^{N_y}{y}_i$$
​ 上述推导中去掉两点云的中心，其目的是： 中心到中心之间的距离就是两点云之间的平移，去中心之后两点云之间的约束只剩下旋转，这样可将平移和旋转解耦开。

​ 令$$E_1(R,t)=\frac{1}{N_y}\sum _{i=1}^{N_y}(||x_i-u_x-R(y_i-u_y)|{|}^2$$$$E_2(R,t)=\frac{1}{N_y}\sum _{i=1}^{N_y}||u_x-R{u}_y-t|{|}^2$$

那么，$E(R,t)=E_1(R,t)+E_2(R,t)$。
在求解$minE(R,t)$的过程中，由于第一项$E_1(R,t)$只和$R$有关，因此只要根据$min{E}_1(R,t)$求出$R$，令第二项$E_2(R,t)$取最小值0，即可求出平移$t$。

求解旋转矩阵$R$

• 化简：将原始点云转换为去质心点云

​ 记两点云的去质心点云分别为：$$x_i^{,}=x_i-u_x$$$$y_i^{,}=y_i-u_y$$则
E 1 ( R , t ) = 1 N y ∑ i = 1 N y ∣ ∣ x i − u x − R ( y i − u y ) ∣ ∣ 2 = 1 N y ∑ i = 1 N y ∣ ∣ x i , − R y i , ∣ ∣ 2      = 1 N y ∑ i = 1 N y ( x i , T x i , + y i , T R T R y i , − 2 x i , T R y i , ) = 1 N y ∑ i = 1 N y ( x i , T x i , + y i , T y i , − 2 x i , T R y i , ) E_1(R,t)=\frac{1}{N_y}\sum_{i=1}^{N_y}||x_i-u_x-R(y_i-u_y)||^2=\frac{1}{N_y}\sum_{i=1}^{N_y}||x_i^,-Ry_i^,||^2 \\ \ \ \ \ =\frac{1}{N_y}\sum_{i=1}^{N_y}(x_i^{,T}x_i^,+y_i^{,T}R^TRy_i^,-2x_i^{,T}Ry_i^,) =\frac{1}{N_y}\sum_{i=1}^{N_y}(x_i^{,T}x_i^,+y_i^{,T}y_i^,-2x_i^{,T}Ry_i^,) E1​(R,t)=Ny​1​i=1∑Ny​​∣∣xi​−ux​−R(yi​−uy​)∣∣2=Ny​1​i=1∑Ny​​∣∣xi,​−Ryi,​∣∣2    =Ny​1​i=1∑Ny​​(xi,T​xi,​+yi,T​RTRyi,​−2xi,T​Ryi,​)=Ny​1​i=1∑Ny​​(x