ORBSlam2学习研究(Code analysis)-ORBSlam2中的闭环检测和后端优化LoopClosing

ORBSlam2中的闭环检测和后端优化LoopClosing

LoopClosing在现在的Slam系统中是非常重要的一个部分，VO总是会有累计误差，而LoopClosing通过检测是否曾经来过此处，进行后端优化，可以将这个累计误差缩小到一个可接受的范围内。从而使得Slam系统应对大范围场景时，拥有更高的鲁棒性和可用性。

ORBSlam2中的LoopClosing闭环检测线程主要进行闭环检测，并在检测到闭环的时候计算Sim3变换，进行后端优化。

检测闭环 DetectLoop

首先我们会检测当前关键帧在Covisibility图中的附近关键帧，并会依次计算当前关键帧和每一个附近关键帧的BoW分值，通过我们所得到分数的最低分，到数据库中查询，查找出所有大于该最低分的关键帧作为候选帧，用以检测闭环。

1. 如果地图中的关键帧数小于10，那么不进行闭环检测

2. 获取共视关键帧，并计算他们和当前关键帧之间的BoW分数，求得最低分

3. 通过上一步计算出的最低分数到数据库中查找出候选关键帧，这一步相当于是找到了曾经到过此处的关键帧们

这一步是非常重要的，相当于是在为闭环检测做前期的预处理

1. 首先，得到与当前帧的链接的关键帧。
   set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();

2. 然后，在地图中搜索与当前关键帧共享一个BOW word的关键帧，并排除上一步搜集到的附近关键帧，得到候选帧，这些候选帧基本上都是曾经来到此处看到的。
   

3. 统计这些帧中，与当前关键帧的Bow共有Word最多的单词数maxCommonWords。
   

4. 计算最低共有单词数阈值minCommonWords = maxCommonWords*0.8f，并搜寻候选帧中，共有单词数大于minCommonWords的关键帧，并计算它与当前帧的score分值。
   

5. 将这些候选帧进行聚类，相连的候选帧聚为同一类，并计算每一组的累计得分，得到最高累计分的组，并得分数最高组的最高分的关键帧。
   这样的话，会把一些拥有很高分数的独立出来的关键帧给去掉，因为他并没有跟其他关键帧相连，没有连续性，所以这一步就是去除这些得分很高的错误关键帧。
   

6. 得到最后一个阈值 minScoreToRetain = 0.75f*bestAccScore，再通过这个最低阈值，计算出这些候选帧中比这个分值高的关键帧，并保存起来返回。
   

4. 对候选关键帧集进行连续性检测(有的文章也翻译为一致性检测)

这一步相当于是检测上一步得到的关键帧集是否是真的可以使用的。
（由于这段网络上很少有介绍，所以这部分我是按照我自己的理解写的，如果有错误，希望有人能够指出）

闭环连续性是什么

每次我们在上一步的数据库查询操作里找到了候选关键帧之后，基本上找到的候选关键帧就是我们所要找的闭环关键帧，但是为了防止错误进行闭环检测，我们非常有必要再进行一次连续性检测，连续性检测的意思就是，是否我们在三个当前的关键帧内都同时发现了某一个闭环候选帧的话，那么就表明当前的SLAM系统已经闭环。

详细解释

比方说，在上图中，通过数据库查询，我们可以在A点得到闭环候选关键帧有两个(1,2)。在下一次进入DetectLoop函数的时候，我们当前拿到的关键帧是B，那么在B点我们可以得到的闭环候选关键帧是(1,2,3)，以此类推，在再下一次进入DetectLoop函数的时候，也就是在C点的时候，我们这时对比到的闭环候选关键帧是(2,3)，所以2这个闭环候选关键帧被检测到了三次。在LoopClosing中，mnCovisibilityConsistencyTh = 3 一致性共视阈值被设为3，并且如果一旦有一个闭环候选关键帧被检测到3次，系统就认为检测到闭环。

