詞典是特征點的描述子的集合，屬于同一類特征的特征點的描述子組成單詞。
在局部建圖線程中，處理完一個關鍵幀后，會將其放入回環檢測線程
?
??在使用關鍵幀數據庫搜索候選關鍵幀組（DetectLoopCandidates）的時候，沒有考慮關鍵幀的連續性。因此在DetectLoop()函數中檢測三個連續的閉環候選關鍵幀，均與當前幀有較高的相似度。
??考慮到單目相機的尺度漂移，計算當前幀與候選閉環幀的Sim3變換，而不是T。（1）使用詞袋加速算法，找到與當前幀具有足夠多的相同單詞的候選關鍵幀集，并初步計算Sim3變換。（2）將候選閉環幀中的路標點投影到當前幀中，通過優化的方法進一步計算Sim3。（3）將閉環幀及其共視幀的路標點投影至當前幀中進行匹配。（4）判斷所選閉環幀是否可靠（（3）步中獲得的匹配數目大于40）。
??閉環校正。計算了當前幀和閉環幀的Sim3變換后，去更新當前幀及其共視幀的位姿，以及路標點的坐標。其中涉及了地圖點的融合，共視圖的更新，本質圖的優化。隨后建立了一個全局BA優化線程，去優化所有的關鍵幀位姿及路標點坐標。
?
??回環檢測是指：找到與當前幀4相似的關鍵幀1，這樣的話就可以根據1直接估計4的位姿，而不是1–2--3–4。減少了誤差傳遞。對于當前幀4的共視幀6，可以1—4---6來獲得，而不是1–2--3–4--6（也就是回環校正，不過在回環校正的時候，還會去校正路標點坐標）。

void LocalMapping::Run()
............................................
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);

?
?

檢測回環

??獲得滿足連續條件的閉環關鍵幀，插入到容器mvpEnoughConsistentCandidates中。
??雖然使用關鍵幀數據庫，可以獲得當前幀的候選閉環關鍵幀：

vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)

但這里檢測出的關鍵幀不滿足連續性，這里篩選出那些具有連續性的關鍵幀。產生閉環的關鍵幀應具有特點：連續的三個關鍵幀，且三個關鍵幀間的相似度評分很高。

bool LoopClosing::DetectLoop()

遍歷當前幀的共視關鍵幀

遍歷當前幀的共視關鍵幀（>15個路標點），使用DBow計算兩幀間的詞袋相似度。

    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;}

尋找候選的閉環關鍵幀

vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);

篩選獲得閉環幀

（1）組(group): 對于某個關鍵幀, 其和其具有共視關系的關鍵幀組成了一個"組";
（2）子候選組(CandidateGroup): 對于某個候選的回環關鍵幀, 其和其具有共視關系的關鍵幀組成的一個"組";
（3）連續(Consistent): 不同的組之間如果共同擁有一個及以上的關鍵幀,那么稱這兩個組之間具有連續關系；
（4）連續性(Consistency):稱之為連續長度可能更合適,表示累計的連續的鏈的長度:A–B 為1, A–B--C–D 為3等;具體反映在數據類型 ConsistentGroup.second上；
（5）連續組(Consistent group): mvConsistentGroups存儲了上次執行回環檢測時, 新的被檢測出來的具有連續性的多個組的集合.由于組之間的連續關系是個網狀結構,因此可能存在 一個組因為和不同的連續組鏈都具有連續關系,而被添加兩次的情況(當然連續性度量是不相同的)；
（6）連續組鏈:自造的稱呼,類似于菊花鏈A–B--C–D這樣形成了一條連續組鏈.對于這個例子中,由于可能E,F都和D有連續關系,因此連續組鏈會產生分叉;為了簡化計算,連續組中將只會保存最后形成連續關系的連續組們。(見下面的連續組的更新)
（7）子連續組: 上面的連續組中的一個組；
（8）連續組的初始值: 在遍歷某個候選幀的過程中,如果該子候選組沒有能夠和任何一個上次的子連續組產生連續關系,那么就將添加自己組為連續組,并且連續性為0；
（9）連續組的更新: 當前次回環檢測過程中,所有被檢測到和之前的連續組鏈有連續的關系的組,都將在對應的連續組鏈后面+1,這些子候選組(可能有重復,見上)都將會成為新的連續組; 換而言之連續組mvConsistentGroups中只保存連續組鏈中末尾的組
?
（1）ConsistentGroup：連續組遍歷，類型為pair<set<KeyFrame*>,int>，其中set<KeyFrame*>為連續組中的關鍵幀，int為連續組的長度。
（2）spCandidateGroup：每個閉環關鍵幀的共視關鍵幀集。

