一种基于快速跟踪的ORB-SLAM3地图修复方法

一种基于快速跟踪的orb-slam3地图修复方法
技术领域
1.本发明涉及机器人同步定位与地图创建领域，尤其涉及一种基于快速跟踪的orb-slam3地图修复方法。


背景技术：

2.近十年来，同时定位和地图构建(simultaneous localization and mapping,slam)的发展非常迅速，研究的重心从最开始的激光雷达slam渐渐的向视觉slam转移。其中slam系统的建图效果被认为是目前研究的关键问题之一，对机器人的实际应用以及应用扩展有着重要意义，一个精确完整的地图将直接决定机器人离线定位跟踪的准确性。在目前的众多slam框架中，orb-slam3是目前性能最好的开源框架之一，在其单目视觉惯性模式中，采用运动恢复结构的方法进行初始化，同时计算本质矩阵和基础矩阵，并根据评分选择较好的一个模型用于初始化，可以提高初始化的成功率和质量。但由于运动恢复结构的本质需要足够大的视角变换才能完成初始化，故很多图像帧的信息没有被利用，其次由于单目惯性模式下，惯性测量单元在初始化阶段很容易激励不足或不稳定，这会导致初始化更加困难从而丢弃更多的帧。
3.现有技术的初始化方法主要存在以下两个问题：
4.1、运动恢复结构方式初始化，必定会消耗多帧才能进行初始化，导致部分信息丢失。
5.2、在初始化过程中，局部地图丢失以及惯性测量单元的激励不足，会导致创建的初始化地图频繁被丢弃，从而使大量的初始化部分的位姿信息不能被保留。
6.故，针对现有技术的缺陷，实有必要提出一种技术方案以解决现有技术存在的技术问题。


技术实现要素：

