我是靠谱客的博主 机智向日葵，最近开发中收集的这篇文章主要介绍SLAM——ORB-SLAM3代码分析（八）LoopClosing（1）LoopClosing分析（1），觉得挺不错的，现在分享给大家，希望可以做个参考。
概述
2021SC@SDUSC
LoopClosing分析（1）
我们在综述中提到过回环检测会判断机器人是否到达过先前的位置。消除误差最有效的办法是发现闭环，并根据闭环对所有结果进行优化。闭环是一个比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则是查看列表中是否有等待被插入的关键帧，如果存在，返回true。
bool LoopClosing::CheckNewKeyFrames()
{
unique_lock<mutex> lock(mMutexLoopQueue);
return(!mlpLoopKeyFrameQueue.empty());
}
接下来的部分下一篇博客再继续分析。
最后
以上就是机智向日葵为你收集整理的SLAM——ORB-SLAM3代码分析（八）LoopClosing（1）LoopClosing分析（1）的全部内容，希望文章能够帮你解决SLAM——ORB-SLAM3代码分析（八）LoopClosing（1）LoopClosing分析（1）所遇到的程序开发问题。
如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。
本图文内容来源于网友提供，作为学习参考使用，或来自网络收集整理，版权属于原作者所有。
发表评论 取消回复