基于icp的帧间匹配

1.icp算法

迭代最近点（Iterative Closest Point, 下简称ICP）算法是一种点云匹配算法。

算法思路：

• 首先对于一幅点云中的每个点，在另一幅点云中计算匹配点（最近点）
• 极小化匹配点间的匹配误差，计算位姿
• 然后将计算的位姿作用于点云
• 再重新计算匹配点
• 如此迭代，直到迭代次数达到阈值，或者极小化函数的变化量小于设定阈值

ICP算法有一些不足： 

首先，ICP对初值比较敏感，初值给的不好，就需要花费更多的迭代次数进行匹配．

其次，由于它是迭代很多次的，所以其花费的时间很长，这一点是非常致命的，之后我会通过程序来让大家体验ICP的费时．

再次，精度与速度是矛盾的，ICP算法理论上可以实现很高的精度，但是要很多很多的迭代次数，以及很长的时间．所以，当限制了迭代次数的情况下，精度就不一定能保证了．

接下来，我将通过代码带着大家感受一下ICP的不足，感受其固定迭代次数时的精度差，以及耗时长．
 

参考：从零开始搭二维激光SLAM --- 基于ICP的帧间匹配_icp scanmatch_李太白lx的博客-CSDN博客

2.launch文件的编写