7.有鉴于此，确有必要提供一种基于快速跟踪的orb-slam3地图修复方法，将所有初始化成功前的图像帧进行保存，然后对这些帧在另一个线程进行快速的反向跟踪，利用词袋模型进行特征点的匹配，利用mlpnp进行位姿估计，然后将新生成的关键帧插入关键帧数据库，其地图点插入地图，将反向跟踪到的所有帧的位姿插入到地图轨迹之中，从而对地图起到修复作用，使最终生成的地图是一个完整的地图。
8.为了克服现有技术的缺陷，本发明的技术方案如下：
9.一种基于快速跟踪的orb-slam3地图修复方法，包括以下步骤：
10.步骤s1：播放当前地图初始化前的所有图像帧；
11.步骤s2：对播放的图像帧进行快速跟踪并进行地图修复；其中步骤s1进一步包括：
12.步骤s11：创建用于存放图像帧的栈；
13.步骤s12：将输入的每一个图像帧都放入栈中，直到当前地图完成惯性测量单元的初始化过程；
14.步骤s13：栈中保留当前活跃地图的初始化帧之前的所有图像帧，剔除初始化帧之后的图像帧；
15.步骤s14：不断将栈中的帧进行出栈，直至栈中的所有图像帧都处理完；
16.步骤s2对s1中获得的图像帧进行快速跟踪，该步骤进一步包括：
17.步骤s21：判断跟踪方式；该步骤进一步包括：
18.步骤s211：若当前帧为栈中弹出的第一帧，则跳转到s22；
19.步骤s212：若当前帧已经与参考关键帧做过匹配，但匹配点数或者内点数不足，则跳转到s22；
20.步骤s213：其余情况跳转到s23；
21.步骤s22：精确定位跟踪；该步骤进一步包括：
22.步骤s221：词袋匹配。获取到栈中弹出的帧f，若f为栈弹出的第一帧，则当前帧只与当前活跃地图的初关键始帧kf0进行匹配。否则，当前帧要与其参考关键帧、kf0以及关键帧数据库中有较多公共节点的关键帧进行分别匹配；
23.步骤s222：利用上述匹配过程中与当前帧匹配点最多的一个关键帧，使用mlpnp进行位姿估计；
24.步骤s223：利用卡方分布进行外点剔除，定义重投影误差为：
[0025][0026]
其中u为f的特征点在像素平面上的位置，为kf0的地图点投影到f像素平面上的位置。重投影误差服从如下高斯分布：
[0027]
e～n(0,∑)
[0028]
其中∑为协方差：
[0029][0030]
其中s为图像金字塔的尺度缩放因子，p为第0层的标准差。将
[0031]
中的误差项利用协方差加权后计算内积得到一个误差标量r＝e
t
∑-1
e，由于单目投影为2自由度，在0.05的显著水平下卡方统计量阈值为5.99，我们认为大于此阈值的点为外点，小于此阈值的为内点；
[0032]
步骤s224：判断内点数。若内点数足够，则将估计位姿作为当前帧的位姿并跳转到s25，否则利用地图点的重投影生成更多的匹配点，同时将阈值调整的更为严格，重新执行mlpnp估计位姿，并进行此步骤的判断，若依然不够则跳转到s24；
[0033]
步骤s23：快速参考关键帧跟踪；该步骤进一步包括：
[0034]
步骤s231：词袋匹配。此过程中，当前帧只与其参考关键帧匹配；
[0035]
步骤s232：判断匹配点的数目。若匹配点数小于阈值则跳转到s21；
[0036]
步骤s233：以上一帧的位姿变换作为初值，加速使用mlpnp估计位姿的收敛速度；
[0037]
步骤s234：利用卡方分布进行外点剔除；
[0038]
步骤s235：判断内点数。若内点数足够则跳转到s25，否则跳转到s21；
[0039]
步骤s24：将上一图像帧的位姿赋值给当前帧；
[0040]
步骤s25：判断是否生成关键帧；该步骤进一步包括：
[0041]
步骤s251：判断距离当前帧距离上一关键帧的帧数是否够多，若不够，则结束s25；
[0042]
步骤s252：判断距离最近关键帧的距离是否够远，若不够，则结束s25；
[0043]
步骤s253：判断当前帧与局部地图中的共视特征点的数目是否小于阈值，若不满足，结束s25；
[0044]
步骤s254：将当前帧构造成关键帧，其地图点插入地图，并将此帧作为后续图像帧的参考关键帧；
[0045]
步骤s26：将当前帧位姿保存到地图中。
[0046]
与现有技术相比较，本发明具有的有益效果：
[0047]
可高效跟踪：本发明在反向快速跟踪中利用词袋匹配和mlpnp估计位姿，将跟踪过程分为精准定位跟踪和快速参考关键帧跟踪，不仅保证了跟踪的实时性，也保证了较高的精度，同时将此跟踪过程与主线程并行运行，可以高效的对未利用的图像帧进行逐一跟踪。
[0048]
可完整建图：通过快速跟踪算法，能够使当前地图未利用的帧都能被估计出一个精确位姿，再结合新关键帧的生成，能够使最终生成的地图是完整的，该地图利用了所有图像帧的位姿。
[0049]
可忽略初始化效果：本发明规避了由初始化时的局部地图跟踪失败、惯性测量单元激励不足等原因导致的初始化时间过长或者频繁丢弃地图的问题，可以在任何初始化的情况下，都能实现地图修复。
附图说明
[0050]
图1为本发明提供的一种基于快速跟踪的orb-slam3地图修复方法的框架流程图；
[0051]
图2为本发明提供的一种基于快速跟踪的orb-slam3地图修复加速方法中步骤s21的详细流程图；
[0052]
图3为本发明提供的一种基于快速跟踪的orb-slam3地图修复加速方法中步骤s22的详细流程图；
[0053]
图4为本发明提供的一种基于快速跟踪的orb-slam3地图修复加速方法中步骤s23的详细流程图；
[0054]
图5为本发明提供的一种基于快速跟踪的orb-slam3地图修复加速方法中步骤s25的详细流程图；
[0055]
图6为本发明与orb-slam3算法在euroc数据集上的地图轨迹对比图；
[0056]
图7为本发明与orb-slam3算法在euroc数据集上的绝对轨迹误差对比图；
[0057]
如下具体实施例将结合上述附图进一步说明本发明。
具体实施方式
[0058]
以下将结合附图对本发明提供的技术方案作进一步说明。
[0059]
在目前的单目惯性slam系统中，orb-slam3是性能最好的系统之一，其采用运动恢复结构方式进行单目的初始化，但是orb-slam3在初始化时，若惯性测量单元激励不足，会导致频繁丢弃地图信息，从而不能建立一个完整的地图。本发明采用词袋模型进行特征点的匹配，以及利用mlpnp进行位姿估计，设计了一种快速的地图修复算法。词袋模型是目前在两帧2d-2d匹配中最高效的方法之一，其使用分层k-means树的存储结构，使用方便并且匹配速度快。而mlpnp是一种速度快、准确率高的位姿估计算法，且不依赖于相机模型，非常
适合用于slam系统中。为此，本发明提供了一种基于快速跟踪的orb-slam3地图修复方法。
[0060]
参见图1-5，本发明提供一种基于快速跟踪的orb-slam3地图修复方法，图1所示为本发明基于快速跟踪的orb-slam3地图修复方法的架构图，整体而言，本发明包括2大步骤，步骤s1：播放当前地图初始化前的所有图像帧；步骤s2：对播放的图像帧进行快速跟踪并进行地图修复；
[0061]
步骤s1将当前活跃地图初始化成功之前的所有帧进行逐一播放，由时间戳大的向时间戳小的播放，具体包括如下步骤：
[0062]
步骤s11：创建用于存放图像帧的栈；
[0063]
步骤s12：将输入的每一个图像帧都放入栈中，直到当前地图完成惯性测量单元的初始化过程；
[0064]
步骤s13：栈中保留当前活跃地图的初始化帧之前的所有图像帧，剔除初始化帧之后的图像帧；
[0065]
步骤s14：不断将栈中的帧进行出栈，直至栈中的所有图像帧都处理完；
[0066]
步骤s2对播放的图像帧逐一进行快速跟踪并完成地图修复，具体包括如下步骤：
[0067]
步骤s21：判断跟踪方式；
[0068]
图2所示为本发明提供的一种基于快速跟踪的orb-slam3地图修复方法中上述步骤的详细流程图，其进一步包括以下步骤：
[0069]
步骤s211：若当前帧为栈中弹出的第一帧，则跳转到s22；
[0070]
步骤s212：若当前帧已经与参考关键帧做过匹配，但匹配点数或者内点数不足，则跳转到s22；
[0071]
步骤s213：其余情况跳转到s23；
[0072]
步骤s22：精确定位跟踪；
[0073]
图3所示为本发明提供的一种基于快速跟踪的orb-slam3地图修复方法中上述步骤的详细流程图，其进一步包括以下步骤：
[0074]
步骤s221：词袋匹配。获取到栈中弹出的帧f，若f为栈弹出的第一帧，则当前帧只与当前活跃地图的初关键始帧kf0进行匹配。否则，当前帧要与其参考关键帧、kf0以及关键帧数据库中有较多公共节点的关键帧进行分别匹配；
[0075]
步骤s222：利用上述匹配过程中与当前帧匹配点最多的一个关键帧，使用mlpnp进行位姿估计；
[0076]
步骤s223：利用卡方分布进行外点剔除，定义重投影误差为：
[0077][0078]
其中u为f的特征点在像素平面上的位置，为kf0的地图点投影到f像素平面上的位置。重投影误差服从如下高斯分布：
[0079]
e～n(0,∑)
[0080]
其中∑为协方差：
[0081][0082]
其中s为图像金字塔的尺度缩放因子，p为第0层的标准差。将
[0083]
中的误差项利用协方差加权后计算内积得到一个误差标量r＝e
t
∑-1
e，由于单目
投影为2自由度，在0.05的显著水平下卡方统计量阈值为5.99，我们认为大于此阈值的点为外点，小于此阈值的为内点；
[0084]
步骤s224：判断内点数。若内点数足够，则将估计位姿作为当前帧的位姿并跳转到s25，否则利用地图点的重投影生成更多的匹配点，同时将阈值调整的更为严格，重新执行mlpnp估计位姿，并进行此步骤的判断，若依然不够则跳转到s24；
[0085]
步骤s23：快速参考关键帧跟踪；
[0086]
图4所示为本发明提供的一种基于快速跟踪的orb-slam3地图修复方法中上述步骤的详细流程图，其进一步包括以下步骤：
[0087]
步骤s231：词袋匹配。此过程中，当前帧只与其参考关键帧匹配；
[0088]
步骤s232：判断匹配点的数目。若匹配点数小于阈值则跳转到s21；
[0089]
步骤s233：以上一帧的位姿变换作为初值，加速使用mlpnp估计位姿的收敛速度；
[0090]
步骤s234：利用卡方分布进行外点剔除；
[0091]
步骤s235：判断内点数。若内点数足够则跳转到s25，否则跳转到s21；
[0092]
步骤s24：将上一图像帧的位姿赋值给当前帧；
[0093]
步骤s25：判断是否生成关键帧；
[0094]
图5所示为本发明提供的一种基于快速跟踪的orb-slam3地图修复方法中上述步骤的详细流程图，其进一步包括以下步骤：
[0095]
步骤s251：判断距离当前帧距离上一关键帧的帧数是否够多，若不够，则结束s25；
[0096]
步骤s252：判断距离最近关键帧的距离是否够远，若不够，则结束s25；
[0097]
步骤s253：判断当前帧与局部地图中的共视特征点的数目是否小于阈值，若不满足，结束s25；
[0098]
步骤s254：将当前帧构造成关键帧，其地图点插入地图，并将此帧作为后续图像帧的参考关键帧；
[0099]
步骤s26：将当前帧位姿保存到地图中。
[0100]
在本发明中使用的输入数据采用euroc数据集中的图像及imu数据，euroc数据集为微型飞行器收集的视觉惯性数据集，包含11段双目视频序列数据集场景为室内房间和工厂，根据各序列中初始化阶段的运动模式不同，将mh01-mh05序列作为难初始化序列，v101-v203为易初始化序列。本文仿真实验平台为一台intel(r)core(tm)i7-7700hq cpu@2.80ghz,24g内存的计算机。将数据集中的所有视频序列都应用到本实验中，观察经本文算法修复后的地图与未修复的地图之间的关系。
[0101]
为了验证本发明的技术效果，将通过算法对比验证本发明的有效性：
[0102]
在本方法与orb-slam3原方法的对比中，以两者均运行十次并取中值为最终的比较结果。在难初始化序列中，初始化由于时间变换过快以及惯性测量单元激励不足等原因，初始化时间过长，从而使所建地图很不完整；而在易初始化序列中，系统初始化时间较短，从而损失的地图信息并不多。首先以算法修复前后的地图的位姿数量以及轨迹长度为指标，对比在euroc数据集所有序列上的地图修复效果。然后以mh05序列代表难初始化序列，v203代表易初始化序列，以绝对轨迹的均方根误差(rmse)和标准差(s.d.)为指标，对比地图修复前后的精度和稳定性的提升。
[0103]
参见表1、表2、图6、图7所示，为本发明与orb-slam3原始算法的实验数据结果对
比。其中表1为本发明与orb-slam3原始算法生成地图的位姿数量和轨迹长度对比；表2为本发明与orb-slam3原始算法的绝对轨迹的均方根误差和标准差对比；图6为本发明与orb-slam3原始算法分别在mh05和v203的地图轨迹对比图；图7为本发明与orb-slam3原始算法分别在mh05和v203的均方根误差对比图。
[0104]
表1本发明与orb-slam3的地图位姿(pose)数量及轨迹长度对比
[0105][0106]
表2本发明与orb-slam3的绝对轨迹的均方根误差(rmse)及标准差(s.d.)对比
[0107][0108]
上表1列出了本发明算法与orb-slam3原始算法的地图位姿数量与轨迹长度的数据对比，能够直观的展示出本发明在不同序列上的不同效果，在难初始化也即初始化时间过长的序列上，修复的地图位姿最多、轨迹最长，在易初始化的序列上修复的地图位姿数量以及轨迹长度有限。表2列出了本发明算法与orb-slam3原始算法的绝对轨迹的均方根误差与标准差的数据对比，通过rmse与s.d.的对比，能够反映出本发明在精度和稳定性的提升
效果。图6展示了本发明与orb-slam3原算法分别在mh05与v203序列上生成的xyz方向的地图轨迹，竖直线左边即为恢复轨迹。图7展示了本发明与orb-slam3原算法分别在mh05与v203序列上的rmse的箱线图对比。综上所述，本发明通过利用快速跟踪的方法对orb-slam3进行地图修复，实现了在任何场景下，不论初始化效果，都能生成一个完整而精确的地图。经实验验证，在euroc数据集上较orb-slam3的绝对轨迹误差的均方根误差平均减少47.51％，标准差平均减少52.18％。在难初始化序列上，轨迹长度平均修复11.68％，地图包含位姿数量平均修复23.38％。
[0109]
以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。
[0110]
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。