迭代最近点算法流程详解（ICP算法）

迭代最近点(Iterative Closest Points, ICP)算法包括两部分：对应点搜索和位姿求解。它的目的是寻求点集之间的匹配关系，求解的结果是两点集之间的平移及旋转量。假设M、P是两个点集，P为待配准点集，M为基准数据点集（可以理解为两个不同坐标系中的点集），其基本原理如下：

1、搜索最近点：取P中一点p（i），在M中找出距离p（i）最近的m（i），则（pi，mi）就构成了一组对应点对集，但是p（i）与m（i）之间存在着某种旋转和平移关系（R，T），这也就是我们要求的。

2、求解变换关系（R，T）：n对点（pi，mi）对于n个方程组，那么就一定能运用数学方法求解得出（R，T），但是为了求解更加精确的变换关系，采用了迭代算法。

3、应用变换：对点集P中的每一个点pi运用变换关系得到点集P2，定义函数E，

根据精度要求，定义终止迭代的条件，即E小于一个具体值时终止迭代。（可以把E理解为经过变换后的P2中每个点与M点集中对应点的距离和）。

4、重复迭代：如果某次迭代满足要求，则终止迭代，输出最优（R，T），否则继续迭代，但是要注意一点：在每一次迭代开始时都要重新寻找对应点集。也就是说要把结果变换的Pn带入函数E中继续迭代。