😀 ORB-SLAM2 代码解读（二）：闭环检测线程

0. 闭环检测线程介绍

  通过检测闭环来消除 SLAM 系统的累计误差是比较直接且有效的方式，在局部建图线程处理完每一帧关键帧序列之后会将该关键帧保存到 mlploopKeyFrameQueue 队列中送到闭环检测线程，进行闭环检测（BoW 二进制词典匹配检测，通过 Sim3 算法计算相似变换），发现闭环之后进行闭环矫正（闭环融合和图优化）。

0.1 闭环检测线程创建与运行

• 闭环检测线程的创建与运行在初始化 SLAM 系统时

  1 2 3 4 5 6

  mpLoopCloser = new LoopClosing( mpMap, //地图 mpKeyFrameDatabase, //关键帧数据库 mpVocabulary, //ORB字典 mSensor!=MONOCULAR); //当前的传感器是否是单目 mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, //线程的主函数 mpLoopCloser); //该函数的参数

• 闭环检测线程并行地运行在 ==LoopClosing::Run()== 函数中
  • 步骤一： 判断闭环检测队列 mlpLoopKeyFrameQueue 中的关键帧是否为空 ==LoopClosing::InsertKeyFrame(KeyFrame *pKF)==；
  • 步骤二： 进行闭环检测 ==LoopClosing::DetectLoop()==；
  • 步骤三： 计算闭环处两帧的相似变换 ==LoopClosing::ComputeSim3()==；
  • 步骤四： 进行闭环矫正，全局 BA 优化 ==LoopClosing::CorrectLoop()==。

1. 闭环检测

  闭环检测在函数 ==LoopClosing::DetectLoop()== 中实现。

• 步骤一： 从 mlpLoopKeyFrameQueue 队列中取出一个关键帧，使用互斥锁，保证不与局部建图线程送入关键帧冲突，并且在未处理完成之前将关键帧的属性设置为不可移除

  1 2 3 4 5 6 7 8

  { // 从队列中取出一个关键帧 unique_lock<mutex> lock(mMutexLoopQueue); mpCurrentKF = mlpLoopKeyFrameQueue.front(); mlpLoopKeyFrameQueue.pop_front(); // Avoid that a keyframe can be erased while it is being process by this thread mpCurrentKF->SetNotErase(); }

• 步骤二： 如果距离上次闭环没多久（小于10帧），或者 map 中关键帧总共还没有 10 帧，则不进行闭环检测
• 步骤三： 遍历所有共视关键帧，计算当前关键帧与每个共视关键帧的 bow 相似度得分，并得到最低得分 minScore，这是一个阈值，如果得分比这个最低得分还要低的话，那么就是说肯定不会成为闭环关键帧；
• 步骤四： 在所有关键帧中找出闭环备选帧，在函数 ==KeyFrameDatabase::DetectLoopCandidates== 中进行；
  • 步骤 1： 找出和当前帧具有公共单词的所有关键帧（不包括与当前帧链接的关键帧）;
  • 步骤 2： 统计所有闭环候选帧中与当前帧具有共同单词最多的单词数，并以这个最多单词数的 0.8 倍作为一个阈值 minCommonWords；
  • 步骤 3： 遍历所有闭环候选帧，挑选出共有单词数大于 minCommonWords 且单词匹配度大于 minScore 的关键帧存入 list<pair<float,KeyFrame*> > lScoreAndMatch;；
  • 步骤 4： 单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分，选出最高的组得分，然后以此的0.75 倍作为阈值 minScoreToRetain，得分大于这个阈值的组，作为得分较高的组；
  • 步骤 5： 只返回累计这些得分较高的组中分数最高的关键帧，完成闭环候选帧的获取。
• 步骤五： 在候选帧中检测具有连续性的候选帧
  • 思想：每次在上一步的数据库查询操作里找到了候选关键帧之后，基本上找到的候选关键帧就是我们所要找的闭环关键帧，但是为了防止错误进行闭环检测，非常有必要再进行一次连续性检测，连续性检测的意思就是，是否在三个当前的关键帧内都同时发现了某一个闭环候选帧的话，那么就表明当前的 SLAM 系统已经闭环;
  • 步骤 1： 每个候选帧将与自己相连的关键帧构成一个子候选组 spCandidateGroup；
  • 步骤 2： 检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在nCurrentConsistency++；
  • 步骤 3： 如果 nCurrentConsistency 大于等于3，那么该子候选组代表的候选帧存在连续性。

2. 相似变换

  闭环检测在函数 ==LoopClosing::ComputeSim3()== 中实现。

• 步骤一： 从上一步闭环检测筛选处的闭环候选帧中逐帧取出
  • 步骤 1： 将当前取出的闭环候选帧设置为不可移除，防止在 LocalMapping 线程中 KeyFrameCulling 函数将此关键帧作为冗余帧剔除；

    1 2	KeyFrame* pKF = mvpEnoughConsistentCandidates[i]; pKF->SetNotErase();

  • 步骤 2： 将当前关键帧与闭环候选关键帧进行匹配，通过 bow 加速得到 mpCurrentKF 与 pKF 之间的匹配特征点，vvpMapPointMatches 是匹配特征点对应的 MapPoints

    1	int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);

    • 如果匹配数少于 20 则跳过；
    • 否则，则通过匹配点对构造 Sim3 求解器 Sim3Solver::Sim3Solver() 将上一步得到的匹配的mappoint对，表达成各自坐标系下的空间点，并变化为像平面下的坐标，并设置 Ransac 参数；

      1 2 3 4 5

      // STEP 构造Sim3求解器 // 如果mbFixScale为true，则是6DoFf优化（双目 RGBD），如果是false，则是7DoF优化（单目） Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale); pSolver->SetRansacParameters(0.99,20,300);// 至少20个inliers 300次迭代 vpSim3Solvers[i] = pSolver;

• 步骤二： 迭代上一步筛选出的闭环候选关键帧，用相似变换求解器求解闭环候选帧到当前关键帧的相似变换，在函数 ==Sim3Solver::iterate()== 中进行，最多迭代 5 次，返回的是两帧之间的 Sim3 变换（T12）。

  1 2 3 4

  // 对步骤2中有较好的匹配的关键帧求取Sim3变换 Sim3Solver* pSolver = vpSim3Solvers[i]; // 最多迭代5次，返回的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换（T12） cv::Mat Scm = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

  • 步骤 1： 当函数的迭代次数不超过 5 次或总迭代次数不超过 300 次，就任意取三组点，然后根据两组匹配的 3D 点，计算之间的 Sim3 变换，求解过程在 ==Sim3Solver::ComputeSim3()== 函数中进行；
  • 步骤 2： 通过相似变换得到的变换矩阵进行投影，检查内点，在函数 ==Sim3Solver::CheckInliers()== 中实现；
• 步骤三： 通过步骤二求取的 Sim3 变换引导关键帧匹配弥补步骤一中的漏匹配，在函数 ==ORBmatcher::SearchBySim3()== 中进行；
  • 查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数，之前使用 SearchByBoW() 进行特征点匹配时会有漏匹配）,通过 Sim3 变换，确定 pKF1 的特征点在 pKF2 中的大致区域，同理，确定 pKF2 的特征点在 pKF1 中的大致区域，在该区域内通过描述子进行匹配捕获 pKF1 和 pKF2 之前漏匹配的特征点，如果成功对上了，则认为是互相匹配上了，更新匹配vpMapPointMatches。
• 步骤四： 进行 Sim3 优化，只要有一个候选帧通过 Sim3 的求解与优化，就跳出停止对其它候选帧的判断，在函数 ==Optimizer::OptimizeSim3()== 中进行；

  1	const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);// 卡方chi2检验阈值

  • 如果 mbFixScale 为 true，则是 6DoFf 优化（双目 RGBD），如果是 false，则是 7DoF 优化（单目），优化 mpCurrentKF 与 pKF 对应的 MapPoints 间的 Sim3，得到优化后的量gScm；
  • 如果优化后，通过卡方检验的内点数超过 20 的话，那么 mpMatchedKF 就是最终闭环检测出来与当前帧形成闭环的关键帧，得到从世界坐标系到该候选帧的 Sim3 变换，Scale=1， 得到g2o 优化后从世界坐标系到当前帧的 Sim3 变换；
  • 只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断；
  • 如果没有一个闭环匹配候选帧通过Sim3的求解与优化，清空 mvpEnoughConsistentCandidates；
