技术摘要：
本发明公开一种基于初始值的快速迭代配准方法、介质、终端和装置，方法包括以下步骤：建立连续两帧图像中关键点和最佳匹配点的点对以及包括所有点对的特征点集，并采集所有点对的二维信息和/或三维信息；利用所有点对的二维信息和/或三维信息建立两帧图像之间相机位姿  全部
背景技术：
SLAM(simultaneous  localization  and  mapping)，即时定位与地图构建技术主 要包括两个部分：前端和后端，前端也称为视觉里程计（VO），它根据相邻图像的信息估计出 相机的运动，给后端提供较好的初始值。VO的实现方法可以根据是否需要提取特征分为两 类：基于特征点的方法，不使用特征点的直接法。直接法通过最小化两帧之间的灰度差估计 相机运动，这种方法的缺点是相机移动太快，光照变化明显，运算量太大。因此基于特征点 的方法是目前的主流方式，通常采用特征匹配和ICP（Iterative  Closest  Point，即迭代最 近点算法）相结合的策略进行配准，即先采用特征匹配计算粗略位姿，为ICP提供初值，再用 ICP算法对位姿精化。基于特征点的方法运行稳定，对光照、动态物体不敏感。现有方法中往 往采用RANSAC算法来得到更加准确的特征匹配结果。RANSAC的思想是从一组包含异常数据 的观测数据集中，通过迭代方式估计数学模型的参数，从而筛选出有效样本数据。RANSAC是 一种不确定的算法，只有一定的概率得到可信的模型，概率与迭代次数成正比，为了提高概 率必须提高迭代次数。RANSAC计算参数的迭代次数与特征点对的个数有关，尤其在特征点 特别多的情况下，RANSAC计算精度与效率是矛盾的，为了得到准确的结果，需要迭代很多 次，对视觉里程计的实时性是一大挑战。若为了提高计算效率，人为设置迭代次数的上限， 此时得到的结果可能不是最优的，甚至可能是错误的。此外，RANSAC要求设置跟问题相关的 阈值，如必须先制定一个合适的容忍误差，此外还可能指定迭代次数作为收敛条件。
技术实现要素：
本发明提供了一种基于初始值的快速迭代配准方法、介质、终端和装置，解决了现 有技术进行特征匹配时效率低且匹配效果较差的技术问题。 本发明解决上述技术问题的技术方案如下：一种基于初始值的快速迭代配准方 法，包括以下步骤： 按照时间顺序将连续的两帧图像标记为第一帧图像和第二帧图像，提取第二帧图像中 每个关键点的特征，并在第一帧图像中查找每个关键点对应的最佳匹配点； 建立关键点和最佳匹配点的点对以及包括所有点对的特征点集，并采集所有点对的二 维信息和/或三维信息； 利用特征点集以及所有点对的二维信息和/或三维信息建立第二帧图像和第一帧图像 之间相机位姿的误差函数； 将第一帧图像与相邻前帧的相机位姿估计值作为所述误差函数的初始值，并对所述误 差函数进行迭代优化，在迭代优化过程中不断从所述特征点集中删除置信度小于预设阈值 的点对，以生成误差最小时的最优位姿估计值。 5 CN 111739071 A 说　明　书 2/10 页 在一个优选实施方式中，所述基于初始值的快速迭代配准方法还包括全局优化步 骤，具体为：将所述最优位姿估计值作为ICP配准的初始值，并利用所采集的深度图像以及 RGB图像，结合几何约束以及光学约束对全局匹配点对进行ICP配准。 在一个优选实施方式中，建立的误差函数具体为： 其中， 为第二帧图像中的关键点， 为第一帧图像中的最佳匹配点， 为点 经位姿 变换后与点 之间的距离误差， 为点 经位姿变换后与点 之间的距离误差， 为特 征点集， 为特征点集 的点对数量， 为惩罚函数。 在一个优选实施方式中，当进行3D-2D的相机位姿估计时，所述 与所述 为重 投影误差，即： ， ； 当进行3D-3D的相机位姿估计时，所述 与所述 为观测误差，即： ， ， 其中， 为点 的二维坐标， 为点 的二维坐标， 为相机内参， 为位姿变 换初始值。 在一个优选实施方式中，所述对误差函数进行迭代优化，并不断从特征点集中删 除置信度小于预设阈值的点对具体为： 采用高斯牛顿法求解所述误差函数，且在迭代求解过程中逐渐减小 值，通过预设置 信度公式计算不同 值下每个所述点对的置信度，并删除置信度小于预设阈值的点对，以 生成误差最小时相机的最优位姿估计值；所述预设置信度公式为： ， ， 其中 为置信度， 为迭代参数， 为点 经位姿变换后与点 之间的距离误差， 为 点 经位姿变换后与点 之间的距离误差， 为激光扫描的重建物体的表面直径。 本发明实施例的第二方面提供了一种计算机可读存储介质，存储有计算机程序， 6 CN 111739071 A 说　明　书 3/10 页 所述计算机程序被处理器执行时，实现以上所述的基于初始值的快速迭代配准方法。 本发明实施例的第三方面提供了一种基于初始值的快速迭代配准终端，包括所述 的计算机可读存储介质和处理器，所述处理器执行所述计算机可读存储介质上的计算机程 序时实现以上所述基于初始值的快速迭代配准方法的步骤。 本发明实施例的第四方面提供了一种基于初始值的快速迭代配准装置，包括匹配 模块、点集建立模块、信息采集模块、误差函数建立模块和优化模块， 所述匹配模块用于按照时间顺序将连续的两帧图像标记为第一帧图像和第二帧图像， 提取第二帧图像中每个关键点的特征，并在第一帧图像中查找每个关键点对应的最佳匹配 点； 所述点集建立模块用于建立关键点和最佳匹配点的点对以及包括所有点对的特征点 集； 所述信息采集模块用于采集所有点对的二维信息和/或三维信息； 所述误差函数建立模块用于利用特征点集以及所有点对的二维信息和/或三维信息建 立第二帧图像和第一帧图像之间相机位姿的误差函数； 所述优化模块用于采用第一帧图像与相邻前帧的相机位姿估计值作为所述误差函数 的初始值对所述误差函数进行迭代优化，且在迭代优化过程中不断从所述特征点集中删除 置信度小于预设阈值的点对，以生成误差最小时的最优位姿估计值。 在一个优选实施方式中，所述误差函数建立模块建立的误差函数具体为： 其中， 为第二帧图像中的关键点， 为第一帧图像中的最佳匹配点， 为特征点集， 为特征点集 的点对数量， 为惩罚函数。当进行3D-2D的相机位姿估计时，所述 与 所述 为重投影误差，即： ， ； 当进行3D-3D的相机位姿估计时，所述 与所述 为观测误差，即： ， ， 其中， 为点 的二维坐标， 为点 的二维坐标， 为相机内参， 为位姿变 换初始值。 在一个优选实施方式中，所述优化模块具体用于采用高斯牛顿法求解所述误差函 数，且在迭代求解过程中逐渐减小 值，通过预设置信度公式计算不同 值下每个所述点对 的置信度，并删除置信度小于预设阈值的点对，以生成误差最小时相机的最优位姿估计值； 所述预设置信度公式为： 7 CN 111739071 A 说　明　书 4/10 页 ， ， 其中 为置信度， 为迭代参数， 为点 经位姿变换后与点 之间的距离误差， 为 点 经位姿变换后与点 之间的距离误差， 为激光扫描的重建物体的表面直径。 本发明提供了一种基于初始值的快速迭代配准方法、介质、终端和装置，具有以下 有益效果： （1）在可靠初始值的基础上，只针对稀疏特征点对进行迭代配准并得到优化位姿，不仅 速度快，而且克服了RANSAC的不确定因素，大大提升了配准效率； （2）在基于初始值的迭代配准过程中加入了惩罚函数，在不增加额外计算成本的情况 下可以对特征匹配点对进行良好的校验与筛选，剔除异常匹配点对，从而确保优化结果的 准确与稳定。 为使发明的上述目的、特征和优点能更明显易懂，下文特举本发明较佳实施例，并 配合所附附图，作详细说明如下。 【附图说明】 为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作 简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围 的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附 图获得其他相关的附图。 图1是实施例1提供的基于初始值的快速迭代配准方法的流程示意图； 图2是实施例2提供的基于初始值的快速迭代配准装置的结构示意图； 图3是实施例3提供的基于初始值的快速迭代配准终端的结构示意图。 【