ICP算法

ICP算法定义

ICP算法(迭代最近点算法)是一种古老又经典的点云匹配算法。
点云配准：求解两个点云之间的变换关系，也就是旋转关系R和平移关系t。

ICP算法思路

找到两组点云集合中距离最近的点对，根据估计的变换关系( R 和 t )来计算距离最近点对经过变换之后的误差，经过不断的迭代直至误差小于某一阈值或达到迭代次数来确定最终的变换关系

ICP算法的数学描述

给定两个点云集合：
$X=(x_1,x_2,x3\dots x_n)$
$P=(p_1,p_2,p_3\dots p_n)$
求解 R 和 t , 使得下式最小：
$E(R,t)=\frac{1}{n}{\sum }_{i=1}^n||x_i-(R{p}_i+t)|{|}^2$

ICP算法的求解

已知点云的对应情况

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

1. 首先，计算两组点云的质心
   $u_x=\frac{1}{n}{\sum }_{i=1}^n||x_i|{|}^2$ 和 $u_p=\frac{1}{n}{\sum }_{i=1}^n||p_i|{|}^2$
2. 计算两组点云中的点以质心为原点的坐标
   $X^{\prime }=(x_i-u_x)=x_i^{\prime }$ 和 $P^{\prime }=(p_i-u_p)=p_i^{\prime }$
3. 计算 $w$并对其进行SVD分解
   $w=\frac{1}{n}{\sum }_{i=1}^n{x}_i^{\prime }{p}_i^{\prime T}=U\left[\begin{array}{ccc}{\delta }_1& 0& 0\\ 0& {\delta }_2& 0\\ 0& 0& {\delta }_1\end{array}\right]V^T$
4. 则两组点云之间的变换关系为（即ICP算法的解）：
   $R=V{U}^T,t=u_x-R{u}_p$
   

未知点云的对应情况

已知对应点的情况下，我们可以一次性计算出点云之间的变换关系。但是在激光slam中，我们并不知道两组点云之间的对应关系。因此也就不能通过一次计算就求解出点云之间的变换关系。这时候我们的策略就是使用迭代的方式。
具体方法为：

1. 寻找两组点云中距离最近的点对。
2. 根据找到的距离最近点对，来求解两组点云之间的位姿关系。
3. 根据求解的位姿关系对点云进行变换，并计算误差。
4. 若误差没有达到要求，则重复1，2，3步直至误差满足要求或达到最大迭代次数。

ICP算法很简单，但是其思想却很巧妙。为什么这么说呢？因为ICP算法要求解的是两组点云之间的变换关系，要求解这种变换关系需要直到两组点云中点的对应关系。可是我们并不知道这种点的对应关系。那怎么办？
ICP算法的策略是，反正我不知道两组点云之间点的对应关系，那就认为距离最近的点是相互匹配的，然后以此来计算两组点云之间的变换关系。在得到变换关系之后，对点云进行变换并计算误差。然后再次选取距离最近的点作为匹配点并求解变换关系，重复该过程直至误差满足要求。