ORB-SLAM3的Local&MapMerging线程的代码主要集中于LocalClosing.cc的run()函数中，主要执行五个函数分别是：CheckNewKeyFrames() 函数、NewDetectCommonRegions()函数、MergeLocal()函数、MergeLocal2()函数、CorrectLoop() 函数。如果新进入的关键帧，在闭环检测的过程中，如果是当前地图下的，不用进入MergeLocal()函数和MergeLocal2()函数，直接做闭环校正；在非当前地图下，先做地图合并，再做闭环校正。

类型	函数
bool	CheckNewKeyFrames()
bool	NewDetectCommonRegions()
void	MergeLocal()
void	MergeLocal2()
void	CorrectLoop()

一、LocalClosing的run()函数

1、CheckNewKeyFrames()

检查队列mlpLoopKeyFrameQueue是否有未处理的关键帧

这个函数和局部建图线程是一样的，首先看队列里是否插入关键帧。对于局部建图线程是跟踪线程给的关键帧，那么对于闭环和地图合并线程是局部建图线程给的关键帧。所以观察是否有未处理的关键帧，如果有，往后执行。

2、NewDetectCommonRegions()

实现闭环检测

布尔函数，在函数中确定有无闭环，有闭环就地图合并。

3、MergeLocal()

非当前地图闭环情况下，纯视觉模式的地图合并与校正

纯视觉误差为重投影误差

4、MergeLocal2()

非当前地图闭环情况下，IMU模式的地图合并与校正

对IMU情况， 考虑IMU误差和视觉的重投影误差

5、CorrectLoop()

当前地图闭环情况下，闭环校正

二、代码详解

1、LoopClosing.h

（1）参数

LoopClosing(Atlas* pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale, const bool bActiveLC);

函数	功能
Atlas	多地图系统
KeyFrameDatabase	关键帧库
ORBVocabulary	ORB词袋
bFixScale	是否固定大小
bActiveLC	主动闭环检测

（2） 主函数Run()

 void Run();

检查是否有执行关键帧到闭环和地图合并线程，闭环检测，地图合并，闭环校正

（3） 插入关键帧函数InsertKeyFrame()

void InsertKeyFrame(KeyFrame *pKF);

将关键帧插入线程

（4） 运行全局BA优化函数RunGlobalBundleAdjustment()

void RunGlobalBundleAdjustment(Map* pActiveMap, unsigned long nLoopKF);

涉及参数：主动图pActiveMap和闭环关键帧nLoopKF

（5）是否有关键帧插入函数bool CheckNewKeyFrames()

核心函数

（6）检测闭环函数()bool NewDetectCommonRegions();

核心函数

有闭环返回为true，没有则为flase

（7）最后一帧中计算相似度()bool DetectAndReffineSim3FromLastKF();

从最后一帧中计算Sim3相似度，通过共视图计算相似关系

（8）通过词袋去计算相似度函数()bool DetectCommonRegionsFromBoW();

通过词袋去计算相似度

2、LoopClosing.cc

