ORB-SLAM3的Local&MapMerging线程详解
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_TIMES
std::chrono::steady_clock::time_point time_StartPR = std::chrono::steady_clock::now();
#endif
// Step 2检测有没有共视的区域
bool bFindedRegion = NewDetectCommonRegions();
#ifdef REGISTER_TIMES
std::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);
#endif
if(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 yaw
if ((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_TIMES
std::chrono::steady_clock::time_point time_StartMerge = std::chrono::steady_clock::now();
nMerges += 1;
#endif
// TODO UNCOMMENT
if (mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD)
MergeLocal2();
else
MergeLocal();
#ifdef REGISTER_TIMES
std::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);
#endif
Verbose::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 variables
mpMergeLastCurrentKF->SetErase();
mpMergeMatchedKF->SetErase();
mnMergeNumCoincidences = 0;
mvpMergeMatchedMPs.clear();
mvpMergeMPs.clear();
mnMergeNumNotFound = 0;
mbMergeDetected = false;
if(mbLoopDetected)
{
// Reset Loop variables
mpLoopLastCurrentKF->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 yaw
if ((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_TIMES
std::chrono::steady_clock::time_point time_StartLoop = std::chrono::steady_clock::now();
nLoop += 1;
#endif
CorrectLoop();
#ifdef REGISTER_TIMES
std::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);
#endif
mnNumCorrection += 1;
}
// Reset all variables
mpLoopLastCurrentKF->SetErase();
mpLoopMatchedKF->SetErase();
mnLoopNumCoincidences = 0;
mvpLoopMatchedMPs.clear();
mvpLoopMPs.clear();
mnLoopNumNotFound = 0;
mbLoopDetected = false;
}
}
mpLastCurrentKF = mpCurrentKF;
}
ResetIfRequested();
if(CheckFinish()){
break;
}
usleep(5000);
}
SetFinish();
}
