VO總是會有累計誤差，而LoopClosing通過檢測是否曾經來過此處，進行後端優化，可以將這個累計誤差縮小到一個可接受的範圍內。閉環是一個比BA更加強烈、更加準確的約束，從而使得Slam系統應對大範圍場景時，擁有更高的魯棒性和可用性。

整個LoopClosing模組是線上程中完成，並在建立執行緒時呼叫LoopClosing::Run()函式讓其執行。

mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run,mpLoopCloser)

一、DetectLoop（）

閉環條件檢測

主要流程：

1、檢測當前關鍵幀在 Covisibility 圖中的附近關鍵幀，並會依次計算當前關鍵幀和每一個附近關鍵幀的BoW分值，通過我們所得到分數的最低分，到資料庫中查詢，查找出所有大於該最低分的關鍵幀作為候選幀。

2、在候選幀中檢測具有連續性的候選幀。

0、從佇列中取出一個關鍵幀，原佇列少一個mpCurrentKF

    {
        // 從佇列中取出一個關鍵幀mpCurrentKF，原佇列少一個
        unique_lock<mutex> lock(mMutexLoopQueue);
        mpCurrentKF = mlpLoopKeyFrameQueue.front();
        mlpLoopKeyFrameQueue.pop_front();
        mpCurrentKF->SetNotErase();//避免處理該關鍵幀時被擦除
    }
 

1、如果地圖中的關鍵幀數小於10或者距離上次閉環少於10幀，那麼不進行閉環檢測

    if(mpCurrentKF->mnId<mLastLoopKFid+10)
    {
        mpKeyFrameDB->add(mpCurrentKF);//關鍵幀資料庫里加入當前關鍵幀
        mpCurrentKF->SetErase();
        return false;//輸出錯誤，不進行loop detection
    }

2、遍歷所有共視關鍵幀，計算當前關鍵幀與每個共視關鍵的bow相似度得分，並得到最低得分minScore

    const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();//Convisible圖中與當前幀相連的關鍵幀
    const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;//當前幀的bow向量 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;//相連關鍵幀的bow向量 pKF->mBowVec

        float score = mpORBVocabulary->score(CurrentBowVec, BowVec);//核心函式：計算當前幀bow向量與相連關鍵幀bow向量的相似度score

        if(score<minScore)
            minScore = score;
    }
 

3、在所有關鍵幀中找出閉環備選幀vpCandidateKFs（大於minscore）

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

    重點函式DetectLoopCandidates（）

4、對候選關鍵幀集進行連續性檢測（並且如果一旦有一個閉環候選關鍵幀被檢測到3次，系統就認為檢測到閉環）

    （1）每個候選幀將與自己相連的關鍵幀構成一個“子候選組spCandidateGroup”，vpCandidateKFs-->spCandidateGroup

        KeyFrame* pCandidateKF = vpCandidateKFs[i];//候選關鍵幀

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

   （2）檢測“子候選組”中每一個關鍵幀是否存在於“連續組”，如果存在nCurrentConsistency++，則將該“子候選組”放入“當前連續組vCurrentConsistentGroups”

        // 遍歷之前的“子連續組”
        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])// 這裡作者原本意思是不是應該是vbConsistentGroup[i]而不是vbConsistentGroup[iG]呢？（wubo???）
                {
                    // 2.將該“子候選組”的該關鍵幀打上編號加入到“當前連續組”
                    ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
                    vCurrentConsistentGroups.push_back(cg);
                    vbConsistentGroup[iG]=true; //this avoid to include the same group more than once
                }

    （3）如果nCurrentConsistency大於等於3，那麼該”子候選組“代表的候選幀過關，進入mvpEnoughConsistentCandidates

                if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent)
                {
                    mvpEnoughConsistentCandidates.push_back(pCandidateKF);
                    bEnoughConsistent=true; //this avoid to insert the same candidate more than once
                }

之後要更新Covisibility Consistent Groups： mvConsistentGroups = vCurrentConsistentGroups;

添加當前關鍵幀到資料庫中：    mpKeyFrameDB->add(mpCurrentKF);

二、ComputeSim3()

計算Sim3

在當前關鍵幀和閉環幀之間找到更多的對應點，並通過這些對應點計算當前關鍵幀和閉環幀之間的Sim3變換，求解出Rt和s。

主要流程：

 * 1. 通過Bow加速描述子的匹配，利用RANSAC粗略地計算出當前幀與閉環幀的Sim3（當前幀---閉環幀）
 * 2. 根據估計的Sim3，對3D點進行投影找到更多匹配，通過優化的方法計算更精確的Sim3（當前幀---閉環幀）
 * 3. 將閉環幀以及閉環幀相連的關鍵幀的MapPoints與當前幀的點進行匹配（當前幀---閉環幀+相連關鍵幀）

1、將當前幀mpCurrentKF與閉環候選關鍵幀pKF匹配，生成匹配的特徵點vvpMapPointMatches

