(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210599284.5 (22)申请日 2022.05.30 (71)申请人 杭州电子科技大 学 地址 310018 浙江省杭州市下沙高教园区 二号路 (72)发明人 胡炜薇　林庆磊　秦会斌　 (74)专利代理 机构 浙江永鼎律师事务所 3 3233 专利代理师 阮玉欣 (51)Int.Cl. G06T 5/00(2006.01) G06T 7/246(2017.01) G06T 7/70(2017.01) G06V 10/762(2022.01) G06V 10/774(2022.01) (54)发明名称 一种基于快速跟踪的ORB-SLAM3地图修复方 法 (57)摘要 本发明公开了一种基于快速跟踪的ORB ‑ SLAM3地图修复方法， 步骤S1： 播放 当前地图初始 化前的所有图像帧； 步骤S2： 对播放的图像帧进 行快速跟踪并进行地图修复。 采用本发明的技术 方案， 在单目视觉惯性模式中， 利用一种快速的 跟踪方法， 从当前地图的初始帧开始， 向时间戳 小的图像进行反向跟踪， 并估计出较为精确的位 姿， 生成一个完整的地图。 该地图包含所有的输 入数据， 从而能够实现在任何初始化的场景下， 不论初始化的效果如何， 都能够修复出一个完整 的地图， 减少SLAM系统对初始化的依赖， 规避由 于初始化激励不足产生的地图丢弃问题， 使机器 人在离线仅定位跟踪时变得更加稳定 。 权利要求书2页 说明书7页 附图6页 CN 114998130 A 2022.09.02 CN 114998130 A 1.一种基于快速跟踪的ORB ‑SLAM3地图修复方法， 其特 征在于， 包括以下步骤： 步骤S1： 播 放当前地图初始化前的所有图像帧； 步骤S2： 对播 放的图像帧进行 快速跟踪并进行地图修复； 其中， 所述 步骤S1进一 步包括： 步骤S11： 创建用于存放图像帧的栈； 步骤S12： 将输入的每一个图像帧都放入栈中， 直到当前地图完成惯性测量单元的初始 化过程； 步骤S13： 栈中保留当前活跃地图的初始化帧之前的所有图像帧， 剔除初始化帧之后的 图像帧； 步骤S14： 不断将栈中的帧进行 出栈， 直至栈中的所有图像帧都处 理完； 所述步骤S2进一 步包括： 步骤S21： 判断跟踪方式； 步骤S22： 精确定位跟踪； 步骤S23： 快速参 考关键帧跟踪； 步骤S24： 将上一图像帧的位姿赋值给当前帧； 步骤S25： 判断是否生成关键帧； 步骤S26： 将当前帧位姿保存到地图中； 其中， 所述 步骤S21进一 步包括： 步骤S211： 若当前帧为栈中弹出的第一帧， 则跳转到S2 2； ； 步骤S212： 若当前帧已经与 参考关键帧做过匹配， 但匹配点数或者 内点数不足， 则跳转 到S22； 步骤S213： 其 余情况跳转到S23； 所述步骤S22进一步包括： 步骤S221： 词袋匹配； 获取到栈中弹出的帧F， 若F为栈弹出的第一帧， 则当前帧只与当 前活跃地图的初关键始帧KF0进 行匹配； 否则， 当前帧要与其参考关键帧、 KF0以及关键帧数 据库中有较多公共节点的关键帧进行分别匹配； 步骤S222： 利用上述匹配过程中与当前帧匹配点最多的一个关键帧， 使用MLPNP进行位 姿估计； 步骤S223： 利用卡方分布进行外点剔除， 定义重投影误差为： 其中u为F的特征点在像 素平面上的位置， 为KF0的地图点投影到F像 素平面上的位置； 重投影误差服从如下高斯分布： e～M(0,∑) 其中∑为协方差： 其中s为图像金字塔的尺度缩放因子， p为第0层的标准差； 将 中的误差项利 用协方差加 权后计算内积得到一个误差标量r＝eT∑‑1e， 由于单 目投影为2自由度， 在0.05 的显著水平下卡方统计量阈值为5.99， 我们认为大于此阈值的点为外点， 小于此阈值的为权　利　要　求　书 1/2 页 2 CN 114998130 A 2内点； 步骤S224： 判断内点数； 若内点数足够， 则将估计位姿作 为当前帧的位姿并跳转到S25， 否则利用地图点的重投影生 成更多的匹配点， 同时将阈值调整的更为严格， 重新执行MLPNP 估计位姿， 并进行 此步骤的判断， 若依然不够则跳转到S24； 所述步骤S23进一 步包括： 步骤S231： 词袋匹配； 此 过程中， 当前帧只与其 参考关键帧匹配； 步骤S232： 判断匹配点的数目； 若匹配点数小于阈值则跳转到S21； 步骤S233： 以上一帧的位姿变换作为初值， 加速使用MLPNP估计位姿的收敛速度； 步骤S234： 利用卡方分布进行外点剔除； 步骤S235： 判断内点数； 若内点数足够则跳转到S25， 否则跳转到S21； 所属步骤S25进一 步包括： 步骤S251： 判断距离当前帧距离上一关键帧的帧数 是否够多， 若不够， 则结束S25； 步骤S252： 判断距离最近关键帧的距离是否够远， 若不够， 则结束S25； 步骤S253： 判断当前帧与局部地图中的共视特征点的数目是否小于阈值， 若不满足， 结 束S25； 步骤S254： 将当前帧构造成关键帧， 其地图点插入地图， 并将此帧作为后续图像帧的参 考关键帧。权　利　要　求　书 2/2 页 3 CN 114998130 A 3