开源SLAM框架学习——OpenVSLAM源码解析：全局优化模块（global optimization module）：回环检测、pose-graph优化、global-BA优化

这篇博客主要介绍OpenVSLAM的全局优化模块（global_optimization_module），该模块是单独运行在一个线程中的。它主要执行的工作是：SLAM的回环检测，以及回环成功之后的回环矫正，还包括紧随着回环检测成功之后的pose graph优化和全局的BA优化。

1.全局优化模块入口

说是全局优化模块，其实也就是一个普通的类。对于类，第一步肯定就是实例化构造对象，全局优化模块的实例化过程是在system.cc代码中的system的构造函数处，对应的代码的如下：

global_optimizer_ = new global_optimization_module(map_db_, bow_db_, bow_vocab_, 
															                                                      camera_->setup_type_ != camera::setup_type_t::Monocular);

可见全局优化模块和其它模块共用的数据包括map_db_（地图的数据库）、bow_db_（词袋数据库）、bow_vocab_（词袋模型），最后一个参数来控制优化时是否优化尺度（scale），对于单目由于存在尺度漂移，所以需要优化尺度。

至此，全局优化模块构造完成。另外由于它需要在一个独立的线程中运行，所以还需要新建一个线程，将该对象中的代码运行在这个线程中。新建线程的代码在system.cc文件中的system::startup()函数中，对应的代码如下：

global_optimization_thread_ = std::unique_ptr<std::thread>(new std::thread(&openvslam::global_optimization_module::run, global_optimizer_));

注意std::thread运行对象中函数的用法！

现在计算机中有一个线程在不停地运行openvslam::global_optimization_module::run()中的代码！

2.全局优化模块线程在干啥？

经过简单地配置，全局优化线程就欢脱的跑起来了，现在就让我们来看看它都具体干了啥？首先祭出一张流程图：

全局优化线程是一个死循环，只要程序正常运行，它将不停地在做同样的重复性工作，好无聊啊！

（1）首先，先检查一下SLAM的状态，确保没有收到SLAM要暂停、停止或者重启的命令！

这里命令的发出主要是通过显示模块中在pangolin的窗口上的那几个按钮！

（2）从关键帧队列中(keyfrms_queue_)，取出最前面的一帧关键帧。如果关键帧队列中啥东西都没有，那怎么办呢？哎，这一趟循环了个屁，重新循环吧！心累

keyfrms_queue_这个成员变量主要用来存储关键帧，用于回环检测。其中关键帧数据主要是来自建图模块(mapping_module)的输入，只要建图模块成功的生成了一个关键帧，那么就将关键帧放入这个队列。

（3）将队列中取出来的这一个关键帧，与地图数据集中的所有关键帧进行比较，找到和自己的相似的关键帧，将其作为备选的回环关键帧。如果找到了，那就继续往下执行，没有找到的话，又是一个无用的循环，那就回到开始，重新下一次循环吧！

这一步对应的函数是loop_detector::detect_loop_candidates()，这是一个重要的函数，下面的内容单独讲解这个函数。

（4）检测到了一些和自己相似的回环候选帧之后，需要验证这些候选关键帧是不是确实是一个真正的回环。如果经过验证这些候选回环关键帧中没有真正的回环帧，那么这次循环就结束了，重新循环！

这一步对应的函数loop_detector::validate_candidates()，它也是一个重要的函数，后面会专门介绍。

（5）经过验证之后，确实是一个成功的回环，那么就需要执行回环矫正！

这一步对应的函数global_optimization_module::correct_loop()，这个函数后面介绍！

以上步骤中，一共出现了三个很重要的函数：

• loop_detector::detect_loop_candidates()
• loop_detector::validate_candidates()
• global_optimization_module::correct_loop()

接下来我们一个一个去学习！

2.1 检测回环候选帧

首先来看第一个函数loop_detector::detect_loop_candidates()，它主要是用来进行回环候选帧的检测，经过它的检测之后，可以获得很多的回环候选帧，这些候选帧中也许有真正的回环帧，也许没有！让我们来看看它都做了啥？首先祭出它的流程图：