匹配的特徵點數太少，該候選幀剔除

正常情況下構造Sim3求解器

int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);

        // 匹配的特徵點數太少，該候選幀剔除
        if(nmatches<20)
        {
            vbDiscarded[i] = true;
            continue;
        }
        else
        {
            // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono)
            // 構造Sim3求解器
            // 如果mbFixScale為true，則是6DoFf優化（雙目 RGBD），如果是false，則是7DoF優化（單目）
            Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
            pSolver->SetRansacParameters(0.99,20,300);// 至少20個inliers 300次迭代
            vpSim3Solvers[i] = pSolver;
        }

        // 參與Sim3計算的候選關鍵幀數加1
        nCandidates++;

2、對上一步得到的每一個滿足條件的閉環幀，通過RANSAC迭代，求解Sim3

 一直迴圈所有的候選幀，每個候選幀迭代5次，如果5次迭代後得不到結果，就換下一個候選幀，直到有一個候選幀首次迭代成功bMatch為true，或者某個候選幀總的迭代次數超過限制，直接將它剔除。

            // 步驟3：對步驟2中有較好的匹配的關鍵幀求取Sim3變換
            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
            // 經過n次迴圈，每次迭代5次，總共迭代 n*5 次
            // 總迭代次數達到最大限制還沒有求出合格的Sim3變換，該候選幀剔除
            if(bNoMore)
            {
                vbDiscarded[i]=true;
                nCandidates--;
            }

3、通過返回的Sim3進行第二次匹配

剛才得到了Sim3，所以現在要利用Sim3再去進行匹配點的查詢，本次查詢的匹配點數量，會在原來的基礎上有所增加。 

                cv::Mat R = pSolver->GetEstimatedRotation();// 候選幀pKF到當前幀mpCurrentKF的R（R12）
                cv::Mat t = pSolver->GetEstimatedTranslation();// 候選幀pKF到當前幀mpCurrentKF的t（t12），當前幀座標系下，方向由pKF指向當前幀
                const float s = pSolver->GetEstimatedScale();// 候選幀pKF到當前幀mpCurrentKF的變換尺度s（s12）
                // 查詢更多的匹配（成功的閉環匹配需要滿足足夠多的匹配特徵點數，之前使用SearchByBoW進行特徵點匹配時會有漏匹配）
                // 通過Sim3變換，確定pKF1的特徵點在pKF2中的大致區域，同理，確定pKF2的特徵點在pKF1中的大致區域
                // 在該區域內通過描述子進行匹配捕獲pKF1和pKF2之前漏匹配的特徵點，更新匹配vpMapPointMatches
                matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);

4、使用非線性最小二乘法優化Sim3. 

在拿到了第二次匹配的結果以後，要通過這些匹配點，再去優化Sim3的值，從而再精細化Rt和s。 

                // 步驟5：Sim3優化，只要有一個候選幀通過Sim3的求解與優化，就跳出停止對其它候選幀的判斷
                // OpenCV的Mat矩陣轉成Eigen的Matrix型別
                g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
                // 如果mbFixScale為true，則是6DoFf優化（雙目 RGBD），如果是false，則是7DoF優化（單目）
                // 優化mpCurrentKF與pKF對應的MapPoints間的Sim3，得到優化後的量gScm
                const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);// 卡方chi2檢驗閾值

                // If optimization is succesful stop ransacs and continue
                if(nInliers>=20)
                {
                    bMatch = true;
                    // mpMatchedKF就是最終閉環檢測出來與當前幀形成閉環的關鍵幀
                    mpMatchedKF = pKF;
                    // 得到從世界座標系到該候選幀的Sim3變換，Scale=1
                    g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);
                    // 得到g2o優化後從世界座標系到當前幀的Sim3變換
                    mg2oScw = gScm*gSmw;
                    mScw = Converter::toCvMat(mg2oScw);

                    mvpCurrentMatchedPoints = vpMapPointMatches;
                    break;// 只要有一個候選幀通過Sim3的求解與優化，就跳出停止對其它候選幀的判斷
                }

5、取出閉環匹配上關鍵幀的相連關鍵幀，得到它們的MapPoints地圖點放入mvpLoopMapPoints

最後一步求解匹配點的時候，將所指的閉環幀和與其連結的關鍵幀所看到的所有的MapPoint都恢復出來。通過這個方法，可以儘可能得到我們當前關鍵幀所能看到的所有的地圖點，為下一步做投影匹配，得到更多的匹配點做準備。 

    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);
                    // 標記該MapPoint被mpCurrentKF閉環時觀測到並新增，避免重複新增
                    pMP->mnLoopPointForKF=mpCurrentKF->mnId;
                }
            }
        }
    }

