[发明专利]基于初始值的快速迭代配准方法、介质、终端和装置

说明书

本发明公开一种基于初始值的快速迭代配准方法、介质、终端和装置，方法包括以下步骤：建立连续两帧图像中关键点和最佳匹配点的点对以及包括所有点对的特征点集，并采集所有点对的二维信息和/或三维信息；利用所有点对的二维信息和/或三维信息建立两帧图像之间相机位姿的误差函数；对误差函数进行迭代优化，并在迭代优化过程中不断删除置信度小于预设阈值的点对。本发明在可靠初始值的基础上，只针对稀疏特征点对进行迭代配准并得到优化位姿，速度快，配准效率高；同时加入了惩罚函数，在不增加额外计算成本的情况下可以对特征匹配点对进行良好的校验与筛选，剔除异常匹配点对，从而确保优化结果的准确与稳定。

技术领域

本发明涉及SLAM领域，尤其涉及一种基于初始值的快速迭代配准方法、介质、终端和装置。

背景技术

SLAM(simultaneous localization and mapping)，即时定位与地图构建技术主要包括两个部分：前端和后端，前端也称为视觉里程计（VO），它根据相邻图像的信息估计出相机的运动，给后端提供较好的初始值。VO的实现方法可以根据是否需要提取特征分为两类：基于特征点的方法，不使用特征点的直接法。直接法通过最小化两帧之间的灰度差估计相机运动，这种方法的缺点是相机移动太快，光照变化明显，运算量太大。因此基于特征点的方法是目前的主流方式，通常采用特征匹配和ICP（Iterative Closest Point，即迭代最近点算法）相结合的策略进行配准，即先采用特征匹配计算粗略位姿，为ICP提供初值，再用ICP算法对位姿精化。基于特征点的方法运行稳定，对光照、动态物体不敏感。现有方法中往往采用RANSAC算法来得到更加准确的特征匹配结果。RANSAC的思想是从一组包含异常数据的观测数据集中，通过迭代方式估计数学模型的参数，从而筛选出有效样本数据。RANSAC是一种不确定的算法，只有一定的概率得到可信的模型，概率与迭代次数成正比，为了提高概率必须提高迭代次数。RANSAC计算参数的迭代次数与特征点对的个数有关，尤其在特征点特别多的情况下，RANSAC计算精度与效率是矛盾的，为了得到准确的结果，需要迭代很多次，对视觉里程计的实时性是一大挑战。若为了提高计算效率，人为设置迭代次数的上限，此时得到的结果可能不是最优的，甚至可能是错误的。此外，RANSAC要求设置跟问题相关的阈值，如必须先制定一个合适的容忍误差，此外还可能指定迭代次数作为收敛条件。

发明内容

本发明提供了一种基于初始值的快速迭代配准方法、介质、终端和装置，解决了现有技术进行特征匹配时效率低且匹配效果较差的技术问题。

本发明解决上述技术问题的技术方案如下：一种基于初始值的快速迭代配准方法，包括以下步骤：

按照时间顺序将连续的两帧图像标记为第一帧图像和第二帧图像，提取第二帧图像中每个关键点的特征，并在第一帧图像中查找每个关键点对应的最佳匹配点；

建立关键点和最佳匹配点的点对以及包括所有点对的特征点集，并采集所有点对的二维信息和/或三维信息；

利用特征点集以及所有点对的二维信息和/或三维信息建立第二帧图像和第一帧图像之间相机位姿的误差函数；

将第一帧图像与相邻前帧的相机位姿估计值作为所述误差函数的初始值，并对所述误差函数进行迭代优化，在迭代优化过程中不断从所述特征点集中删除置信度小于预设阈值的点对，以生成误差最小时的最优位姿估计值。

在一个优选实施方式中，所述基于初始值的快速迭代配准方法还包括全局优化步骤，具体为：将所述最优位姿估计值作为ICP配准的初始值，并利用所采集的深度图像以及RGB图像，结合几何约束以及光学约束对全局匹配点对进行ICP配准。

在一个优选实施方式中，建立的误差函数具体为：

其中，为第二帧图像中的关键点，为第一帧图像中的最佳匹配点，为点经位姿变换后与点之间的距离误差，为点经位姿变换后与点之间的距离误差，为特征点集，为特征点集的点对数量，为惩罚函数。