双向KD-Tree的点云配准算法

文章目录

目录

前言

一、算法原理

1. 经典ICP算法

2.双向KD-Tree的点云配准算法

参考文献

二、代码实现

三、结果展示

---

前言

        经典ICP算法请看这里。传统ICP算法

        原始的随机 KD-Tree的ICP算法在搜索匹配点对的时候是的单向的，但是会出现一对多的问题，造成匹配不准确。基于此提出一种基于双向KD-Tree的点云配准算法，该算法的搜索时间会比经典的算法高一倍，但是能够确定一一对应的点对，能加快迭代速度，提高配准精度。

---

一、算法原理

1. 经典ICP算法

经典ICP算法和随机KD树一般可将ICP算法流程大致分为4个步骤：

1）设有从不同角度获取的点集P和Q，对点集P中每一点，在点集Q中搜索其最近点，组成对应点对集。

2）根据对应点对集，计算旋转矩阵R和平移矢量T使得所有对应点的距离总和最小。