6、使用投影得到更多的匹配點，如果匹配點數量充足，則接受該閉環。 

    matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);// 搜尋範圍係數為10

    // If enough matches accept Loop
    // 步驟8：判斷當前幀與檢測出的所有閉環關鍵幀是否有足夠多的MapPoints匹配
    int nTotalMatches = 0;
    for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
    {
        if(mvpCurrentMatchedPoints[i])
            nTotalMatches++;
    }

    // 步驟9：清空mvpEnoughConsistentCandidates
    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;
    }
}

三、CorrectLoop()

糾正閉環後端優化

上一步得到Sim3和匹配點之後，可以糾正當前幀的位姿，但是誤差不僅出現在當前幀，此前的每一幀都有累計誤差需要消除，需要CorrectLoop進行整體的調節。

閉環矯正的第一步是融合重複的點雲，並且在Covisibility Graph中插入新的邊以連線閉環。首先當前幀的位姿會根據相似變換而被矯正，同時所有與其相連的關鍵幀也會被矯正。所有的被閉環處的關鍵幀觀察到的地圖點會通過對映在一個小範圍裡，然後去搜索它的近鄰匹配。這樣就可以對所有匹配的點雲進行更加有效的資料融合，並更新關鍵幀位姿，以及在圖中的邊。

0、如果有區域性地圖和全域性BA運算在執行的話，終止。

    mpLocalMapper->RequestStop();

    // If a Global Bundle Adjustment is running, abort it
    if(isRunningGBA())
    {
        // 這個標誌位僅用於控制輸出提示，可忽略
        unique_lock<mutex> lock(mMutexGBA);
        mbStopGBA = true;

        mnFullBAIdx++;

        if(mpThreadGBA)
        {
            mpThreadGBA->detach();
            delete mpThreadGBA;
        }
    }

1、根據共視關係更新當前幀與其它關鍵幀之間的連線

    mpCurrentKF->UpdateConnections();

2、使用傳播法計算每一個關鍵幀正確的Sim3變換值

2.1 得到當前關鍵幀的附近關鍵幀的Sim3位姿並用糾正的Sim3位姿與其相乘，儲存結果到CorrectedSim3變數中。 

        for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
        {
            KeyFrame* pKFi = *vit;

            cv::Mat Tiw = pKFi->GetPose();

            // currentKF在前面已經新增
            if(pKFi!=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);
                g2o::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優化後各個關鍵幀的位姿
                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
            // 當前幀相連關鍵幀，沒有進行閉環g2o優化的位姿
            NonCorrectedSim3[pKFi]=g2oSiw;
        }

2.2 使用反向投影的方法，將當前關鍵幀和鄰居觀察到的地圖點得到三維場景下的位姿，並更新關鍵幀的位姿 

步驟2.1得到調整相連幀位姿後，修正這些關鍵幀的MapPoints

        // 步驟2.2：步驟2.1得到調整相連幀位姿後，修正這些關鍵幀的MapPoints
        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);
                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();
            }

2.3  將Sim3轉換為SE3，根據更新的Sim3，更新關鍵幀的位姿

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

3、檢查當前幀的MapPoints與閉環匹配幀的MapPoints是否存在衝突，其實融合就是判斷如果是同一個點的話，那麼將當前的地圖點強制換成原本的地圖點。 

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

    }

4、通過將閉環時相連關鍵幀的mvpLoopMapPoints投影到這些關鍵幀中，進行MapPoints檢查與替換。

5、 更新當前關鍵幀之間的共視相連關係，得到因閉環時MapPoints融合而新得到的連線關係

    map<KeyFrame*, set<KeyFrame*> > LoopConnections;

    // 步驟5.1：遍歷當前幀相連關鍵幀（一級相連）
    for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
    {
        KeyFrame* pKFi = *vit;
        // 步驟5.2：得到與當前幀相連關鍵幀的相連關鍵幀（二級相連）
        vector<KeyFrame*> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();

        // Update connections. Detect new links.
        // 步驟5.3：更新一級相連關鍵幀的連線關係
        pKFi->UpdateConnections();
        // 步驟5.4：取出該幀更新後的連線關係
        LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames();
        // 步驟5.5：從連線關係中去除閉環之前的二級連線關係，剩下的連線就是由閉環得到的連線關係
        for(vector<KeyFrame*>::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++)
        {
            LoopConnections[pKFi].erase(*vit_prev);
        }
        // 步驟5.6：從連線關係中去除閉環之前的一級連線關係，剩下的連線就是由閉環得到的連線關係
        for(vector<KeyFrame*>::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++)
        {
            LoopConnections[pKFi].erase(*vit2);
        }
    }

6、進行EssentialGraph優化

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

7、 添加當前幀與閉環匹配幀之間的邊（這個連線關係不優化）

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

8、新建一個執行緒用於全域性BA優化

    // OptimizeEssentialGraph只是優化了一些主要關鍵幀的位姿，這裡進行全域性BA可以全域性優化所有位姿和MapPoints
    mbRunningGBA = true;
    mbFinishedGBA = false;
    mbStopGBA = false;
    mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);