[发明专利]基于点云匹配快速搜索最近点的方法、介质、终端和装置

说明书

本发明公开一种基于点云匹配快速搜索最近点的方法、介质、终端和装置。方法包括以下步骤：创建矩形栅格地图；将参考点云和目标点云在矩形栅格地图中栅格化；采用栅格滤波法去除参考点云中的孤立参考点和目标点云中的孤立目标点；计算每个有效栅格的重心点；在第一预设范围内搜索目标点云中每个目标点在参考点云中的最近点，所述最近点为与目标点距离最近的重心点。本发明将参考点云和目标点云在给定的地图上进行栅格化，且在栅格化的同时去除了参考点云与目标点云内孤立的点，在大幅提高ICP运算效率的同时提高了匹配的精度，对于较大旋转或平移的点云匹配有更好的匹配效果与适应能力。

【技术领域】

本发明涉及导航定位领域，尤其涉及一种基于点云匹配快速搜索最近点的方法、介质、终端和装置。

【背景技术】

迭代最近点算法(Iterated Closest Points,ICP)顾名思义，就是采用迭代优化的思想以空间距离作为匹配点的选择依据，通过不断调整点云的位姿使得匹配点之间距离累计最小。ICP算法能够使两个不同坐标系下的点云匹配到一个坐标系中，这个过程就是匹配，目的就是找到待匹配点云与参考点云之间的刚体变换，即旋转参数R和平移参数T，使得两点云数据之间满足某种度量准则下的最优匹配，其本质是基于最小二乘的最优匹配方法。该方法重复进行选择最近点对，计算最优刚体变换，直到满足正确配准的收敛精度要求。计算目标点云中每个点在参考点云中的最近点是基于ICP点云匹配最重要的部分，当前搜索最近点的方法计算量非常大，严重影响了ICP整个算法的运行效率，据统计该步骤的运行时间大约占总体时间的70％-90％，因此提高最近点搜索的速度是提升ICP算法效率的关键。

普通的最近点搜索策略为，根据目标点云中任一点在目标点云的索引，查找参考点云对应索引的点，然后在该点前后一定数量点的范围内搜索目标点的最近点。对于较大角度旋转或平移的两个点云的最近点搜索，普通方法在搜索距离较远点时浪费更多的搜索时间，而搜索范围过小又难以获得准确的匹配甚至无法完成点云的匹配。为了提高ICP最近点搜索效率，出现了多种多样的方法，比如依靠金字塔原理提出来的分级收缩算法、采用反向定标技术的投影搜索算法、Greenspan提出的近似多维二元搜索树(AK-D Tree)算法等等，大大加快了搜索速度，提高了搜索效率。这些方法各有利弊，如大量回溯会导致kd-tree最近邻搜索的性能大大下降，而分级收缩算法对于比较分散点云的最近点搜索效率较低等等。

【发明内容】

本发明提供了一种基于点云匹配快速搜索最近点的方法、介质、终端和装置，解决了以上所述的技术问题。

本发明解决上述技术问题的技术方案如下：一种基于点云匹配快速搜索最近点的方法，包括以下步骤：

步骤1，创建矩形栅格地图；

步骤2，获取相邻两帧点云数据，其中第一帧为参考点云，第二帧为目标点云，将参考点云在所述矩形栅格地图的原点栅格化，并根据目标点云在参考点云坐标系的初始坐标将目标点云在所述矩形栅格地图中栅格化；

步骤3，采用栅格滤波法去除所述参考点云中的孤立参考点和所述目标点云中的孤立目标点；

步骤4，计算每个有效栅格的重心点，所述有效栅格为矩形栅格地图中包含至少一个参考点的栅格；

步骤5，在第一预设范围内搜索目标点云中每个目标点在参考点云中的最近点，所述最近点为与所述目标点距离最近的重心点。

在一个优选实施方式中，计算每个有效栅格的重心点具体为：若所述有效栅格内包含一个参考点，则所述参考点为所述有效栅格的重心点；若所述有效栅格内包含多个参考点，则采用重心法计算所述多个参考点的重心并将计算结果作为所述有效栅格的重心点。

在一个优选实施方式中，采用栅格滤波法去除所述参考点云中的孤立参考点具体为：

获取第一栅格的信息，所述第一栅格为只包含一个参考点的栅格；