ORB-SLAM3源码学习：LoopClosing.cc：LoopClosing::NewDetectCommonRegions检测共同区域

前言

在ORB-SLAM2中校验闭环候选关键帧时，需要满足时序上连续3次成功校验才能通过。这就需要检测至少3个新进来的关键帧，这种方法牺牲了召回率来提升精度。由于必须严格的满足至少连续的三个条件这使得闭环的条件更加严格，即使存在真实的闭环但是由于环境改变，视角发生大的改变导致的误匹配也会使得不能形成闭环导致召回率的下降。而在ORB-SLAM3中，采用了新的位置识别算法，该算法首先检查几何一致性，也就是当前关键帧的5个共视关键帧（已经在地图中）中只要有3个满足条件（和候选关键帧组匹配成功），即可认为检测到共同区域。如果不够3个满足条件，则再检查后续新进来的关键帧（不在地图中）的时间一致性。这种策略在最理想的情况下，不需要等待新进来的关键帧就可以完成验证，不仅提高了召回率，也提高了地图精度，不过计算量也略有增加。

1.函数声明

bool LoopClosing::NewDetectCommonRegions()

2.函数定义

/*检测有没有共同区域,包括检测回环和融合匹配,sim3计算,验证对应于ORB-SLAM2里的函数DetectLoop
*/
bool LoopClosing::NewDetectCommonRegions()
{// To deactivate placerecognition. No loopclosing nor merging will be performed// 如果一开始就不做回环的话这里就退出了，这个线程也就名存实亡了if(!mbActiveLC)return false;{// Step 1 从队列中取出一个关键帧,作为当前检测共同区域的关键帧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();mpCurrentKF->mbCurrentPlaceRecognition = true;// 当前关键帧对应的地图mpLastMap = mpCurrentKF->GetMap();}// Step 2 在某些情况下不进行共同区域检测// 1.imu模式下还没经过第二阶段初始化则不考虑if(mpLastMap->IsInertial() && !mpLastMap->GetIniertialBA2()){mpKeyFrameDB->add(mpCurrentKF);mpCurrentKF->SetErase();return false;}// 2.双目模式下且当前地图关键帧数少于5则不考虑if(mpTracker->mSensor == System::STEREO && mpLastMap->GetAllKeyFrames().size() < 5) //12{// cout << "LoopClousure: Stereo KF inserted without check: " << mpCurrentKF->mnId << endl;mpKeyFrameDB->add(mpCurrentKF);mpCurrentKF->SetErase();return false;}// 3.当前地图关键帧少于12则不进行检测if(mpLastMap->GetAllKeyFrames().size() < 12){// cout << "LoopClousure: Stereo KF inserted without check, map is small: " << mpCurrentKF->mnId << endl;mpKeyFrameDB->add(mpCurrentKF);mpCurrentKF->SetErase();return false;}//cout << "LoopClousure: Checking KF: " << mpCurrentKF->mnId << endl;//Check the last candidates with geometric validation// Loop candidates// Step 3 基于前一帧的历史信息判断是否进行时序几何校验,注意这里是基于共视几何校验失败才会运行的代码,阅读代码的时候可以先看后面bool bLoopDetectedInKF = false;  // 某次时序验证是否成功bool bCheckSpatial = false;#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_StartEstSim3_1 = std::chrono::steady_clock::now();
#endif// 实际执行顺序3是在2没完成时就开始了，当2完成时就不执行3了。// 实际执行顺序3// Step 3.1 回环的时序几何校验。注意初始化时mnLoopNumCoincidences=0, 所以可以先跳过看后面// 如果成功验证总次数大于0if(mnLoopNumCoincidences > 0){bCheckSpatial = true;// Find from the last KF candidates// 通过上一关键帧的信息,计算新的当前帧的sim3 // Tcl = Tcw*TwlSophus::SE3d mTcl = (mpCurrentKF->GetPose() * mpLoopLastCurrentKF->GetPoseInverse()).cast<double>();g2o::Sim3 gScl(mTcl.unit_quaternion(),mTcl.translation(),1.0);g2o::Sim3 gScw = gScl * mg2oLoopSlw;int numProjMatches = 0;vector<MapPoint*> vpMatchedMPs;// 通过把候选帧局部窗口内的地图点向新进来的关键帧投影来验证回环检测结果,并优化Sim3位姿//669行定义的函数实现几何一致性验证bool bCommonRegion = DetectAndReffineSim3FromLastKF(mpCurrentKF, mpLoopMatchedKF, gScw, numProjMatches, mvpLoopMPs, vpMatchedMPs);// 如果找到共同区域(时序验证成功一次)if(bCommonRegion){//标记时序检验成功一次bLoopDetectedInKF = true;// 累计正检验的成功次数mnLoopNumCoincidences++;// 不再参与新的回环检测mpLoopLastCurrentKF->SetErase();// 将当前关键帧作为上次关键帧mpLoopLastCurrentKF = mpCurrentKF;mg2oLoopSlw = gScw;  // 记录当前优化的结果为{last T_cw}即为 T_lw// 记录匹配到的点mvpLoopMatchedMPs = vpMatchedMPs;// 如果验证数大于等于3则为成功回环mbLoopDetected = mnLoopNumCoincidences >= 3;// 记录失败的时序校验数为0mnLoopNumNotFound = 0;//! 这里的条件反了,不过对功能没什么影响,只是打印信息if(!mbLoopDetected){cout << "PR: Loop detected with Reffine Sim3" << endl;}}// 如果没找到共同区域(时序验证失败一次)else{// 当前时序验证失败bLoopDetectedInKF = false;// 递增失败的时序验证次数mnLoopNumNotFound++;// 若果连续两帧时序验证失败则整个回环检测失败if(mnLoopNumNotFound >= 2){// 失败后标记重置一些信息mpLoopLastCurrentKF->SetErase();mpLoopMatchedKF->SetErase();mnLoopNumCoincidences = 0;mvpLoopMatchedMPs.clear();mvpLoopMPs.clear();mnLoopNumNotFound = 0;}}}//Merge candidatesbool bMergeDetectedInKF = false;// 实际执行顺序3// Step 3.2 融合的时序几何校验: 注意初始化时mnMergeNumCoincidences=0, 所以可以先跳过看后面// mnMergeNumCoincidences表示成功校验总次数，初始化为0// 会先经过后面共视几何校验，如果小于3，会进到如下判断开始时序几何校验if(mnMergeNumCoincidences > 0){// Find from the last KF candidates// 通过上一关键帧的信息,计算新的当前帧的sim3 // Tcl = Tcw*TwlSophus::SE3d mTcl = (mpCurrentKF->GetPose() * mpMergeLastCurrentKF->GetPoseInverse()).cast<double>();g2o::Sim3 gScl(mTcl.unit_quaternion(), mTcl.translation(), 1.0);// mg2oMergeSlw 中的w指的是融合候选关键帧世界坐标系g2o::Sim3 gScw = gScl * mg2oMergeSlw;int numProjMatches = 0;vector<MapPoint*> vpMatchedMPs;// 通过把候选帧局部窗口内的地图点向新进来的关键帧投影来验证回环检测结果,并优化Sim3位姿bool bCommonRegion = DetectAndReffineSim3FromLastKF(mpCurrentKF, mpMergeMatchedKF, gScw, numProjMatches, mvpMergeMPs, vpMatchedMPs);// 如果找到共同区域(时序验证成功一次)if(bCommonRegion){// 标记时序检验成功一次 bMergeDetectedInKF = true;// 成功验证的总次数+1mnMergeNumCoincidences++;// 不再参与新的回环检测mpMergeLastCurrentKF->SetErase();mpMergeLastCurrentKF = mpCurrentKF;mg2oMergeSlw = gScw;mvpMergeMatchedMPs = vpMatchedMPs;// 如果验证数大于等于3则为成功mbMergeDetected = mnMergeNumCoincidences >= 3;}// 如果没找到共同区域(时序验证失败一次)else{// 没有找到融合mbMergeDetected = false;// 当前时序验证失败bMergeDetectedInKF = false;// 递增失败的时序验证次数mnMergeNumNotFound++;// 若果连续两帧时序验证失败则整个融合检测失败if(mnMergeNumNotFound >= 2){// 失败后标记重置一些信息mpMergeLastCurrentKF->SetErase();mpMergeMatchedKF->SetErase();mnMergeNumCoincidences = 0;mvpMergeMatchedMPs.clear();mvpMergeMPs.clear();mnMergeNumNotFound = 0;}}}  
#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_EndEstSim3_1 = std::chrono::steady_clock::now();double timeEstSim3 = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndEstSim3_1 - time_StartEstSim3_1).count();
#endif// Step 3.3 若校验成功则把当前帧添加进数据库,且返回true表示找到共同区域// 注意初始化时mbMergeDetected=mbLoopDetected=falseif(mbMergeDetected || mbLoopDetected)//如果前面的闭环检测或地图融合校验已经成功，系统确认当前帧与之前的地图存在共同区域。{
#ifdef REGISTER_TIMESvdEstSim3_ms.push_back(timeEstSim3);
#endifmpKeyFrameDB->add(mpCurrentKF);return true;}//TODO: This is only necessary if we use a minimun score for pick the best candidates// 这句并没有使用,作者把orbslam2里面通过minScore作为阀值筛选候选帧的策略抛弃了const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();// Extract candidates from the bag of words// Step 4 若当前关键帧没有被检测到回环或融合,则分别通过bow拿到当前帧最好的三个回环候选帧和融合候选帧// 实际执行顺序1vector<KeyFrame*> vpMergeBowCand, vpLoopBowCand;if(!bMergeDetectedInKF || !bLoopDetectedInKF){// Search in BoW
#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_StartQuery = std::chrono::steady_clock::now();
#endif// 分别找到3个最好的候选帧, 回环候选帧放在vpLoopBowCand中,融合候选帧放在vpMergeBowCand中// 调用函数，该函数定义在KeyFrameData.cc的源文件中。找到3个回环候选关键帧和融合候选关键帧。mpKeyFrameDB->DetectNBestCandidates(mpCurrentKF, vpLoopBowCand, vpMergeBowCand,3);
#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_EndQuery = std::chrono::steady_clock::now();double timeDataQuery = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndQuery - time_StartQuery).count();vdDataQuery_ms.push_back(timeDataQuery);
#endif}#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_StartEstSim3_2 = std::chrono::steady_clock::now();
#endif// 实际执行顺序2。// Check the BoW candidates if the geometric candidate list is empty//Loop candidates// Step 4.1 若当前关键帧没有被检测到回环,并且候选帧数量不为0,则对回环候选帧进行论文中第8页的2-5步if(!bLoopDetectedInKF && !vpLoopBowCand.empty()){// mnLoopNumCoincidences是成功几何验证的帧数，超过3就认为最终验证成功（mbLoopDetected=true），不超过继续进行时序验证// mpLoopMatchedKF 最后成功匹配的候选关键帧// 后续定义的函数mbLoopDetected = DetectCommonRegionsFromBoW(vpLoopBowCand, mpLoopMatchedKF, mpLoopLastCurrentKF, mg2oLoopSlw, mnLoopNumCoincidences, mvpLoopMPs, mvpLoopMatchedMPs);}// 实际执行顺序2.// Merge candidates// Step 4.2 若当前关键帧没有被检测到融合,并且候选帧数量不为0,则对融合候帧进行论文中第8页的2-5步if(!bMergeDetectedInKF && !vpMergeBowCand.empty()){// mnLoopNumCoincidences是成功几何验证的帧数，超过3就认为最终验证成功（mbMergeDetected=true），不超过继续进行时序验证// 后续定义的函数mbMergeDetected = DetectCommonRegionsFromBoW(vpMergeBowCand, mpMergeMatchedKF, mpMergeLastCurrentKF, mg2oMergeSlw, mnMergeNumCoincidences, mvpMergeMPs, mvpMergeMatchedMPs);}#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_EndEstSim3_2 = std::chrono::steady_clock::now();timeEstSim3 += std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndEstSim3_2 - time_StartEstSim3_2).count();vdEstSim3_ms.push_back(timeEstSim3);
#endif// Step 5 根据结果确定有没有检测到共同区域// 把当前帧添加到关键帧数据库中mpKeyFrameDB->add(mpCurrentKF);// 如果检测到一种类型共同区域返回true// 实际执行顺序4if(mbMergeDetected || mbLoopDetected){return true;}// 如果没检测到则把当前关键帧erase(不参与后续共同区域检测)mpCurrentKF->SetErase();// 标记当前关键帧不是当前在进行共同区域检测的帧mpCurrentKF->mbCurrentPlaceRecognition = false;return false;
}

 结束语

以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。