PCL KD-ICP实现点云精配准

ICP基础理论参考：PCL ICP算法实现点云精配准

一、 KD-ICP配准

1、KD-tree优化的ICP

  通过构建的KD-tree实现最近邻算法,对粗配准后的点进行近邻搜索。以欧氏距离为判断标准,剔除欧氏距离大于阈值的配准关键点,保存配准精度高的点,具体步骤如下：

1. 寻找目标点的叶节点:从根节点递归向下搜索KD-tree,如果目标点当前维度的坐标小于切分点坐标,则转移到左子节点;反之,转移到右子节点。进入叶节点后,将该叶节点内与待配准点之间距离最小的点作为当前最近点。
2. 递归向上回溯:如果该节点保存的实例点比当前最近点距离目标点更近,则更新当前最近点;反之,检查另一子节点区域。如果该区域与以目标点为球心,以最近点间的距离为半径的球相交,则另外一个子节点区域内可能存在距离目标点更近的点,此时转移到另一子节点,继续递归搜索。如果不相交,则向上回溯。
3. 直到退回根节点,搜索结束,当前搜索的最近点为目标点的近邻点。
4. 设定阈值,当目标点与最近点之间的欧氏距离小于阈值时,认为配准精度高,保留该点。反之,则剔除该点。

  根据上述原理,可快速剔除配准精度低的点,仅保留配准质量高的点。通过剔除配准误差较大的点以及优化剩余关键点的质量,实现配准点云精度优化的粗处理,为后续细处理提供基础。

2、双向KD-tree优化的ICP

  初始配准后的