在每一次进行完闭环候选连续性检测之后，该线程都会保存在这一关键帧下的计数情况，保存的变量也就是mvConsistentGroups，以供给下一轮循环使用。

计算Sim3 ComputeSim3

既然上一步我们已经检测到了闭环，那么我们现在就需要开始进行后端优化了。该函数主要工作就是在当前关键帧和闭环帧之间找到更多的对应点，并通过这些对应点计算当前关键帧和闭环帧之间的Sim3变换，求解出Rt和s。在这一过程中，共进行了三次对应点的查找工作。

1. 对每一个闭环帧，通过BoW的matcher方法进行第一次匹配，匹配闭环帧和当前关键帧之间的匹配关系，如果对应关系少于20个，则丢弃，否则构造一个Sim3求解器并保存起来。这一步主要是对效果较好的闭环帧构建Sim3求解器
   

2. 对上一步得到的每一个满足条件的闭环帧，通过RANSAC迭代，求解Sim3。
   对于这里的Sim3的Ransac迭代，我在网上也没有找到很多的解释，我们都知道Ransac是根据一组包含异常数据的样本数据集，计算出数据的数学模型参数的方法，在这里使用Ransac的方法，可以大大提高对杂点干扰的鲁棒性。
   

3. 通过返回的Sim3进行第二次匹配。
   刚才得到了Sim3，所以现在要利用Sim3再去进行匹配点的查找，本次查找的匹配点数量，会在原来的基础上有所增加。
   

4. 使用非线性最小二乘法优化Sim3.
   在拿到了第二次匹配的结果以后，要通过这些匹配点，再去优化Sim3的值，从而再精细化Rt和s。
   const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);

5. 恢复闭环关键帧和其邻居关键帧的MapPoint地图点
   最后一步求解匹配点的时候，将所指的闭环帧和与其链接的关键帧所看到的所有的MapPoint都恢复出来。通过这个方法，可以尽可能得到我们当前关键帧所能看到的所有的地图点，为下一步做投影匹配，得到更多的匹配点做准备。
   

6. 使用投影得到更多的匹配点，如果匹配点数量充足，则接受该闭环。
   最后我们通过投影匹配得到了尽可能多的匹配点，通过匹配点的数量判断是否接受闭环。
   

纠正闭环后端优化 CorrectLoop

在上一步求得了Sim3和对应点之后，就纠正了当前帧的位姿，但是我们的误差不仅仅在当前帧，此前的每一帧都有累计误差需要消除，所以这个函数CorrectLoop就是用来消除这个累计误差，进行整体的调节。

1. 如果有全局BA运算在运行的话，终止之前的BA运算。
   

2. 使用传播法计算每一个关键帧正确的Sim3变换值

   • 得到当前关键帧的附近关键帧的Sim3位姿并用纠正的Sim3位姿与其相乘，保存结果到CorrectedSim3变量中。
     

   • 使用反向投影的方法，将当前关键帧和邻居观察到的地图点得到三维场景下的位姿，并更新关键帧的位姿
     

   • 将当前关键帧的地图点进行融合，其实融合就是判断如果是同一个点的话，那么将当前的地图点强制换成原本的地图点。
     

   • 使用已纠正的位姿，将在循环关键帧附近观察到的地图点投影到当前的关键帧和邻居，融合重复点。
     

   • 更新链接，检测新连接
     

3. 优化图
   Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
   使用非线性最小二乘图优化的方法来优化EssentialGraph。

4. 全局BA优化
   

参考链接
LoopClosing中的Sim3求解 http://www.cnblogs.com/shang-slam/p/6480863.html
ORBSlam2闭环检测 http://blog.csdn.net/u010128736/article/details/53409199
ORBSlam2原文翻译 http://www.sohu.com/a/154011668_715754
泡泡机器人ORBSlam2解析 http://rosclub.cn/post-505.html