让我向你具体介绍它都做了啥吧！

（1）首先，检测是否开启回环检测功能，另外如果距离上一次回环检测成功还没有超过10个关键帧，那就先不要进行回环检测，不然的话太频繁了，也不符合实际情况！如果不满足这两个条件，那就退出这个函数！

你可以想象一下，刚做完回环检测，你又回到原地的情况应该还是比较少见的吧！

（2）根据共视关系计算回环检测的最小评分。可以想象一下，如果拿着当前帧到地图数据库中去找和我比较相似的，那怎么判断拿个和我算比较相似，那个算比较不相似呢？如果设置的阈值过高可能错过了回环，如果阈值过低可能会检测到很多候选帧。于是作者这里的策略是把当前帧的共视关键帧取出来，以此作为判断依据，将共视关键帧中和自己bow评分最低的那个得分最为评判回环候选帧的阈值。

这一步操作对应的函数是loop_detector::compute_min_score_in_covisibilities()。这一点你是可以想象的，既然是回环帧，那至少和我们看起来还是比较相似的，至于取用共视关键帧中的最低评分，我估计作者是做了大量的实验之后，得出来的一个经验做法！

（3）根据上一步计算出来的评分，去地图数据库进行查找，那些只要和当前帧相似评分大于这个最低阈值的关键帧，都将作为回环候选帧取出来。

（4）如果没有得到任何的回环候选帧，那么结束当前函数。

（5）将每一个回环候选帧以及这个候选帧对应的所有共视关键帧打包组成一个集合（类似于下图虚线框起来的那样），以备后用。注意，这里其实有一个连续性检测条件，也就是算法认为，如果确实是一个真正的回环，那么应该是可以连续被检测到的，不可能上次检测到，这一次检测不到了。
　　这个理解起来还是比较难的，我通过一张向你解释一下这个过程吧！

上图中的三角形和圆形都是关键帧，为了以示区分我把它画成关键帧加共视关键帧组合的形式（图中虚线框起来的那样）。红色的三角形是当前的关键帧，它经过回环候选帧检测之后，获得了上图所示的四个虚线框组成的组合。连续性检测认为，如果下一次再进行回环候选帧检测，检测出来的组合和上一次检测出来的组合没有交集，那么它们就都不是可能的候选回环帧。例如，上图是本次的回环候选帧检测结果，下一次检测得到的回环候选帧组合与当前检测结果没有相交（所谓的相交，就是虚线框中这样的组合之间是否有公共的关键帧），则这两次结果里都不存在回环候选帧。

你品，你细品！

（6）如果打包的关键帧集合连续被观测到三次，也就是说当前观测组成的回环帧集合，下一次又被检测到，总共被检测到三次，那么这个回环候选关键帧很可能是真正的回环帧，将其记录下来。

注意，如果连续性检测某一次失败了，那么之前累计的连续性结果也都归零，重新开始新的连续性检测。

通过以上算法之后，可能会获得很多回环候选帧，下一步就需要进行验证到底是不是真的回环！

2.2 验证回环候选帧

经过检测现在已经获得了很多回环候选帧，loop_detector::validate_candidates()函数主要就是用来对回环候选帧中的关键帧做检验，找到其中真正的回环帧。它的原理主要是做几何校验，验证其是否满足投影关系。

废话不多说，首先祭出流程图：

（1）首先，对候选关键帧与当前关键帧进行Sim3变换的几何校验。找到满足投影关系的回环候选关键帧。这一步对应的函数是：loop_detector::select_loop_candidate_via_Sim3()它的主要算法过程如下：

• 通过bow加速搜索，获得候选关键帧与当前关键帧的匹配特征点；
• 如果匹配的特征点少于20个，则说明当前候选关键帧不是一个好的候选关键帧，结束当前循环，进行下一次循环；
• 根据搜索出来的匹配特征点对，求解当前关键帧到候选关键帧的Sim3变换，内点数量小于20，则认为不是一个好的候选帧；
• 通过计算出来的Sim3变换，将两个关键帧对应的地图点相互向对方关键帧投影，获得更多的匹配关系；
• 对获得的匹配关系进行优化，获得更加准确的结果；
• 如果最终得到的内点数量大于20个，则认为当前候选关键帧是一个真正的回环帧，否则进行下一次循环，对下一个候选帧执行上述步骤。