• 步骤五： 取出闭环匹配上关键帧的相连关键帧，得到它们的地图点 MapPoints 放入 mvpLoopMapPoints；
• 步骤六： 将闭环匹配上关键帧以及相连关键帧的，地图点 MapPoints 投影到当前关键帧进行投影匹配为当前帧查找更多的匹配，实现在 ==ORBmatcher::SearchByProjection()== 函数中；
• 步骤七： 统计当前帧与检测出的所有闭环关键帧的 MapPoints 匹配点数；
  • 如果大于 40 则匹配成功，清空候选关键帧队列；
  • 否则跳过该关键帧。
• 至此完成了对闭环候选帧的选取。

3. 闭环矫正全局 BA

  闭环矫正在函数 ==LoopClosing::CorrectLoop()== 中实现。

• 步骤一： 请求局部地图停止，防止局部地图线程中 InsertKeyFrame 函数插入新的关键帧；

  1	mpLocalMapper->RequestStop();

• 步骤二： 根据共视关系更新当前帧与其它关键帧之间的连接，在函数 ==KeyFrame::UpdateConnections()== 中更新；
• 步骤三： 通过位姿传播，得到 Sim3 优化后与当前帧相连的关键帧的位姿，以及它们的 MapPoints；
  • 步骤 1： 通过位姿传播，得到 Sim3 调整后其它与当前帧相连关键帧的位姿（只是得到，还没有修正）；
  • 步骤 2： 上一步得到调整相连帧位姿后，修正这些关键帧的 MapPoints；
  • 步骤 3： 将 Sim3 转换为 SE3，根据更新的 Sim3，更新关键帧的位姿；
  • 步骤 4： 根据共视关系更新当前帧与其它关键帧之间的连接；