遍歷每一個候選閉環關鍵幀
（1）找到當前候選閉環關鍵幀的共視關鍵幀（構成一個子候選組）

set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();

（2）遍歷上一次閉環檢測到的子連續組

for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++)

（3） 遍歷子候選組，判斷子候選組中的關鍵幀是否存在于子連續組中，如果存在，結束循環

            for(set<KeyFrame*>::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++){if(sPreviousGroup.count(*sit)){// 如果存在，該“子候選組”與該“子連續組”相連bConsistent=true;// 該“子候選組”至少與一個”子連續組“相連，跳出循環bConsistentForSomeGroup=true;break;}}

（4）將該子候選組插入到子連續組中，如果子連續組的長度滿足要求，則將子候選關鍵幀插入候選閉環容器中

（5）如果子候選組中的關鍵幀在所有的子連續組中都找不到，則創建一個新的子連續組，插入到連續組中

        if(!bConsistentForSomeGroup){ConsistentGroup cg = make_pair(spCandidateGroup,0);vCurrentConsistentGroups.push_back(cg);}

?
?

計算Sim3變換，獲得閉環幀

（1）通過Bow加速描述子的匹配，篩選出與當前幀的匹配特征點數大于20的候選幀集合，利用RANSAC粗略地計算出當前幀與閉環幀的Sim3（當前幀—閉環幀）
（2）根據估計的Sim3，將每個候選幀中的路標點投影到當前幀中找到更多匹配，通過優化的方法計算更精確的Sim3（當前幀—閉環幀）。有一個幀成功了，就結束此次的循環。
（3）找到候選幀的共視關鍵幀，找到所有的路標點，投影到當前幀中進行匹配（當前幀–閉環幀+共視幀）（不進行優化）。
（4）判斷候選幀是否可靠（如果（3）步匹配上的路標點的數量大于40，則閉環幀可靠）。

計算當前關鍵幀和上一步中的閉環候選關鍵幀的Sim3變換。Sim3變換多了一個尺度變換。
計算SIm3，而不是T的原因就是存在尺度漂移。

/*** @brief 計算當前關鍵幀和上一步閉環候選幀的Sim3變換* 1. 遍歷閉環候選幀集，篩選出與當前幀的匹配特征點數大于20的候選幀集合，并為每一個候選幀構造一個Sim3Solver* 2. 對每一個候選幀進行 Sim3Solver 迭代匹配，直到有一個候選幀匹配成功，或者全部失敗* 3. 取出閉環匹配上關鍵幀的相連關鍵幀，得到它們的MapPoints放入 mvpLoopMapPoints* 4. 將閉環匹配上關鍵幀以及相連關鍵幀的MapPoints投影到當前關鍵幀進行投影匹配* 5. 判斷當前幀與檢測出的所有閉環關鍵幀是否有足夠多的MapPoints匹配* 6. 清空mvpEnoughConsistentCandidates* @return true         只要有一個候選關鍵幀通過Sim3的求解與優化，就返回true* @return false        所有候選關鍵幀與當前關鍵幀都沒有有效Sim3變換*/
bool LoopClosing::ComputeSim3()

注意以上匹配的結果均都存在成員變量mvpCurrentMatchedPoints中，實際的更新步驟見CorrectLoop()步驟3
對于雙目或者是RGBD輸入的情況,計算得到的尺度=1

遍歷上一步中的候選關鍵幀

如果候選關鍵幀在局部建圖線程中被刪除的話，就直接跳過。

    // Step 1. 遍歷閉環候選幀集，初步篩選出與當前關鍵幀的匹配特征點數大于20的候選幀集合，并為每一個候選幀構造一個Sim3Solverfor(int i=0; i<nInitialCandidates; i++){// Step 1.1 從篩選的閉環候選幀中取出一幀有效關鍵幀pKFKeyFrame* pKF = mvpEnoughConsistentCandidates[i];// 避免在LocalMapping中KeyFrameCulling函數將此關鍵幀作為冗余幀剔除pKF->SetNotErase();// 如果候選幀質量不高，直接PASS// 在局部建圖線程中，如果一個關鍵幀被刪除，則isBad為trueif(pKF->isBad()){vbDiscarded[i] = true;continue;}

通過詞袋算法獲得當前幀與候選幀間匹配上的特征點數量

        // Step 1.2 將當前幀 mpCurrentKF 與閉環候選關鍵幀pKF匹配// 通過bow加速得到 mpCurrentKF 與 pKF 之間的匹配特征點// vvpMapPointMatches 是匹配特征點對應的地圖點,本質上來自于候選閉環幀int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);// 粗篩：匹配的特征點數太少，該候選幀剔除if(nmatches<20){vbDiscarded[i] = true;continue;}else{// Step 1.3 為保留的候選幀構造Sim3求解器// 如果 mbFixScale（是否固定尺度） 為 true，則是6 自由度優化（雙目 RGBD）// 如果是false，則是7 自由度優化（單目）Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);// Sim3Solver Ransac 過程置信度0.99，至少20個inliers 最多300次迭代pSolver->SetRansacParameters(0.99,20,300);vpSim3Solvers[i] = pSolver;}// 保留的候選幀數量nCandidates++;

對每個候選關鍵幀求解Sim3變換，成功的關鍵幀進行SearchBySim3

（1）遍歷每個候選幀

for(int i=0; i<nInitialCandidates; i++)

（2）迭代計算每個候選幀的Sim3變換，如果達到迭代次數上限后，還沒有合格的結果，就刪除當前候選幀

            KeyFrame* pKF = mvpEnoughConsistentCandidates[i];// 內點（Inliers）標志// 即標記經過RANSAC sim3 求解后,vvpMapPointMatches中的哪些作為內點vector<bool> vbInliers; // 內點（Inliers）數量int nInliers;// 是否到達了最優解bool bNoMore;// Step 2.1 取出從 Step 1.3 中為當前候選幀構建的 Sim3Solver 并開始迭代Sim3Solver* pSolver = vpSim3Solvers[i];// 最多迭代5次，返回的Scm是候選幀pKF到當前幀mpCurrentKF的Sim3變換（T12）cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);// If Ransac reachs max. iterations discard keyframe// 總迭代次數達到最大限制還沒有求出合格的Sim3變換，該候選幀剔除if(bNoMore){vbDiscarded[i]=true;nCandidates--;}

（3）如果計算出了Sim3變換，就將當前幀mpCurrentKF和候選關鍵幀pKF中的路標點相互投影匹配，以匹配上更多的路標點。隨后根據匹配關系進行優化。只要有一個優化成功了，就直接結果while循環

            if(!Scm.empty()){// 取出經過Sim3Solver 后匹配點中的內點集合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];}// Step 2.2 通過上面求取的Sim3變換引導關鍵幀匹配，彌補Step 1中的漏匹配// 候選幀pKF到當前幀mpCurrentKF的R（R12），t（t12），變換尺度s（s12）cv::Mat R = pSolver->GetEstimatedRotation();cv::Mat t = pSolver->GetEstimatedTranslation();const float s = pSolver->GetEstimatedScale();// 查找更多的匹配（成功的閉環匹配需要滿足足夠多的匹配特征點數，之前使用SearchByBoW進行特征點匹配時會有漏匹配）// 通過Sim3變換，投影搜索pKF1的特征點在pKF2中的匹配，同理，投影搜索pKF2的特征點在pKF1中的匹配// 只有互相都成功匹配的才認為是可靠的匹配matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);// Step 2.3 用新的匹配來優化 Sim3，只要有一個候選幀通過Sim3的求解與優化，就跳出停止對其它候選幀的判斷// OpenCV的Mat矩陣轉成Eigen的Matrix類型// gScm：候選關鍵幀到當前幀的Sim3變換g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);// 如果mbFixScale為true，則是6 自由度優化（雙目 RGBD），如果是false，則是7 自由度優化（單目）// 優化mpCurrentKF與pKF對應的MapPoints間的Sim3，得到優化后的量gScmconst int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);// 如果優化成功，則停止while循環遍歷閉環候選if(nInliers>=20){// 為True時將不再進入 while循環bMatch = true;// mpMatchedKF就是最終閉環檢測出來與當前幀形成閉環的關鍵幀mpMatchedKF = pKF;// gSmw：從世界坐標系 w 到該候選幀 m 的Sim3變換，都在一個坐標系下，所以尺度 Scale=1g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);// 得到g2o優化后從世界坐標系到當前幀的Sim3變換mg2oScw = gScm*gSmw;mScw = Converter::toCvMat(mg2oScw);mvpCurrentMatchedPoints = vpMapPointMatches;// 只要有一個候選幀通過Sim3的求解與優化，就跳出停止對其它候選幀的判斷break;}}

如果所有候選關鍵幀經過SIM3變換均失敗，則返回false

    // 退出上面while循環的原因有兩種,一種是求解到了bMatch置位后出的,另外一種是nCandidates耗盡為0if(!bMatch){// 如果沒有一個閉環匹配候選幀通過Sim3的求解與優化// 清空mvpEnoughConsistentCandidates，這些候選關鍵幀以后都不會在再參加回環檢測過程了for(int i=0; i<nInitialCandidates; i++)mvpEnoughConsistentCandidates[i]->SetErase();// 當前關鍵幀也將不會再參加回環檢測了mpCurrentKF->SetErase();// Sim3 計算失敗，退出了return false;}

取出與當前幀閉環匹配上的關鍵幀及其共視關鍵幀，以及這些共視關鍵幀的地圖點

    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){// mnLoopPointForKF 用于標記，避免重復添加if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId){mvpLoopMapPoints.push_back(pMP);// 標記一下pMP->mnLoopPointForKF=mpCurrentKF->mnId;}}}}

將閉環關鍵幀及其共視關鍵幀的所有地圖點投影到當前關鍵幀進行投影匹配

matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);

判斷閉環是否可靠

統計當前幀與候選閉環及其共視幀中匹配上的路標點的數量，小于40的話，認為不可靠。

    // Step 4：將閉環關鍵幀及其共視關鍵幀的所有地圖點投影到當前關鍵幀進行投影匹配// 根據投影查找更多的匹配（成功的閉環匹配需要滿足足夠多的匹配特征點數）// 根據Sim3變換，將每個mvpLoopMapPoints投影到mpCurrentKF上，搜索新的匹配對// mvpCurrentMatchedPoints是前面經過SearchBySim3得到的已經匹配的點對，這里就忽略不再匹配了// 搜索范圍系數為10matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);// If enough matches accept Loop// Step 5: 統計當前幀與檢測出的所有閉環關鍵幀的匹配地圖點數目，超過40個說明成功閉環，否則失敗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;}

?
?

閉環校正

閉環關鍵幀的位姿會根據Sim3進行校正，同時與其相連的關鍵幀的位姿即路標點的位置也會被校正。
Sim3校正對旋轉矩陣沒有影響，只會影響平移矩陣。

---

（1）在上一幀計算當前幀和閉環幀的Sim3位姿變換時，建立了閉環幀及其共視幀的路標點與當前幀的聯系，因此先更新共視圖。
（2）根據計算的當前幀和閉環幀的Sim3變換，去更新當前幀及其共視幀的位姿，以及路標點的坐標。
（3）因為閉環幀已經經過了多次優化，認為是精確的，因此建立閉環幀及其共視幀的路標點與當前幀及其共視幀的聯系，進行路標點的匹配、融合。
（4）優化本質圖（只優化位姿）
（5）建立一個全局BA優化線程

結束局部地圖線程、全局BA，為閉環校正做準備

    mpLocalMapper->RequestStop();if(isRunningGBA()){// 如果有全局BA在運行，終止掉，迎接新的全局BAunique_lock<mutex> lock(mMutexGBA);mbStopGBA = true;// 記錄全局BA次數mnFullBAIdx++;if(mpThreadGBA){// 停止全局BA線程mpThreadGBA->detach();delete mpThreadGBA;}}

更新共視圖

在閉環檢測、計算Sim3的過程中，建立了當前幀的特征點和其閉環幀、閉環幀的共視幀的路標點間的聯系，因此這里需要更新一個共視圖。

    // Step 1：根據共視關系更新當前關鍵幀與其它關鍵幀之間的連接關系// 因為之前閉環檢測、計算Sim3中改變了該關鍵幀的地圖點，所以需要更新mpCurrentKF->UpdateConnections();

更新當前幀、與其相連的關鍵幀的位姿。

當前幀與世界坐標系之間的Sim變換在ComputeSim3函數中已經確定并優化，通過相對位姿關系，可以確定與當前幀相連的關鍵幀與世界坐標系之間的Sim3變換

（1）通過mg2oScw（認為是準的）來進行位姿傳播，得到當前關鍵幀的共視關鍵幀的世界坐標系下Sim3 位姿.遍歷"當前關鍵幀組"（當前幀+共視幀）

        // Step 2.1：通過mg2oScw（認為是準的）來進行位姿傳播，得到當前關鍵幀的共視關鍵幀的世界坐標系下Sim3 位姿（還沒有修正）// 遍歷"當前關鍵幀組""for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++){KeyFrame* pKFi = *vit;cv::Mat Tiw = pKFi->GetPose();if(pKFi!=mpCurrentKF)      //跳過當前關鍵幀，因為當前關鍵幀的位姿已經在前面優化過了，在這里是參考基準{// 得到當前關鍵幀 mpCurrentKF 到其共視關鍵幀 pKFi 的相對變換cv::Mat Tic = Tiw*Twc;cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);cv::Mat tic = Tic.rowRange(0,3).col(3);// g2oSic：當前關鍵幀 mpCurrentKF 到其共視關鍵幀 pKFi 的Sim3 相對變換// 這里是non-correct, 所以scale=1.0g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);// 當前幀的位姿固定不動，其它的關鍵幀根據相對關系得到Sim3調整的位姿g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;// Pose corrected with the Sim3 of the loop closure// 存放閉環g2o優化后當前關鍵幀的共視關鍵幀的Sim3 位姿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// 存放沒有矯正的當前關鍵幀的共視關鍵幀的Sim3變換NonCorrectedSim3[pKFi]=g2oSiw;}

（2）校正當前幀的共視關鍵幀的路標點坐標
路標點世界坐標------（未校正的T）--------路標點相機坐標-------（校正的Sim3）--------路標點世界坐標
保持路標點和幀間的相對位置不變。
要記得更新地圖點的平均觀測方向和觀測范圍

        // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop// Step 2.2：得到矯正的當前關鍵幀的共視關鍵幀位姿后，修正這些關鍵幀的地圖點// 遍歷待矯正的共視關鍵幀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// 將該未校正的eigP3Dw先從世界坐標系映射到未校正的pKFi相機坐標系，然后再反映射到校正后的世界坐標系下cv::Mat P3Dw = pMPi->GetWorldPos();// 地圖點世界坐標系下坐標Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);// map(P) 內部做了變換 R*P +t  // 下面變換是：eigP3Dw： world →g2oSiw→ i →g2oCorrectedSwi→ worldEigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);pMPi->SetWorldPos(cvCorrectedP3Dw);// 記錄矯正該地圖點的關鍵幀id，防止重復pMPi->mnCorrectedByKF = mpCurrentKF->mnId;// 記錄該地圖點所在的關鍵幀idpMPi->mnCorrectedReference = pKFi->mnId;// 因為地圖點更新了，需要更新其平均觀測方向以及觀測距離范圍pMPi->UpdateNormalAndDepth();}

（3）將共視關鍵幀的Sim3轉換為SE3，根據更新的Sim3，更新關鍵幀的位姿

            // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)// Step 2.3：將共視關鍵幀的Sim3轉換為SE3，根據更新的Sim3，更新關鍵幀的位姿// 其實是現在已經有了更新后的關鍵幀組中關鍵幀的位姿,但是在上面的操作時只是暫時存儲到了 KeyFrameAndPose 類型的變量中,還沒有寫回到關鍵幀對象中// 調用toRotationMatrix 可以自動歸一化旋轉矩陣Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix(); Eigen::Vector3d eigt = g2oCorrectedSiw.translation();                  double s = g2oCorrectedSiw.scale();// 平移向量中包含有尺度信息，還需要用尺度歸一化eigt *=(1./s); cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);// 設置矯正后的新的posepKFi->SetPose(correctedTiw);

更新當前幀路標點

更新當前幀中的路標點，應為在ComputeSim3()函數獲取閉環幀的時候，將閉環幀及其共視幀的路標點和當前幀的特征點進行了匹配

        // Step 3：檢查當前幀的地圖點與經過閉環匹配后該幀的地圖點是否存在沖突，對沖突的進行替換或填補// mvpCurrentMatchedPoints 是當前關鍵幀和閉環關鍵幀組的所有地圖點進行投影得到的匹配點for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++){if(mvpCurrentMatchedPoints[i]){//取出同一個索引對應的兩種地圖點，決定是否要替換// 匹配投影得到的地圖點MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];// 原來的地圖點MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i); if(pCurMP)// 如果有重復的MapPoint，則用匹配的地圖點代替現有的// 因為匹配的地圖點是經過一系列操作后比較精確的，現有的地圖點很可能有累計誤差pCurMP->Replace(pLoopMP);else{// 如果當前幀沒有該MapPoint，則直接添加mpCurrentKF->AddMapPoint(pLoopMP,i);pLoopMP->AddObservation(mpCurrentKF,i);pLoopMP->ComputeDistinctiveDescriptors();}}} }

路標點融合

在局部建圖的時候，已經獲得了當前幀及其共視幀所聯系的路標點。這里又已知閉環幀及其共視幀的路標點。閉環幀出現在當前幀之前，進行了多次優化，因此其路標點準確。所以將閉環幀及其共視幀的路標點投影到當前幀組中進行匹配，融合。

SearchAndFuse(CorrectedSim3);

更新當前幀的連接關系

前面進行了路標點的融合，這里要更改一下連接關系。
map<KeyFrame*, set<KeyFrame*> > LoopConnections：
KeyFrame：當前幀及其共視關鍵幀中的一幀
set<KeyFrame*>：KeyFrame的共視關鍵幀

優化共視圖

Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);

添加當前幀與閉環匹配幀之間的邊

    mpMatchedKF->AddLoopEdge(mpCurrentKF);mpCurrentKF->AddLoopEdge(mpMatchedKF);

建立一個線程進行全局BA優化

mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);

全局BA優化線程

優化所有的關鍵幀及路標點

void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)

總結

以上是生活随笔為你收集整理的ORB_SLAM2回环检测的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。