目录
4.2.2.2 DetectLoopCandidates()
1 作用
不断检查新的关键帧是否进入闭环检测队列，并在检测到闭环时执行：
闭环检测（DetectLoop）；
相似变换估计（ComputeSim3）；
闭环修正（CorrectLoop）。
2 参数及含义
3 代码
void LoopClosing::Run() { mbFinished =false; while(1) { // Check if there are keyframes in the queue if(CheckNewKeyFrames()) { // Detect loop candidates and check covisibility consistency if(DetectLoop()) { // Compute similarity transformation [sR|t] // In the stereo/RGBD case s=1 if(ComputeSim3()) { // Perform loop fusion and pose graph optimization CorrectLoop(); } } } ResetIfRequested(); if(CheckFinish()) break; std::this_thread::sleep_for(std::chrono::microseconds(5000)); } SetFinish(); }
4 解析
4.1 初始化线程
//线程未结束 mbFinished =false;
4.2 主循环
while(1) { // Check if there are keyframes in the queue if(CheckNewKeyFrames()) { // Detect loop candidates and check covisibility consistency //闭环检测，详见4.2.2 if(DetectLoop()) { // Compute similarity transformation [sR|t] // In the stereo/RGBD case s=1 if(ComputeSim3()) { // Perform loop fusion and pose graph optimization CorrectLoop(); } } } ResetIfRequested(); if(CheckFinish()) break; std::this_thread::sleep_for(std::chrono::microseconds(5000)); }
4.2.1 CheckNewKeyFrames()
bool LoopClosing::CheckNewKeyFrames() { unique_lock<mutex> lock(mMutexLoopQueue); return(!mlpLoopKeyFrameQueue.empty()); }
4.2.2 DetectLoop()
（1）作用
使用 BoW 词袋模型（
mpKeyFrameDB）在数据库中搜索与当前关键帧相似的旧关键帧；验证这些候选关键帧是否满足共视一致性（covisibility consistency）；
如果检测到潜在闭环，则返回
true。（2）运行逻辑
（3）参数及含义
（4）代码
bool LoopClosing::DetectLoop() { { //队列中取出当前关键帧 unique_lock<mutex> lock(mMutexLoopQueue); mpCurrentKF = mlpLoopKeyFrameQueue.front(); //关键帧队列中取出最前面的关键帧并删除 mlpLoopKeyFrameQueue.pop_front(); // Avoid that a keyframe can be erased while it is being process by this thread mpCurrentKF->SetNotErase(); } //If the map contains less than 10 KF or less than 10 KF have passed from last loop detection //跳过太近的关键帧 if(mpCurrentKF->mnId<mLastLoopKFid+10) { //加入词袋数据库 mpKeyFrameDB->add(mpCurrentKF); //关键帧删除权限开启 mpCurrentKF->SetErase(); return false; } // Compute reference BoW similarity score // This is the lowest score to a connected keyframe in the covisibility graph // We will impose loop candidates to have a higher similarity than this //计算参考BOW相似度阀值 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; //共视关键帧pKF中取出BoW向量表示 const DBoW2::BowVector &BowVec = pKF->mBowVec; //计算当前关键帧与候选关键帧之间的相似度，计算方法如4.2.2.1 float score = mpORBVocabulary->score(CurrentBowVec, BowVec); //找出相似度最小匹配阀值 if(score<minScore) minScore = score; } // Query the database imposing the minimum score //BOW查询候选关键帧，详见4.2.2.2 vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore); // If there are no loop candidates, just add new keyframe and return false //无候选时直接退出 if(vpCandidateKFs.empty()) { mpKeyFrameDB->add(mpCurrentKF); mvConsistentGroups.clear(); mpCurrentKF->SetErase(); return false; } // For each loop candidate check consistency with previous loop candidates // Each candidate expands a covisibility group (keyframes connected to the loop candidate in the covisibility graph) // A group is consistent with a previous group if they share at least a keyframe // We must detect a consistent loop in several consecutive keyframes to accept it //一致性验证：如果连续几帧都检测到相同区域的回环，说明这个回环是真实且稳定的 //通过验证的共视关键帧候选，清除历史数据 mvpEnoughConsistentCandidates.clear(); vector<ConsistentGroup> vCurrentConsistentGroups; //候选关键帧集合，为当前所有旧的候选组创建一个标志数组，初始都设为未使用（false） //ConsistentGroup：由一组关键帧+一致性计数器组成 vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false); for(size_t i=0, iend=vpCandidateKFs.size(); i<iend; i++) { KeyFrame* pCandidateKF = vpCandidateKFs[i]; //候选组=候选关键帧及其共视帧 set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames(); spCandidateGroup.insert(pCandidateKF); bool bEnoughConsistent = false; bool bConsistentForSomeGroup = false; for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++) { set<KeyFrame*> sPreviousGroup = mvConsistentGroups[iG].first; bool bConsistent = false; for(set<KeyFrame*>::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++) { if(sPreviousGroup.count(*sit)) { bConsistent=true; //当前候选组至少与一个历史组一致 bConsistentForSomeGroup=true; break; } } if(bConsistent) { //获取上次一致次数 int nPreviousConsistency = mvConsistentGroups[iG].second; int nCurrentConsistency = nPreviousConsistency + 1; if(!vbConsistentGroup[iG]) { //把当前候选组（spCandidateGroup）加入到新的怀疑组集合 vCurrentConsistentGroups ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency); vCurrentConsistentGroups.push_back(cg); vbConsistentGroup[iG]=true; //this avoid to include the same group more than once } if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent) { mvpEnoughConsistentCandidates.push_back(pCandidateKF); bEnoughConsistent=true; //this avoid to insert the same candidate more than once } } } // If the group is not consistent with any previous group insert with consistency counter set to zero //该候选组是否与之前的怀疑组一致 if(!bConsistentForSomeGroup) { ConsistentGroup cg = make_pair(spCandidateGroup,0); vCurrentConsistentGroups.push_back(cg); } } // Update Covisibility Consistent Groups mvConsistentGroups = vCurrentConsistentGroups; // Add Current Keyframe to database //加入数据库 mpKeyFrameDB->add(mpCurrentKF); //最终结果 if(mvpEnoughConsistentCandidates.empty()) { mpCurrentKF->SetErase(); return false; } else { return true; } mpCurrentKF->SetErase(); return false; }
4.2.2.1 score()
4.2.2.2 DetectLoopCandidates()
（1）作用
在所有历史关键帧中，找出与当前关键帧可能形成闭环的候选关键帧集合。
（2）参数及含义表
（3）代码：KeyFrameDatabase.cc
vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore) { set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames(); list<KeyFrame*> lKFsSharingWords; // Search all keyframes that share a word with current keyframes // Discard keyframes connected to the query keyframe { unique_lock<mutex> lock(mMutex); for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++) { //取出包含当前单词ID的所有关键帧列表 //mvInvertedFile：倒排索引表，vit->first：词袋中单词ID list<KeyFrame*> &lKFs = mvInvertedFile[vit->first]; for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++) { KeyFrame* pKFi=*lit; //检查该候选关键帧是否已在本次循环处理过 if(pKFi->mnLoopQuery!=pKF->mnId) { pKFi->mnLoopWords=0; //如果不是直接关联的关键帧，邻近帧 if(!spConnectedKeyFrames.count(pKFi)) { pKFi->mnLoopQuery=pKF->mnId; lKFsSharingWords.push_back(pKFi); } } pKFi->mnLoopWords++; } } } if(lKFsSharingWords.empty()) return vector<KeyFrame*>(); //存储候选帧的相似度得分(score, KeyFrame*) list<pair<float,KeyFrame*> > lScoreAndMatch; // Only compare against those keyframes that share enough words int maxCommonWords=0; for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) { if((*lit)->mnLoopWords>maxCommonWords) maxCommonWords=(*lit)->mnLoopWords; } int minCommonWords = maxCommonWords*0.8f; int nscores=0; // Compute similarity score. Retain the matches whose score is higher than minScore for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++) { KeyFrame* pKFi = *lit; if(pKFi->mnLoopWords>minCommonWords) { nscores++; float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec); pKFi->mLoopScore = si; if(si>=minScore) lScoreAndMatch.push_back(make_pair(si,pKFi)); } } if(lScoreAndMatch.empty()) return vector<KeyFrame*>(); list<pair<float,KeyFrame*> > lAccScoreAndMatch; float bestAccScore = minScore; // Lets now accumulate score by covisibility for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++) { KeyFrame* pKFi = it->second; vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10); float bestScore = it->first; float accScore = it->first; KeyFrame* pBestKF = pKFi; for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++) { KeyFrame* pKF2 = *vit; if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords) { accScore+=pKF2->mLoopScore; if(pKF2->mLoopScore>bestScore) { pBestKF=pKF2; bestScore = pKF2->mLoopScore; } } } lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF)); if(accScore>bestAccScore) bestAccScore=accScore; } // Return all those keyframes with a score higher than 0.75*bestScore float minScoreToRetain = 0.75f*bestAccScore; set<KeyFrame*> spAlreadyAddedKF; vector<KeyFrame*> vpLoopCandidates; vpLoopCandidates.reserve(lAccScoreAndMatch.size()); for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++) { if(it->first>minScoreToRetain) { KeyFrame* pKFi = it->second; if(!spAlreadyAddedKF.count(pKFi)) { vpLoopCandidates.push_back(pKFi); spAlreadyAddedKF.insert(pKFi); } } } return vpLoopCandidates; }（4）实例
4.2.3 ComputeSim3()
（1）作用
用于在检测到可能的回环候选关键帧后，计算当前关键帧
mpCurrentKF与候选关键帧pKF之间的Sim3相似变换（旋转+平移+尺度）。（2）运行流程
（3）参数及含义
（4）代码
bool LoopClosing::ComputeSim3() { // For each consistent loop candidate we try to compute a Sim3 //一致性通过的回环候选关键帧 const int nInitialCandidates = mvpEnoughConsistentCandidates.size(); // We compute first ORB matches for each candidate // If enough matches are found, we setup a Sim3Solver ORBmatcher matcher(0.75,true); vector<Sim3Solver*> vpSim3Solvers; vpSim3Solvers.resize(nInitialCandidates); vector<vector<MapPoint*> > vvpMapPointMatches; vvpMapPointMatches.resize(nInitialCandidates); vector<bool> vbDiscarded; //标记该候选是否被舍弃 vbDiscarded.resize(nInitialCandidates); int nCandidates=0; //candidates with enough matches for(int i=0; i<nInitialCandidates; i++) { KeyFrame* pKF = mvpEnoughConsistentCandidates[i]; // avoid that local mapping erase it while it is being processed in this thread pKF->SetNotErase(); if(pKF->isBad()) { vbDiscarded[i] = true; continue; } //在当前关键帧mpCurrentKF与候选关键帧pKF之间进行快速ORB特征匹配， //匹配结果存入 vvpMapPointMatches[i] int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]); if(nmatches<20) { vbDiscarded[i] = true; continue; } else { //创建Sim3求解器，7自由度（3旋转，3平移，1尺度） Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale); //设置RANSAC参数，（置信概率，最小内点数，最大迭代次数） pSolver->SetRansacParameters(0.99,20,300); vpSim3Solvers[i] = pSolver; } nCandidates++; } //是否找到回环匹配 bool bMatch = false; // Perform alternatively RANSAC iterations for each candidate // until one is succesful or all fail while(nCandidates>0 && !bMatch) { for(int i=0; i<nInitialCandidates; i++) { if(vbDiscarded[i]) continue; KeyFrame* pKF = mvpEnoughConsistentCandidates[i]; // Perform 5 Ransac Iterations vector<bool> vbInliers; int nInliers; //是否达到最大迭代次数 bool bNoMore; Sim3Solver* pSolver = vpSim3Solvers[i]; //vbInliers：哪些匹配点被判定为内点，nInliers：内点数量 cv::Mat Scm = pSolver->iterate(5,bNoMore,vbInliers,nInliers); // If Ransac reachs max. iterations discard keyframe if(bNoMore) { vbDiscarded[i]=true; nCandidates--; } // If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences if(!Scm.empty()) { vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL)); for(size_t j=0, jend=vbInliers.size(); j<jend; j++) { if(vbInliers[j]) vpMapPointMatches[j]=vvpMapPointMatches[i][j]; } cv::Mat R = pSolver->GetEstimatedRotation(); cv::Mat t = pSolver->GetEstimatedTranslation(); const float s = pSolver->GetEstimatedScale(); matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5); //优化Sim3 g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s); const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale); // If optimization is succesful stop ransacs and continue if(nInliers>=20) { bMatch = true; mpMatchedKF = pKF; g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0); mg2oScw = gScm*gSmw; mScw = Converter::toCvMat(mg2oScw); mvpCurrentMatchedPoints = vpMapPointMatches; break; } } } } if(!bMatch) { for(int i=0; i<nInitialCandidates; i++) mvpEnoughConsistentCandidates[i]->SetErase(); mpCurrentKF->SetErase(); return false; } // Retrieve MapPoints seen in Loop Keyframe and neighbors vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames(); vpLoopConnectedKFs.push_back(mpMatchedKF); mvpLoopMapPoints.clear(); for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++) { KeyFrame* pKF = *vit; vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches(); for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++) { MapPoint* pMP = vpMapPoints[i]; if(pMP) { if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId) { mvpLoopMapPoints.push_back(pMP); pMP->mnLoopPointForKF=mpCurrentKF->mnId; } } } } // Find more matches projecting with the computed Sim3 matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10); // If enough matches accept Loop int nTotalMatches = 0; for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++) { if(mvpCurrentMatchedPoints[i]) nTotalMatches++; } if(nTotalMatches>=40) { for(int i=0; i<nInitialCandidates; i++) if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF) mvpEnoughConsistentCandidates[i]->SetErase(); return true; } else { for(int i=0; i<nInitialCandidates; i++) mvpEnoughConsistentCandidates[i]->SetErase(); mpCurrentKF->SetErase(); return false; } }
4.2.4 CorrectLoop()
（1）作用运行流程
在检测到回环后，对整个地图进行全局一致性校正。分为：
暂停局部建图线程（LocalMapping）；
传播并修正当前帧及邻接关键帧的位姿（通过 Sim3）；
调整这些关键帧下的地图点（MapPoint）；
融合重复点；
更新共视图（Covisibility Graph）；
执行图优化（Essential Graph Optimization）；
最后启动全局 BA（Bundle Adjustment）线程 来进一步优化全图
（2）参数及含义
（3）代码解析
void LoopClosing::CorrectLoop() { cout << "Loop detected!" << endl; // Send a stop signal to Local Mapping // Avoid new keyframes are inserted while correcting the loop //告诉局部建图，忙完手上的事先停下来 mpLocalMapper->RequestStop(); // If a Global Bundle Adjustment is running, abort it if(isRunningGBA()) { unique_lock<mutex> lock(mMutexGBA); mbStopGBA = true; //全局BA版本号 mnFullBAIdx++; //如果全局BA线程在运行，分离该线程，运行结束后删除，该线程是旧的。 if(mpThreadGBA) { mpThreadGBA->detach(); delete mpThreadGBA; } } // Wait until Local Mapping has effectively stopped //等待局部建图停止 while(!mpLocalMapper->isStopped()) { std::this_thread::sleep_for(std::chrono::microseconds(1000)); } // Ensure current keyframe is updated //更新与当前关键帧存在共视关系的边，UpdateConnections()详见4.2.4.1 mpCurrentKF->UpdateConnections(); // Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames(); mvpCurrentConnectedKFs.push_back(mpCurrentKF); //NonCorrectedSim3：未修正的原始Sim3 //CorrectedSim3：回环校正后的Sim3 KeyFrameAndPose CorrectedSim3, NonCorrectedSim3; CorrectedSim3[mpCurrentKF]=mg2oScw; cv::Mat Twc = mpCurrentKF->GetPoseInverse(); //将回环矫正后的Sim3位姿传播到相邻关键帧和地图点，并在地图层面实现两端的对齐与融合。 { // Get Map Mutex unique_lock<mutex> lock(mpMap->mMutexMapUpdate); for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++) { KeyFrame* pKFi = *vit; cv::Mat Tiw = pKFi->GetPose(); if(pKFi!=mpCurrentKF) { cv::Mat Tic = Tiw*Twc; cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3); cv::Mat tic = Tic.rowRange(0,3).col(3); g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0); g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw; //Pose corrected with the Sim3 of the loop closure CorrectedSim3[pKFi]=g2oCorrectedSiw; } cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3); cv::Mat tiw = Tiw.rowRange(0,3).col(3); g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0); //Pose without correction NonCorrectedSim3[pKFi]=g2oSiw; } // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++) { KeyFrame* pKFi = mit->first; g2o::Sim3 g2oCorrectedSiw = mit->second; g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse(); g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi]; vector<MapPoint*> vpMPsi = pKFi->GetMapPointMatches(); for(size_t iMP=0, endMPi = vpMPsi.size(); iMP<endMPi; iMP++) { MapPoint* pMPi = vpMPsi[iMP]; if(!pMPi) continue; if(pMPi->isBad()) continue; if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId) continue; // Project with non-corrected pose and project back with corrected pose cv::Mat P3Dw = pMPi->GetWorldPos(); Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw); Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw)); cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw); pMPi->SetWorldPos(cvCorrectedP3Dw); pMPi->mnCorrectedByKF = mpCurrentKF->mnId; pMPi->mnCorrectedReference = pKFi->mnId; pMPi->UpdateNormalAndDepth(); } // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation) Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix(); Eigen::Vector3d eigt = g2oCorrectedSiw.translation(); double s = g2oCorrectedSiw.scale(); eigt *=(1./s); //[R t/s;0 1] cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt); pKFi->SetPose(correctedTiw); // Make sure connections are updated pKFi->UpdateConnections(); } // Start Loop Fusion // Update matched map points and replace if duplicated for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++) { if(mvpCurrentMatchedPoints[i]) { MapPoint* pLoopMP = mvpCurrentMatchedPoints[i]; MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i); if(pCurMP) pCurMP->Replace(pLoopMP); else { mpCurrentKF->AddMapPoint(pLoopMP,i); pLoopMP->AddObservation(mpCurrentKF,i); pLoopMP->ComputeDistinctiveDescriptors(); } } } } // Project MapPoints observed in the neighborhood of the loop keyframe // into the current keyframe and neighbors using corrected poses. // Fuse duplications. SearchAndFuse(CorrectedSim3); // After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop map<KeyFrame*, set<KeyFrame*> > LoopConnections; for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++) { KeyFrame* pKFi = *vit; vector<KeyFrame*> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames(); // Update connections. Detect new links. pKFi->UpdateConnections(); LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames(); for(vector<KeyFrame*>::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++) { LoopConnections[pKFi].erase(*vit_prev); } for(vector<KeyFrame*>::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++) { LoopConnections[pKFi].erase(*vit2); } } // Optimize graph Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale); mpMap->InformNewBigChange(); // Add loop edge mpMatchedKF->AddLoopEdge(mpCurrentKF); mpCurrentKF->AddLoopEdge(mpMatchedKF); // Launch a new thread to perform Global Bundle Adjustment mbRunningGBA = true; mbFinishedGBA = false; mbStopGBA = false; mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId); // Loop closed. Release Local Mapping. mpLocalMapper->Release(); mLastLoopKFid = mpCurrentKF->mnId; }
4.2.4.1 UpdateConnections()
详见：
4.2.5 ResetIfRequested()
（1）作用
检查是否有重置（Reset）请求，如果有，则清空回环检测队列并重置内部状态。
（2）代码
void LoopClosing::ResetIfRequested() { unique_lock<mutex> lock(mMutexReset); if(mbResetRequested) { mlpLoopKeyFrameQueue.clear(); mLastLoopKFid=0; mbResetRequested=false; } }
4.2.6 CheckFinish()
（1）作用
检查当前回环检测线程是否请求结束
bool LoopClosing::CheckFinish() { unique_lock<mutex> lock(mMutexFinish); return mbFinishRequested; }
4.3 结束标志
SetFinish();
4.3.1 SetFinish()
void LoopClosing::SetFinish() { unique_lock<mutex> lock(mMutexFinish); mbFinished = true; }
扫一扫
435
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
