ORB-SLAM2详解（六）闭环检测

转载自 http://blog.csdn.net/u010128736/

毋庸置疑的是，随着相机的运动，我们计算的相机位姿，三角化得到的点云位置，都是有误差的，即使我们使用局部的或全局的BA去优化，仍然会存在累积误差。而消除误差最有效的办法是发现闭环，并根据闭环对所有结果进行优化。闭环是一个比BA更加强烈、更加准确的约束，所有如果能够检测到闭环，并对其优化，就可以让结果更加准确。 
  整个LoopClosing模块是在线程中完成，并在创建线程时调用LoopClosing::Run()函数让其运行。整个框架如下图所示：

一、闭环条件检测

  首先我们计算关键帧Ki和在Covisibility Graph中与其相连的关键帧之间的词袋(BOW)之间的相似度。本文中，作者离线训练了大量的基于ORB描述的词袋，在程序运行时加载进去。这里的词袋作为对该关键帧的描述，将闭环检测转变为一个类似于模式识别的问题。当相机再次来到之前到过的场景时，就会因为看到相同的景物，而得到类似的词袋描述，从而检测到闭环。这里的接口是

// Detect loop candidates and check covisibility consistency
    if(DetectLoop())

跟进函数接口，可以很清晰地看到检测闭环的过程，这里作者使用了DBOW2库。

二、计算Sim3

  单目SLAM系统存在一个问题，就是尺度的模糊性。如下图所示：

这个很好理解，因为针对一个三维点P点来说，我们在单目拍摄的两幅图中可以匹配到和，但是无法确定其在三角化里的具体位置，所以存在尺度模糊。而对于立体相机或RGBD相机，这里的尺度就可以唯一确定。所以，单目SLAM是一个Sim3相似变换群，有7个自由度，即1个尺度因子s、3个旋转角度、3个位移，即[|t]。显然，立体相机和RGBD相机中s=1。所以单目相机在优化时，是在相似变换群优化，本文是在RANSAC框架下进行优化。这个接口为：

// Compute similarity transformation [sR|t]
// In the stereo/RGBD case s=1
   if(ComputeSim3())

三、闭环融合

  闭环矫正的第一步是融合重复的点云，并且在Covisibility Graph中插入新的边以连接闭环。首先当前帧的位姿会根据相似变换而被矫正，同时所有与其相连的关键帧也会被矫正。所有的被闭环处的关键帧观察到的地图点会通过映射在一个小范围里，然后去搜索它的近邻匹配。这样就可以对所有匹配的点云进行更加有效的数据融合，并更新关键帧位姿，以及在图中的边。这部分接口为：

// Perform loop fusion and pose graph optimization
   CorrectLoop();

四、Essential Graph优化

  为了有效地完成闭环，本文使用本质图（Essential Graph）去优化位姿图。这样就可以将闭环的误差分散到整个图中。这部分主要是在CorrectLoop()中的后部分：

 // Optimize graph
    Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);

// Add loop edge
    mpMatchedKF->AddLoopEdge(mpCurrentKF);
    mpCurrentKF->AddLoopEdge(mpMatchedKF);

至此，整个ORB_SLAM2系统介绍完了。在整个记录过程中，我也学到了很多。ORB_SLAM2是一个十分优秀的项目，整个工程代码十分清晰简洁，且与论文描述高度吻合，是一个优秀的开源学习项目。另外ORB_SALM2中集合了当下各种流行的SLAM框架，所以对SLAM的学习也是具有十分重要的意义。希望越来越多的人加入计算机视觉领域，互相分享互相学习，为了更新更有意思的技术给人类带来福祉。