ICP算法

1.ICP算法（迭代最近点算法）

ICP(迭代最近点算法)是一种古老而又经典的点云匹配算法，其思路也很清晰。点云配准就是求解两堆点云之间的变换关系——也就是旋转关系 $R$ 和平移关系 $t$。

ICP算法的思路就是：找到两组点云集合中距离最近的点对，根据估计的变换关系（$R$ 和 $t$）来计算距离最近点对经过变换之后的误差，进过不断的迭代直至误差小于某一阈值或者达到迭代次数来确定最终的变换关系。

ICP算法的数学描述：
给定两个点云集合：
$$X=(x_1,x_2,x_3...x_n)$$$$P=(p_1,p_2,p_3...p_m)$$
求解$R$ 和 $t$，使下式最小
$$E(R,t)=\frac{1}{n}\sum _{i=1}^n||x_i-(R{p}_i+t)|{|}^2$$

1.ICP算法的求解

1.1 已知对应点的情况

如果已知两个点云中点的对应关系，那么ICP算法的求解过程将非常简单，这个在视觉slam中较为常用，因为我们可以根据特种匹配的方式找出空间点的对应关系。

具体方法为：

1. 首先，计算两组点云的质心
   $$u_x=\frac{1}{n}\sum _{i=1}^n||x_i|{|}^2$$
   $$u_p=\frac{1}{m}\sum _{i=1}^m||p_i|{|}^2$$
2. 计算两组点云中的点以质心为原点的坐标
   $$X^{\prime }=(x_i-u_x)=(x_i^{\prime })$$$$P^{\prime }=(p_i-u_p)=(p$$