2021SC@SDUSC
LoopClosing分析（2）
这一篇博客依旧是LoopClosing的范畴，主要聚焦于分析NewDetectCommonRegions函数。由于代码相对较长，因此本篇博客都直接将分析以注释的形式写在代码中。
- NewDetectCommonRegions函数是用来进行闭环检测： 成功检测到闭环时返回true；反之返回false。
bool LoopClosing::NewDetectCommonRegions()
{
{
// Step 1 从队列中取出一个关键帧,作为当前检测闭环关键帧
unique_lock<mutex> lock(mMutexLoopQueue);
// 从队列头开始取，也就是先取早进来的关键帧
mpCurrentKF = mlpLoopKeyFrameQueue.front();
// 取出关键帧后从队列里弹出该关键帧
mlpLoopKeyFrameQueue.pop_front();
// 设置当前关键帧不要在优化的过程中被删除
mpCurrentKF->SetNotErase();
mpCurrentKF->mbCurrentPlaceRecognition = true;
mpLastMap = mpCurrentKF->GetMap();
}
// Step 2：如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
if(mpLastMap->IsInertial() && !mpLastMap->GetIniertialBA1())
{
mpKeyFrameDB->add(mpCurrentKF);
mpCurrentKF->SetErase();
return false;
}
if(mpTracker->mSensor == System::STEREO && mpLastMap->GetAllKeyFrames().size() < 5) //12
{
mpKeyFrameDB->add(mpCurrentKF);
mpCurrentKF->SetErase();
return false;
}
if(mpLastMap->GetAllKeyFrames().size() < 12)
{
mpKeyFrameDB->add(mpCurrentKF);
mpCurrentKF->SetErase();
return false;
}
//Check the last candidates with geometric validation
// Loop candidates
bool bLoopDetectedInKF = false;
bool bCheckSpatial = false;
if(mnLoopNumCoincidences > 0)
{
bCheckSpatial = true;
// Find from the last KF candidates
cv::Mat mTcl = mpCurrentKF->GetPose() * mpLoopLastCurrentKF->GetPoseInverse();
g2o::Sim3 gScl(Converter::toMatrix3d(mTcl.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(mTcl.rowRange(0, 3).col(3)),1.0);
g2o::Sim3 gScw = gScl * mg2oLoopSlw;
int numProjMatches = 0;
vector<MapPoint*> vpMatchedMPs;
// 当前帧与匹配闭环帧局部map进行匹配，优化，如果最终匹配数量超过一定阈值，认为闭环成立
bool bCommonRegion = DetectAndReffineSim3FromLastKF(mpCurrentKF, mpLoopMatchedKF, gScw, numProjMatches, mvpLoopMPs, vpMatchedMPs);
if(bCommonRegion)
{
bLoopDetectedInKF = true;
mnLoopNumCoincidences++;
mpLoopLastCurrentKF->SetErase();
mpLoopLastCurrentKF = mpCurrentKF;
mg2oLoopSlw = gScw;
mvpLoopMatchedMPs = vpMatchedMPs;
mbLoopDetected = mnLoopNumCoincidences >= 3;
mnLoopNumNotFound = 0;
if(!mbLoopDetected)
{
cout << "PR: Loop detected with Reffine Sim3" << endl;
}
}
else
{
bLoopDetectedInKF = false;
mnLoopNumNotFound++;
if(mnLoopNumNotFound >= 2)
{
mpLoopLastCurrentKF->SetErase();
mpLoopMatchedKF->SetErase();
//mg2oLoopScw;
mnLoopNumCoincidences = 0;
mvpLoopMatchedMPs.clear();
mvpLoopMPs.clear();
mnLoopNumNotFound = 0;
}
}
}
//Merge candidates
bool bMergeDetectedInKF = false;
if(mnMergeNumCoincidences > 0)
{
// Find from the last KF candidates
cv::Mat mTcl = mpCurrentKF->GetPose() * mpMergeLastCurrentKF->GetPoseInverse();
g2o::Sim3 gScl(Converter::toMatrix3d(mTcl.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(mTcl.rowRange(0, 3).col(3)),1.0);
g2o::Sim3 gScw = gScl * mg2oMergeSlw;
int numProjMatches = 0;
vector<MapPoint*> vpMatchedMPs;
// 当前帧与匹配闭环帧局部map进行匹配，优化，如果最终匹配数量超过一定阈值，认为闭环成立
bool bCommonRegion = DetectAndReffineSim3FromLastKF(mpCurrentKF, mpMergeMatchedKF, gScw, numProjMatches, mvpMergeMPs, vpMatchedMPs);
if(bCommonRegion)
{
//cout << "BoW: Merge reffined Sim3 transformation suscelful" << endl;
bMergeDetectedInKF = true;
mnMergeNumCoincidences++;
mpMergeLastCurrentKF->SetErase();
mpMergeLastCurrentKF = mpCurrentKF;
mg2oMergeSlw = gScw;
mvpMergeMatchedMPs = vpMatchedMPs;
mbMergeDetected = mnMergeNumCoincidences >= 3;
}
else
{
//cout << "BoW: Merge reffined Sim3 transformation failed" << endl;
mbMergeDetected = false;
bMergeDetectedInKF = false;
mnMergeNumNotFound++;
if(mnMergeNumNotFound >= 2)
{
/*cout << "+++++++Merge detected failed in KF" << endl;
for(int i=0; i<mvpMergeLastKF.size(); ++i)
{
mvpMergeLastKF[i]->SetErase();
mvpMergeCandidateKF[i]->SetErase();
mvpMergeLastKF[i]->mbCurrentPlaceRecognition = true;
}
mvpMergeCandidateKF.clear();
mvpMergeLastKF.clear();
mvg2oSim3MergeTcw.clear();
mvnMergeNumMatches.clear();
mvvpMergeMapPoints.clear();
mvvpMergeMatchedMapPoints.clear();*/
mpMergeLastCurrentKF->SetErase();
mpMergeMatchedKF->SetErase();
mnMergeNumCoincidences = 0;
mvpMergeMatchedMPs.clear();
mvpMergeMPs.clear();
mnMergeNumNotFound = 0;
}
}
}
if(mbMergeDetected || mbLoopDetected)
{
//f_time_pr << "Geo" << " " << timeGeoKF_ms.count() << endl;
mpKeyFrameDB->add(mpCurrentKF);
return true;
}
//-------------
//TODO: This is only necessary if we use a minimun score for pick the best candidates
const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
/*float minScore = 1;
for(size_t i=0; i<vpConnectedKeyFrames.size(); i++)
{
KeyFrame* pKF = vpConnectedKeyFrames[i];
if(pKF->isBad())
continue;
const DBoW2::BowVector &BowVec = pKF->mBowVec;
float score = mpORBVocabulary->score(CurrentBowVec, BowVec);
if(score<minScore)
minScore = score;
}*/
//-------------
// Extract candidates from the bag of words
vector<KeyFrame*> vpMergeBowCand, vpLoopBowCand;
//cout << "LC: bMergeDetectedInKF: " << bMergeDetectedInKF << " bLoopDetectedInKF: " << bLoopDetectedInKF << endl;
if(!bMergeDetectedInKF || !bLoopDetectedInKF)
{
// Search in BoW
mpKeyFrameDB->DetectNBestCandidates(mpCurrentKF, vpLoopBowCand, vpMergeBowCand,3);
}
// Check the BoW candidates if the geometric candidate list is empty
//Loop candidates
/*#ifdef COMPILEDWITHC11
std::chrono::steady_clock::time_point timeStartGeoBoW = std::chrono::steady_clock::now();
#else
std::chrono::monotonic_clock::time_point timeStartGeoBoW = std::chrono::monotonic_clock::now();
#endif*/
if(!bLoopDetectedInKF && !vpLoopBowCand.empty())
{
mbLoopDetected = DetectCommonRegionsFromBoW(vpLoopBowCand, mpLoopMatchedKF, mpLoopLastCurrentKF, mg2oLoopSlw, mnLoopNumCoincidences, mvpLoopMPs, mvpLoopMatchedMPs);
}
// Merge candidates
//cout << "LC: Find BoW candidates" << endl;
if(!bMergeDetectedInKF && !vpMergeBowCand.empty())
{
mbMergeDetected = DetectCommonRegionsFromBoW(vpMergeBowCand, mpMergeMatchedKF, mpMergeLastCurrentKF, mg2oMergeSlw, mnMergeNumCoincidences, mvpMergeMPs, mvpMergeMatchedMPs);
}
//cout << "LC: add to KFDB" << endl;
mpKeyFrameDB->add(mpCurrentKF);
if(mbMergeDetected || mbLoopDetected)
{
return true;
}
//cout << "LC: erase KF" << endl;
mpCurrentKF->SetErase();
mpCurrentKF->mbCurrentPlaceRecognition = false;
return false;
}
- 对于DetectAndReffineSim3FromLastKF函数，将当前帧与匹配闭环帧局部map进行匹配，优化，如果最终匹配数量超过一定阈值，认为闭环成立。
bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs)
{
set<MapPoint*> spAlreadyMatchedMPs;
// 提取候选匹配帧的共视帧集合，以及二级共视帧集合，作为一个大map，在当前帧中寻找2d匹配点，返回匹配点数量
nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw, spAlreadyMatchedMPs, vpMPs, vpMatchedMPs);
cout << "REFFINE-SIM3: Projection from last KF with " << nNumProjMatches << " matches" << endl;
int nProjMatches = 30;
int nProjOptMatches = 50;
int nProjMatchesRep = 100;
// 如果匹配数量超过阈值
if(nNumProjMatches >= nProjMatches)
{
cv::Mat mScw = Converter::toCvMat(gScw);
cv::Mat mTwm = pMatchedKF->GetPoseInverse();
g2o::Sim3 gSwm(Converter::toMatrix3d(mTwm.rowRange(0, 3).colRange(0, 3)),Converter::toVector3d(mTwm.rowRange(0, 3).col(3)),1.0);
g2o::Sim3 gScm = gScw * gSwm;
Eigen::Matrix<double, 7, 7> mHessian7x7;
bool bFixedScale = mbFixScale; // TODO CHECK; Solo para el monocular inertial
if(mpTracker->mSensor==System::IMU_MONOCULAR && !pCurrentKF->GetMap()->GetIniertialBA2())
bFixedScale=false;
// 已知匹配点与相似变换，优化匹配
int numOptMatches = Optimizer::OptimizeSim3(mpCurrentKF, pMatchedKF, vpMatchedMPs, gScm, 10, bFixedScale, mHessian7x7, true);
cout << "REFFINE-SIM3: Optimize Sim3 from last KF with " << numOptMatches << " inliers" << endl;
// 如果匹配数量超过阈值
if(numOptMatches > nProjOptMatches)
{
g2o::Sim3 gScw_estimation(Converter::toMatrix3d(mScw.rowRange(0, 3).colRange(0, 3)),
Converter::toVector3d(mScw.rowRange(0, 3).col(3)),1.0);
vector<MapPoint*> vpMatchedMP;
vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
// 提取候选匹配帧的共视帧集合，以及二级共视帧集合，作为一个大map，在当前帧中寻找2d匹配点，返回匹配点数量
nNumProjMatches = FindMatchesByProjection(pCurrentKF, pMatchedKF, gScw_estimation, spAlreadyMatchedMPs, vpMPs, vpMatchedMPs);
//cout << "REFFINE-SIM3: Projection with optimize Sim3 from last KF with " << nNumProjMatches << " matches" << endl;
if(nNumProjMatches >= nProjMatchesRep)
{
gScw = gScw_estimation;
return true;
}
}
}
return false;
}
- FindMatchesByProjection函数会提取候选匹配帧的共视帧集合，以及二级共视帧集合，作为一个大map，在当前帧中寻找2d匹配点，返回匹配点数量。
int LoopClosing::FindMatchesByProjection(KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, g2o::Sim3 &g2oScw,
set<MapPoint*> &spMatchedMPinOrigin, vector<MapPoint*> &vpMapPoints,
vector<MapPoint*> &vpMatchedMapPoints)
{
int nNumCovisibles = 5;
// 排序共视关键帧的前N帧，候选闭环帧的局部邻近帧集合
vector<KeyFrame*> vpCovKFm = pMatchedKFw->GetBestCovisibilityKeyFrames(nNumCovisibles);
int nInitialCov = vpCovKFm.size();
vpCovKFm.push_back(pMatchedKFw);
// 候选闭环检测集合
set<KeyFrame*> spCheckKFs(vpCovKFm.begin(), vpCovKFm.end());
// 当前帧的共视关键帧集合
set<KeyFrame*> spCurrentCovisbles = pCurrentKF->GetConnectedKeyFrames();
// 遍历候选闭环帧的共视关键帧集合
for(int i=0; i<nInitialCov; ++i)
{
// 选5帧最近的关键帧
vector<KeyFrame*> vpKFs = vpCovKFm[i]->GetBestCovisibilityKeyFrames(nNumCovisibles);
int nInserted = 0;
int j = 0;
while(j < vpKFs.size() && nInserted < nNumCovisibles)
{
// 如果邻近帧不在检查集合中，同时也不在当前帧共视帧集合中，认为可能是候选闭环，加入检测集合
if(spCheckKFs.find(vpKFs[j]) == spCheckKFs.end() && spCurrentCovisbles.find(vpKFs[j]) == spCurrentCovisbles.end())
{
spCheckKFs.insert(vpKFs[j]);
++nInserted;
}
++j;
}
// 加入邻近帧
vpCovKFm.insert(vpCovKFm.end(), vpKFs.begin(), vpKFs.end());
}
set<MapPoint*> spMapPoints;
vpMapPoints.clear();
vpMatchedMapPoints.clear();
// 局部候选map，提取MP
for(KeyFrame* pKFi : vpCovKFm)
{
for(MapPoint* pMPij : pKFi->GetMapPointMatches())
{
if(!pMPij || pMPij->isBad())
continue;
if(spMapPoints.find(pMPij) == spMapPoints.end())
{
spMapPoints.insert(pMPij);
vpMapPoints.push_back(pMPij);
}
}
}
//cout << "Point cloud: " << vpMapPoints.size() << endl;
cv::Mat mScw = Converter::toCvMat(g2oScw);
ORBmatcher matcher(0.9, true);
vpMatchedMapPoints.resize(pCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
/**
* 3d-2d，闭环关键帧与其共视关键帧的MP，投影到当前关键帧中，计算描述子距离，寻找当前帧匹配特征点
* 1、投影MP到当前关键帧，深度值不得超过估计范围，视差不得超过60°
* 2、通过深度值估计特征点的层级，搜索框提取fast特征点集合
* 3、匹配层级上下只允许浮动一层，描述子距离小于阈值即可
* 4、当前关键帧已有的MP不参与上面的过程
* @param pKF 当前关键帧
* @param Scw 当前关键帧位姿
* @param vpPoints 闭环关键帧与其共视关键帧的地图点
* @param vpMatched 当前关键帧已经匹配的点
* @param th 搜索窗大小
* @param ratioHamming 描述子距离阈值系数
*/
int num_matches = matcher.SearchByProjection(pCurrentKF, mScw, vpMapPoints, vpMatchedMapPoints, 3, 1.5);
return num_matches;
}
由于LoopClosing的代码较多，这几篇博客都会是与其相关的，之后会继续分析。