（十三）ORBSLAM3子地图融合优化
前言
距离上一次博客更新已经过去了N年。。。
ORB-SLAM2继续更新，这真的是有生之年系列。
ORB-SLAM3在2的基础上，引进了IMU的初始化和融合估计，以及子地图功能。笔者目前 IMU 相关的知识相对比较薄弱，因此准备在第十四讲的时候再为大家介绍VI-ORBSLAM相关的内容。本讲主要关心的是纯视觉的子地图融合功能。
ORB-SLAM子地图简介
ORB-SLAM3使用子地图的原因是：当我们在跟踪丢失的情况下，不至于在唯一的一张地图中疯狂重定位。因为重定位的结果只有两个，一个是马上重定位成功，那么这是最理想的情况，我们可以继续进行环境探索；另一种就比较吃亏了，我们一直无法重定位成功，最终的结果就是系统判断当前跟踪状态为跟踪丢失，系统会直接将之前所重建的环境信息全部清除！重新生成一张地图。
这意味着什么？这就好比我们辛辛苦苦在word里面写了成千上万字的文档，word一个bug直接把你所有的数据清空了，你还找不回，只能从头写那么头疼！
那么解决的策略是啥？ORB-SLAM3就提出用子地图的方案！那么子地图好在哪里？我们还是以word为例：
当我们写了成千上万字的word文档被bug卡没以后，这部分的文字会自动存储到某个位置（历史子地图），新增的文字会写在另一个word文档中（当前子地图），当缘分来临的时候（回环检测），这两个word文档就有机会合并到一起（子地图合并），形成统一的一个word文档！oh man，想想就觉得激动人心。
ps：笔者无意针对word文档，只是因为前不久有个同事刚好因为word把文档内容卡没了，所以笔者才以此为例，xxxxxxx。。。
ORB-SLAM子地图算法
在介绍子地图功能之前，我们有几个问题需要着重关心一下：
1. 子地图功能和ORB-SLAM2中使用的单一地图的方法有什么异同？
2. 存在多个子地图的情况下，回环检测怎么做？这个方法与ORB-SLAM2中的回环检测有何区别？
3. 两个子地图怎么进行合并？
4. 合并的过程中，前端tracking到的新帧要怎么处理，此处必然会存在一个异步问题。
OK，那么我们逐个问题来回答！
1. 子地图的好处
其实子地图的好处我们已经在简介的时候有所提及，当我们只维护一个地图，在跟踪丢失的情况下会进行重定位。此时，重定位有两种情况：一种是重定位失败，地图重置；另一种是重定位成功，但是重定位成功也可能会出现两种情况：第一种是估算的相对变换准确，使得我们的机器人跟踪回到正确的道路上，这当然是最理想的情况！但是还有另一种情况会非常致命，那就是估算的相对变换出现偏差，导致最终的地图出现严重的不一致性。论文“ORBSLAM-Atlas: a robust and accurate multi-map system”中就介绍了重定位估算pose出现偏差的情况：
可见，上述左图即是ORB-SLAM2中只维护一个地图会出现的情况。而上述右图则是采用子地图的策略得到的结果，孰优孰劣一目了然。
除此之外，笔者认为子地图策略还有一个好处：那就是可以尽可能保证不丢帧，毕竟ORB-SLAM2需要进行多次重定位失败才会最终进行地图重置，而ORB-SLAM3只要一定位失败则直接重新创建子地图，可以有效地保存尽可能多的图像，这对于帧率较低的应用场景非常友好！
2. 回环检测
ORB-SLAM3的回环检测方法相比于2的有所改进，首先确定的一个地方是：即使系统中存在子地图，我们也只会存储唯一的一个BOW数据集，而每个关键帧都会存储各自所在的子地图，以此来加以区分。
具体的回环检测策略大体上与ORB-SLAM2并无太大的区别，即：
a. 将tracking阶段判断为关键帧的帧插入回环检测线程中，转换为BOW向量，在唯一的BOW数据集中搜索最相似的前几个候选关键帧，这与我们之前介绍的（八）ORBSLAM回环检测之位置识别 并无不同。
b. 对每个候选关键帧进行逐一筛查，具体为搜索帧间匹配，基于匹配关系确定对应的地图点之间的关系，从而计算相对变换，进一步验证投影匹配点数量是否满足要求。
最大的不同之处在于ORB3不需要在时序上有至少连续三帧被检测为回环，而是：
c. 与当前关键帧共视区域最多的 5 个关键帧，回环候选帧与其中至少 3 个关键帧存在足够的共视关系，即投影匹配满足约束要求，则视为回环。
这种方法相比于ORB-SLAM2的时序性要求来说，可以有效地提高召回率（在所有真实回环中被正确检测出来的概率），使得系统在该回环的时候回环，可以有效提高整体的精度。
最后一个值得注意的地方是，ORB3中存在多个子地图，在检测到最终的回环帧后，我们还需要判断当前回环的位置：
d. 判断回环帧所在的子地图，如果回环帧是在当前激活的子地图当中，那么则在当前子地图中执行闭环操作；如果回环帧是在历史子地图当中，那么则将当前子地图和历史子地图进行融合，并以历史子地图为主，将当前子地图融合到历史子地图中，并将当前子地图剔除，将融合后的历史子地图设置为当前子地图。
此处需要特别特别注意的是，由于基于词袋模型进行检索，可能会同时出现既有当前地图的回环候选帧，也有历史地图的回环候选帧．
3. 子地图融合
如果回环帧是在当前子地图中，则执行闭环操作，具体思路如：（十）ORBSLAM闭环所述。
如果回环帧是在历史子地图中，则执行子地图融合操作，具体思路如下：
由于在看代码的时候做了脑图，因此笔者也就偷个懒，直接把部分图抠出来放这里了．
合并两个子地图的思路非常直接，跟回环的闭环操作非常相似：
a. 当前帧与回环帧的子地图构建匹配关系，首先是基于ＢＯＷ搜索匹配；
b. 满足匹配约束以后计算回环帧的共视图与当前帧的匹配关系，从而找到更多的匹配关系；
c. 根据匹配关系估计一个相对变换关系；
d. 根据相对变换关系将回环帧子地图投影到当前帧中去搜索更多匹配关系，进而优化相对变换关系；
e. 将这个相对变换关系作为两个地图之间的＂纽带＂，即一条强约束的边，再对两个共视窗口进行 Local Bundle Adjustment，但是会先固定回环帧的局部地图，只调整当前帧的局部地图；
f. 基于LBA的结果，将当前帧的局部地图全部转换到回环帧的子地图中，并将其从当前地图删除；
g. 最后将当前地图中的剩余帧全部转换到回环帧子地图中．
恩...思路确实是非常朴素的，不过在使用策略上需要有一些要注意的地方，这些问题才是实际应用当中需要注意，比如：
a. 子地图什么时候才稳定下来，应该要保证每个子地图至少多少帧才用起来？
b. 在融合子地图之后，合并出来的结果会不会对之前导出的结果什么影响？
c. 理论上，所有子地图最好有一个标准的基准，比如一个世界坐标系，然而纯视觉的每一次初始化都定义不同的世界坐标系，那该怎么办？
4. 线程异步问题
这个问题应该是挺多人在读代码的时候比较容易忽略的地方，然而在实际功能添加的时候，这个却是没有办法忽略的．
之前和范帝楷范大佬探讨过这个问题，这次也顺便在这里提一下．
首先需要明确的是，ORB-SLAM3一共有３个主线程，还有一个是随时需要随时启动的GBA线程，即共计４个线程．
a. 在进行子地图合并的时候，首先关闭的功能就是GBA，如果GBA还有任务在进行，就强行将其终止，并且释放掉该线程．OK，GBA这个线程的异步问题我们可以直接忽略了；
b. 那么tracking这个线程咋办？这个线程我们可没有办法终止它，一旦终止了就意味着不再跟踪了，显然对于我们的实时定位肯定会存在大问题，因此这个地方我们不动他，可是他会一直产生新的关键帧，咋办？
c. 既然tracking没法关，Local Mapping总可以了吧？在关完GBA以后，我们就把LocalMapping也给关了，并且把所有新关键帧直接插入当前子地图中，不做额外处理，全部到合并子地图环节进行操作．这里有两个地方需要注意：
c1. 在进行当前帧共视图和回环帧共视图之间的融合时，Local Mapping是始终关闭的，因此tracking关键帧插不进来；
c2. 在c1步骤的融合结束以后，Local Mapping线程会被释放，因此此时新的关键帧会正常在Local Mapping中进行插入，此时调用当前子地图可以得到所有新插入的关键帧，从而将其进行转换，进而将新帧也一并融合了．
d. 最后这里可能还需要做一个pose graph的优化，此处是对当前所有关键帧的Pose进行联合优化，也是需要锁定Local Mapping的．
总结：
按照惯例，我们还是需要进行总结！
这一讲我们主要介绍了ORB-SLAM3中引入的子地图功能，其中包括其与回环检测操作的异同，以及子地图的关键方法和工程实现上可能存在坑的地方．
当然，笔者也是认知有限，难免有所偏差，还请各位读者多多批评指正．
后续应该还会有第十四讲，笔者需要去补补IMU等相关的知识，以期给大家带来VI-ORB相关的介绍，防止误人子弟．
还望各位读者朋友们能继续支持我！
参考文献：
[1] ORBSLAM-Atlas: a robust and accurate multi-map system
[2] ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM
PS：
如果您觉得我的博客对您有所帮助，欢迎关注我的博客。此外，欢迎转载我的文章，但请注明出处链接。
对本文有任何问题可以在留言区进行评论，也可以在泡泡机器人论坛：http://paopaorobot.org/bbs/index.php?c=cate&fid=1中的SLAM技术交流模块发帖提问。
我的github链接是：https://github.com/yepeichu123/orbslam2_learn。