• 步骤四： 检查当前帧的 MapPoints 与闭环匹配帧的 MapPoints 是否存在冲突，对冲突的 MapPoints进行替换或填补

  1 2 3 4 5 6 7 8

  if(pCurMP)// 如果有重复的MapPoint（当前帧和匹配帧各有一个），则用匹配帧的代替现有的 pCurMP->Replace(pLoopMP); else// 如果当前帧没有该MapPoint，则直接添加 { mpCurrentKF->AddMapPoint(pLoopMP,i); pLoopMP->AddObservation(mpCurrentKF,i); pLoopMP->ComputeDistinctiveDescriptors(); }

• 步骤五： 通过将闭环时相连关键帧的 mvpLoopMapPoints 投影到这些关键帧中，进行 MapPoints 检查与替换，在函数 ==LoopClosing::SearchAndFuse()== 中实现；
  • 遍历闭环相连的关键帧，将闭环相连帧的 MapPoints 坐标变换到当前帧坐标系，然后投影，检查冲突并融合，在函数 ==ORBmatcher::Fuse()== 中实现;
• 步骤六： 更新当前关键帧之间的共视相连关系，得到因闭环时 MapPoints 融合而新得到的连接关系
• 步骤七： 进行 Essential Graph 优化，LoopConnections 是形成闭环后新生成的连接关系，不包括下一步中当前帧与闭环匹配帧之间的连接关系，优化在函数 ==Optimizer::OptimizeEssentialGraph()== 中进行；
• 步骤八：添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）

  1 2	mpMatchedKF->AddLoopEdge(mpCurrentKF); mpCurrentKF->AddLoopEdge(mpMatchedKF);

• 步骤九： 新建一个线程用于全局 BA 优化，在函数 ==LoopClosing::RunGlobalBundleAdjustment()== 中进入；

【R】参考资料

• ORB-SLAM2详解（六）闭环检测
• ORBSlam2学习研究(Code analysis)-ORBSlam2中的闭环检测和后端优化LoopClosing
• orb_slam代码解析(4)LocalClosing线程

【Q】问题

• ORBmatcher::SearchForTriangulation() 函数判断两帧之间点是否满足极限约束，没有详细展开；
• ORBmatcher::Fuse() 相邻关键帧之间的特征点融合没有展开，后续补上；
• Sim3Solver::ComputeSim3() sim3 求解的代码对照论文公式，没展开；
• Optimizer::OptimizeSim3() sim3 优化没有展开，待补；
• Optimizer::OptimizeEssentialGraph 闭环矫正时的 Essential Graph 优化，没展开；
• Optimizer::GlobalBundleAdjustemnt() 全局 BA 优化；

2019.05.30
wuyanminmax@gmail.com