SLAM——ORB-SLAM3代码分析（八）LoopClosing（1）
发布时间
阅读量：
阅读量
2021SC@SDUSC
LoopClosing分析（1）
在综述文献中我们涉及到了循环检测这一机制用于判别机器人所处的位置是否曾有过 revisit 的经历。为了最大程度地减少误差最优的方式是在系统运行过程中实时识别循环并据此对整体计算结果进行校正优化工作。相比BA方法循环机制相较于BA方法更具强度和精确性因而能够显著提升计算效率和结果可靠性。一旦系统检测到循环出现的情况该机制会将相关信息传递给后续处理模块完成数据整合工作这在整个流程体系中扮演着关键角色。
LoopClosing的主要组成部分包括三个关键环节：
- 初始状态估计
- 循环匹配与状态更新
- 约束传播与优化
检测回环
计算Sim3
回环校正
整体流程大致如下图所示：
接下来我们来看具体代码：
开头包括LoopClosing构造函数以及另外两个用于设置相关线程的函数。其中SetTracker负责指定追踪相关线程的句柄,SetLocalMapper则用于指定局部建图相关的线程句柄。
void LoopClosing::SetTracker(Tracking *pTracker)
{
mpTracker=pTracker;
}
void LoopClosing::SetLocalMapper(LocalMapping *pLocalMapper)
{
mpLocalMapper=pLocalMapper;
}接下来是非常关键的LoopClosing主函数Run( )。考虑到代码长度较长且复杂性较高，在其中进行分析注释会更加合理。
void LoopClosing::Run()
{
mbFinished =false;
while(1)
{
// Check if there are keyframes in the queue
// Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的
// 在LocalMapping中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue
// Step 1 查看闭环检测队列mlpLoopKeyFrameQueue中有没有关键帧进来
if(CheckNewKeyFrames())
{
if(mpLastCurrentKF)
{
mpLastCurrentKF->mvpLoopCandKFs.clear();
mpLastCurrentKF->mvpMergeCandKFs.clear();
}
if(NewDetectCommonRegions())
{
if(mbMergeDetected)
{
if ((mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO) &&
(!mpCurrentKF->GetMap()->isImuInitialized()))
{
cout << "IMU is not initilized, merge is aborted" << endl;
}
else
{
Verbose::PrintMess("*Merged detected", Verbose::VERBOSITY_QUIET);
Verbose::PrintMess("Number of KFs in the current map: " + to_string(mpCurrentKF->GetMap()->KeyFramesInMap()), Verbose::VERBOSITY_DEBUG);
cv::Mat mTmw = mpMergeMatchedKF->GetPose();
g2o::Sim3 gSmw2(Converter::toMatrix3d(mTmw.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(mTmw.rowRange(0, 3).col(3)),1.0);
cv::Mat mTcw = mpCurrentKF->GetPose();
g2o::Sim3 gScw1(Converter::toMatrix3d(mTcw.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(mTcw.rowRange(0, 3).col(3)),1.0);
g2o::Sim3 gSw2c = mg2oMergeSlw.inverse();
g2o::Sim3 gSw1m = mg2oMergeSlw;
mSold_new = (gSw2c * gScw1);
if(mpCurrentKF->GetMap()->IsInertial() && mpMergeMatchedKF->GetMap()->IsInertial())
{
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) &&
mpCurrentKF->GetMap()->GetIniertialBA1()) // TODO, maybe with 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;
// TODO UNCOMMENT
if (mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO)
MergeLocal2();
else
MergeLocal();
}
vdPR_CurrentTime.push_back(mpCurrentKF->mTimeStamp);
vdPR_MatchedTime.push_back(mpMergeMatchedKF->mTimeStamp);
vnPR_TypeRecogn.push_back(1);
// 重新设置循环的变量
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)
{
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())
{
cv::Mat Twc = mpCurrentKF->GetPoseInverse();
g2o::Sim3 g2oTwc(Converter::toMatrix3d(Twc.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(Twc.rowRange(0, 3).col(3)),1.0);
g2o::Sim3 g2oSww_new = g2oTwc*mg2oLoopScw;
Eigen::Vector3d phi = LogSO3(g2oSww_new.rotation().toRotationMatrix());
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) &&
mpCurrentKF->GetMap()->GetIniertialBA2()) // TODO, maybe with GetIniertialBA1
{
phi(0)=0;
phi(1)=0;
g2oSww_new = g2o::Sim3(ExpSO3(phi),g2oSww_new.translation(),1.0);
mg2oLoopScw = g2oTwc.inverse()*g2oSww_new;
}
}
mvpLoopMapPoints = mvpLoopMPs;//*mvvpLoopMapPoints[nCurrentIndex];
CorrectLoop();
}
else
{
cout << "BAD LOOP!!!" << endl;
}
}
else
{
mvpLoopMapPoints = mvpLoopMPs;
CorrectLoop();
}
//重新设置所有的变量
mpLoopLastCurrentKF->SetErase();
mpLoopMatchedKF->SetErase();
mnLoopNumCoincidences = 0;
mvpLoopMatchedMPs.clear();
mvpLoopMPs.clear();
mnLoopNumNotFound = 0;
mbLoopDetected = false;
}
}
mpLastCurrentKF = mpCurrentKF;
}
// 查看是否有外部线程请求复位当前线程
ResetIfRequested();
// 查看外部线程是否有终止当前线程的请求,如果有的话就跳出这个线程的主函数的主循环
if(CheckFinish()){
break;
}
usleep(5000);
}
// 运行到这里说明有外部线程请求终止当前线程,在这个函数中执行终止当前线程的一些操作
SetFinish();
}InsertKeyFrame 负责将特定的关键帧纳入回环检测机制中，在局部建图线程中执行此操作。值得注意的是，在本系统中第0个关键帧不具备参与回环检测的能力，这是因为第0个关键帧被特意设定为地图全局定位的基础框架
void LoopClosing::InsertKeyFrame(KeyFrame *pKF)
{
unique_lock<mutex> lock(mMutexLoopQueue);
if(pKF->mnId!=0)
mlpLoopKeyFrameQueue.push_back(pKF);
}- CheckNewKeyFrames 是用于 检查 列表中 是否 存在 等待 插入 的 关键帧 吗？
bool LoopClosing::CheckNewKeyFrames()
{
unique_lock<mutex> lock(mMutexLoopQueue);
return(!mlpLoopKeyFrameQueue.empty());
}接下来的部分下一篇博客再继续分析。
全部评论 (0)
还没有任何评论哟~