/**@brief 回环线程主函数 */void LoopClosing::Run()
{mbFinished =false;// 线程主循环 while(1){//NEW LOOP AND MERGE DETECTION ALGORITHM//----------------------------// LoopClosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的// 在LocalMapping中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue// Step 1查看闭环检测队列mlpLoopKeyFrameQueue中有没有关键帧进来 if(CheckNewKeyFrames()){// 这部分后续未使用 if(mpLastCurrentKF){mpLastCurrentKF->mvpLoopCandKFs.clear();mpLastCurrentKF->mvpMergeCandKFs.clear();}
#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_StartPR = std::chrono::steady_clock::now();
#endif// Step 2检测有没有共视的区域 bool bFindedRegion = NewDetectCommonRegions();#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_EndPR = std::chrono::steady_clock::now();double timePRTotal = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndPR - time_StartPR).count();vdPRTotal_ms.push_back(timePRTotal);
#endifif(bFindedRegion){// Step 3如果检测到融合（当前关键帧与其他地图有关联），则合并地图 if(mbMergeDetected){if ((mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) &&(!mpCurrentKF->GetMap()->isImuInitialized())){cout << "IMU is not initilized, merge is aborted" << endl;}else{Sophus::SE3d mTmw = mpMergeMatchedKF->GetPose().cast<double>();g2o::Sim3 gSmw2(mTmw.unit_quaternion(), mTmw.translation(), 1.0);Sophus::SE3d mTcw = mpCurrentKF->GetPose().cast<double>();g2o::Sim3 gScw1(mTcw.unit_quaternion(), mTcw.translation(), 1.0);g2o::Sim3 gSw2c = mg2oMergeSlw.inverse();g2o::Sim3 gSw1m = mg2oMergeSlw;mSold_new = (gSw2c * gScw1);if(mpCurrentKF->GetMap()->IsInertial() && mpMergeMatchedKF->GetMap()->IsInertial()){cout << "Merge check transformation with IMU" << endl;if(mSold_new.scale()<0.90||mSold_new.scale()>1.1){mpMergeLastCurrentKF->SetErase();mpMergeMatchedKF->SetErase();mnMergeNumCoincidences = 0;mvpMergeMatchedMPs.clear();mvpMergeMPs.clear();mnMergeNumNotFound = 0;mbMergeDetected = false;Verbose::PrintMess("scale bad estimated. Abort merging", Verbose::VERBOSITY_NORMAL);continue;}// If inertial, force only yawif ((mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) &&mpCurrentKF->GetMap()->GetIniertialBA1()){Eigen::Vector3d phi = LogSO3(mSold_new.rotation().toRotationMatrix());phi(0)=0;phi(1)=0;mSold_new = g2o::Sim3(ExpSO3(phi),mSold_new.translation(),1.0);}}mg2oMergeSmw = gSmw2 * gSw2c * gScw1;mg2oMergeScw = mg2oMergeSlw;//mpTracker->SetStepByStep(true);Verbose::PrintMess("*Merge detected", Verbose::VERBOSITY_QUIET);#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_StartMerge = std::chrono::steady_clock::now();nMerges += 1;
#endif// TODO UNCOMMENTif (mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD)MergeLocal2();elseMergeLocal();#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_EndMerge = std::chrono::steady_clock::now();double timeMergeTotal = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndMerge - time_StartMerge).count();vdMergeTotal_ms.push_back(timeMergeTotal);
#endifVerbose::PrintMess("Merge finished!", Verbose::VERBOSITY_QUIET);}vdPR_CurrentTime.push_back(mpCurrentKF->mTimeStamp);vdPR_MatchedTime.push_back(mpMergeMatchedKF->mTimeStamp);vnPR_TypeRecogn.push_back(1);// Reset all variablesmpMergeLastCurrentKF->SetErase();mpMergeMatchedKF->SetErase();mnMergeNumCoincidences = 0;mvpMergeMatchedMPs.clear();mvpMergeMPs.clear();mnMergeNumNotFound = 0;mbMergeDetected = false;if(mbLoopDetected){// Reset Loop variablesmpLoopLastCurrentKF->SetErase();mpLoopMatchedKF->SetErase();mnLoopNumCoincidences = 0;mvpLoopMatchedMPs.clear();mvpLoopMPs.clear();mnLoopNumNotFound = 0;mbLoopDetected = false;}}if(mbLoopDetected){bool bGoodLoop = true;vdPR_CurrentTime.push_back(mpCurrentKF->mTimeStamp);vdPR_MatchedTime.push_back(mpLoopMatchedKF->mTimeStamp);vnPR_TypeRecogn.push_back(0);Verbose::PrintMess("*Loop detected", Verbose::VERBOSITY_QUIET);mg2oLoopScw = mg2oLoopSlw; //*mvg2oSim3LoopTcw[nCurrentIndex];if(mpCurrentKF->GetMap()->IsInertial()){Sophus::SE3d Twc = mpCurrentKF->GetPoseInverse().cast<double>();g2o::Sim3 g2oTwc(Twc.unit_quaternion(),Twc.translation(),1.0);g2o::Sim3 g2oSww_new = g2oTwc*mg2oLoopScw;Eigen::Vector3d phi = LogSO3(g2oSww_new.rotation().toRotationMatrix());cout << "phi = " << phi.transpose() << endl; if (fabs(phi(0))<0.008f && fabs(phi(1))<0.008f && fabs(phi(2))<0.349f){if(mpCurrentKF->GetMap()->IsInertial()){// If inertial, force only yawif ((mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) &&mpCurrentKF->GetMap()->GetIniertialBA2()){phi(0)=0;phi(1)=0;g2oSww_new = g2o::Sim3(ExpSO3(phi),g2oSww_new.translation(),1.0);mg2oLoopScw = g2oTwc.inverse()*g2oSww_new;}}}else{cout << "BAD LOOP!!!" << endl;bGoodLoop = false;}}if (bGoodLoop) {mvpLoopMapPoints = mvpLoopMPs;#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_StartLoop = std::chrono::steady_clock::now();nLoop += 1;#endifCorrectLoop();
#ifdef REGISTER_TIMESstd::chrono::steady_clock::time_point time_EndLoop = std::chrono::steady_clock::now();double timeLoopTotal = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndLoop - time_StartLoop).count();vdLoopTotal_ms.push_back(timeLoopTotal);
#endifmnNumCorrection += 1;}// Reset all variablesmpLoopLastCurrentKF->SetErase();mpLoopMatchedKF->SetErase();mnLoopNumCoincidences = 0;mvpLoopMatchedMPs.clear();mvpLoopMPs.clear();mnLoopNumNotFound = 0;mbLoopDetected = false;}}mpLastCurrentKF = mpCurrentKF;}ResetIfRequested();if(CheckFinish()){break;}usleep(5000);}SetFinish();
}