（2）对上一步找到的回环帧，取出它的所有共视关键帧，以及共视关键帧赌赢的地图点。
（3）将上一步获得的地图点，全部向当前关键帧进行投影，获得更多的匹配关系，然后再重新计算一次Sim3变换，如果最终的匹配点对数量大于40个，则又进一步印证了当前检测出来的回环帧确实是一个回环帧。

经过上面的回环帧验证之后，我们获得的回环帧，也确实是一个真正的、可靠的回环帧了，下面该要执行的就是通过这个回环帧去消除掉累计误差了！

2.3 进行回环矫正

回环矫正对应的函数是：global_optimization_module::correct_loop()。它其中有一些细节还是比较难的，你可能需要关注一下它是怎么处理那些重复的关键帧，以及重复的地图点，怎么构建一个pose graph优化！

废话不多说，继续祭出我亲手画的流程图：

（1）首先，将我们辛辛苦苦取得的回环帧取出来，它可是咱们的宝贝啊，千呼万唤才弄出来的。
（2）如果目前系统中还正有一个回环全局优化在执行，就关闭它。毕竟又出现了新的回环帧之后，前面的累计误差可以消除，在此之前的别的回环优化都是浪费。
（3）确保mapp_module模块已经暂停。
（4）更新一下当前关键帧的连接关系，包括共视关系、权重等等。
（5）将当前关键帧以及它的共视关键帧，通过之前就算出来的Sim3变换进行位姿修正，并且也要将地图点也修正到它本该在的地方。
（6）替换当前关键帧以及共视关键帧和回环关键帧以及共视关键帧重复的地图点。

你可以想象一下，既然是回环成功了，那肯定说明相机回到了以前出现过的地方，那么它们必然看到一样的地图点，但是由于存在累计误差，同样的地图点却出现了不同的坐标，所以是不是应该需要删除这些重复的地图点呢？如果当前帧和回环帧有共同的地图点，用回环帧中的地图点替换当前帧的地图点。而对于

（7）经过地图点的替换处理之后，当前关键帧和回环关键帧的共视关系发生了一些变换，将当前关键帧中新的共视关键帧取出来。

对于当前关键帧和共视关键帧，它们其实是很相似的，所以只要保留回环关键帧就行，并且当前关键帧中的共视关键帧其实也不需要了。

（8）处理完这些地图点和关键帧的关系之后，需要进行一个pose graph优化，它只优化共视关键帧中权重大于100的边，将回环之后的得到的累计误差平分给每一个关键帧。
（9）重新开一个线程，执行全局的BA优化。
（10）一次完整的回环检测结束了，打开mapping_module模块继续进行建图工作。

3. 总结

以上就是一个完整的全局优化过程，我们从其中可以发现，它的回环检测策略十分严格，因为检测的成功率必须保证100%，一旦检测错误那么对于地图将是毁灭性的打击。不过从另一个角度来说，回环检测的逻辑并不怎么优美，执行了很多重复的耗时的操作。我甚至怀疑，为了追求100%的成功率，设计的算法过于复杂了。如果使用CNN的思路，那么回环检测将会是一个非常简单的策略，说明人工提取的特征还是远不及神经网络提取出的特征点鲁棒性高，我觉得将来肯定会有人在这方面做出一些可行的工作，取代现在的回环检测策略，越简单则会越优美！

回环检测的代码我不推荐你每一行都看的明明白白，我觉得看个大概，知道思路就行了，因为这种方案不具备移植性，基于ORB特征点的SLAM系统用这个方案可行，如果换成别的特征点可能这种方法就不一定可行了。

该算法基本延续了ORB-SLAM的回环检测思路，在策略上没有什么变化！