PCL点云配准-ICP

标准ICP算法是一种点集对点集配准方法(基于点-点距离的算法)，标准的ICP算法需要粗配，满足距离足够近这一条件之后才能进行精确配准。

选取目标点云P和源点云Q，按照一定的约束条件，找到最邻近点（pi,qi）,然后计算出最优R和t（旋转和平移），使得误差函数最小，误差函数E（R，t）：

基本算法流程：

1、在目标点云P中选取点集pi。

2、在源点云Q中选取对应的点集qi，使得||qi-pi||==min，也就是距离最小，这里的距离为欧氏距离。

3、计算旋转R和t，使得误差函数最小。

4、此时经过步骤3的R旋转和t平移后得到新的点集pi`，pi`=Rpi+t

5、计算pi`到点集qi的平均距离d。

6、如果d小于预设的阈值或者超过了迭代的次数，则停止，否则跳到步骤2，直到满足收敛条件。