# LoopClosing.cpp

### LoopClosing::Run()
閉環線程
在[System::System()](#System::System())中引用

#### 函式流程
1. 查看閉環檢測隊列mlpLoopKeyFrameQueue中有沒有關鍵幀進來
2. 檢測是否有閉環
3. 計算sim3相似變換
4. 閉環校正

#### 函式詳解
1. 查看閉環檢測隊列mlpLoopKeyFrameQueue中有沒有關鍵幀進來

mbFinished =false;

// 線程主循環
while(1)
{
    if(CheckNewKeyFrames())
    {

跳到[LoopClosing::CheckNewKeyFrames()](#LoopClosing::CheckNewKeyFrames())

2. 檢測是否有閉環

        // Detect loop candidates and check covisibility consistency
        if(DetectLoop())
        {

跳到[LoopClosing::DetectLoop()](#LoopClosing::DetectLoop())

3. 計算sim3相似變換

           // Compute similarity transformation [sR|t]
           // In the stereo/RGBD case s=1
           if(ComputeSim3())
           {

跳到[LoopClosing::ComputeSim3()](#LoopClosing::ComputeSim3())

4. 閉環校正

               // Perform loop fusion and pose graph optimization
               CorrectLoop();
           }
        }
    }

    // 查看是否有外部線程請求復位當前線程
    ResetIfRequested();

    // 查看外部線程是否有終止當前線程的請求,如果有的話就跳出這個線程的主函數的主循環
    if(CheckFinish())
        break;

    //usleep(5000);
    std::this_thread::sleep_for(std::chrono::milliseconds(5));

}

// 運行到這裡說明有外部線程請求終止當前線程,在這個函數中執行終止當前線程的一些操作
SetFinish();

跳到[LoopClosing::CorrectLoop()](#LoopClosing::CorrectLoop())

### LoopClosing::CheckNewKeyFrames()
查看列表中是否有等待被插入的關鍵幀，如果存在，返回true
在[LoopClosing::Run()](#LoopClosing::Run())中引用

#### 函式流程
1. 列表中是否有等待被插入的關鍵幀

#### 函式詳解
1. 列表中是否有等待被插入的關鍵幀

unique_lock<mutex> lock(mMutexLoopQueue);
return(!mlpLoopKeyFrameQueue.empty());

mlpLoopKeyFrameQueue 在[LoopClosing::InsertKeyFrame()](#LoopClosing::InsertKeyFrame())中更新

### LoopClosing::InsertKeyFrame()
將某個關鍵幀加入到閉環檢測中
在[LocalMapping::Run()](#LocalMapping::Run())中引用

#### 定義

void LoopClosing::InsertKeyFrame(KeyFrame *pKF)    //輸入關鍵幀

#### 函式流程
1. 插入關鍵幀

#### 函式詳解

1. 插入關鍵幀

unique_lock<mutex> lock(mMutexLoopQueue);

// 注這裡第0個關鍵幀不能夠參與到閉環檢測的過程中,因為第0關鍵幀定義了整個地圖的世界坐標系
if(pKF->mnId!=0)
    mlpLoopKeyFrameQueue.push_back(pKF);

### LoopClosing::DetectLoop()
閉環檢測
在[LoopClosing::Run()](#LoopClosing::Run()))中引用

#### 函式流程
1. 從隊列中取出一個關鍵幀,作為當前檢測閉環關鍵幀
2. 如果距離上次閉環沒多久(小於10幀)，或者地圖中關鍵幀總共還沒有10幀，則不進行閉環檢測
3. 遍歷當前閉環關鍵幀所有連接(>15個共視地圖點)關鍵幀，計算當前關鍵幀與每個共視關鍵的詞袋相似度得分，並得到最低得分
4. 在所有關鍵幀中找出閉環候選幀（注意不和當前幀連接）
5. 將關鍵幀以及與關鍵幀相連的關鍵幀構成一個子候選組
6. 檢測子候選組中每一個關鍵幀在“子連續組”中是否存在
7. 如果判定為連續，接下來判斷是否達到連續的條件
8. 如果該“子候選組”的所有關鍵幀都和上次閉環無關（不連續），vCurrentConsistentGroups 沒有新添加連續關係

#### 函式詳解

1. 從隊列中取出一個關鍵幀,作為當前檢測閉環關鍵幀

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

2. 如果距離上次閉環沒多久(小於10幀)，或者地圖中關鍵幀總共還沒有10幀，則不進行閉環檢測

// 後者的體現是當mLastLoopKFid為0的時候
if(mpCurrentKF->mnId<mLastLoopKFid+10)
{
    mpKeyFrameDB->add(mpCurrentKF);
    mpCurrentKF->SetErase();
    return false;
}

3. 遍歷當前閉環關鍵幀所有連接(>15個共視地圖點)關鍵幀，計算當前關鍵幀與每個共視關鍵的詞袋相似度得分，並得到最低得分

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

4. 在所有關鍵幀中找出閉環候選幀（注意不和當前幀連接）

// minScore的作用：認為和當前關鍵幀具有回環關係的關鍵幀,不應該低於當前關鍵幀的相鄰關鍵幀的最低的相似度minScore
// 得到的這些關鍵幀,和當前關鍵幀具有較多的公共單詞,並且相似度評分都挺高
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);

// If there are no loop candidates, just add new keyframe and return false
// 如果沒有閉環候選幀，返回false
if(vpCandidateKFs.empty())
{
    mpKeyFrameDB->add(mpCurrentKF);
    mvConsistentGroups.clear();
    mpCurrentKF->SetErase();
    return false;           
}

跳到[KeyFrameDatabase::DetectLoopCandidates()](#KeyFrameDatabase::DetectLoopCandidates())

// 1、每個候選幀將與自己相連的關鍵幀構成一個“子候選組spCandidateGroup”， vpCandidateKFs-->spCandidateGroup
// 2、檢測“子候選組”中每一個關鍵幀是否存在於“連續組”，如果存在 nCurrentConsistency++，則將該“子候選組”放入“當前連續組vCurrentConsistentGroups”
// 3、如果nCurrentConsistency大於等於3，那麼該”子候選組“代表的候選幀過關，進入mvpEnoughConsistentCandidates

// 相關的概念說明:
// 組(group): 對於某個關鍵幀, 其和其具有共視關係的關鍵幀組成了一個"組";
// 子候選組(CandidateGroup): 對於某個候選的回環關鍵幀, 其和其具有共視關係的關鍵幀組成的一個"組";
// 連續(Consistent):  不同的組之間如果共同擁有一個及以上的關鍵幀,那麼稱這兩個組之間具有連續關係
// 連續性(Consistency):稱之為連續長度可能更合適,表示累計的連續的鏈的長度:A--B 為1, A--B--C--D 為3等;具體反映在數據類型 ConsistentGroup.second上
// 連續組(Consistent group): mvConsistentGroups存儲了上次執行回環檢測時, 新的被檢測出來的具有連續性的多個組的集合.由於組之間的連續關係是個網狀結構,因此可能存在
//                          一個組因為和不同的連續組鏈都具有連續關係,而被添加兩次的情況(當然連續性度量是不相同的)
// 連續組鏈:自造的稱呼,類似於菊花鏈A--B--C--D這樣形成了一條連續組鏈.對於這個例子中,由於可能E,F都和D有連續關係,因此連續組鏈會產生分叉;為了簡化計算,連續組中將只會保存
//         最後形成連續關係的連續組們(見下面的連續組的更新)
// 子連續組: 上面的連續組中的一個組
// 連續組的初始值: 在遍歷某個候選幀的過程中,如果該子候選組沒有能夠和任何一個上次的子連續組產生連續關係,那麼就將添加自己組為連續組,並且連續性為0(相當於新開了一個連續鏈)
// 連續組的更新: 當前次回環檢測過程中,所有被檢測到和之前的連續組鏈有連續的關係的組,都將在對應的連續組鏈後面+1,這些子候選組(可能有重複,見上)都將會成為新的連續組;
//              換而言之連續組mvConsistentGroups中只保存連續組鏈中末尾的組

5. 將關鍵幀以及與關鍵幀相連的關鍵幀構成一個子候選組

// 最終篩選後得到的閉環幀
mvpEnoughConsistentCandidates.clear();

// ConsistentGroup數據類型為pair<set<KeyFrame*>,int>
// ConsistentGroup.first對應每個“連續組”中的關鍵幀，ConsistentGroup.second為每個“連續組”的已連續幾個的序號

vector<ConsistentGroup> vCurrentConsistentGroups;

// 這個下標是每個"子連續組"的下標,bool表示當前的候選組中是否有和該組相同的一個關鍵幀
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);

跳到[KeyFrame::GetConnectedKeyFrames()](#KeyFrame::GetConnectedKeyFrames())

6. 檢測子候選組中每一個關鍵幀在“子連續組”中是否存在

    // 連續性達標的標誌
    bool bEnoughConsistent = false;
    bool bConsistentForSomeGroup = false;

    // 上一次閉環的連續組鏈 std::vector<ConsistentGroup> mvConsistentGroups
    // 其中 ConsistentGroup的定義：typedef pair<set<KeyFrame*>,int> ConsistentGroup
    // 其中 ConsistentGroup.first對應每個“連續組”中的關鍵幀集合，ConsistentGroup.second為每個“連續組”的連續長度
    
    //遍歷前一次閉環檢測到的子連續組
    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;
            }
        }

7. 如果判定為連續，接下來判斷是否達到連續的條件

        if(bConsistent)
        {
            // 取出和當前的候選組發生"連續"關係的子連續組的"已連續次數"
            int nPreviousConsistency = mvConsistentGroups[iG].second;
            // 將當前候選組連續長度+1，
            int nCurrentConsistency = nPreviousConsistency + 1;
            // 如果上述連續關係還未記錄到 vCurrentConsistentGroups，那麼記錄一下
            // 注意這裡spCandidateGroup 可能放置在vbConsistentGroup中其他索引(iG)下
            if(!vbConsistentGroup[iG])
            {
                // 將該“子候選組”的該關鍵幀打上連續編號加入到“當前連續組”
                ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
                // 放入本次閉環檢測的連續組vCurrentConsistentGroups裡
                vCurrentConsistentGroups.push_back(cg);
                //this avoid to include the same group more than once
                // 標記一下，防止重複添加到同一個索引iG
                // 但是spCandidateGroup可能重複添加到不同的索引iG對應的vbConsistentGroup 中
                vbConsistentGroup[iG]=true; 
            }
            
            // 如果連續長度滿足要求，那麼當前的這個候選關鍵幀是足夠靠譜的
            // 連續性閾值 mnCovisibilityConsistencyTh=3
            // 足夠連續的標記 bEnoughConsistent
            if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent)
            {
                // 記錄為達到連續條件了
                mvpEnoughConsistentCandidates.push_back(pCandidateKF);

                // 標記一下，防止重複添加
                bEnoughConsistent=true; 
            }
        }
    }

8. 如果該“子候選組”的所有關鍵幀都和上次閉環無關（不連續），vCurrentConsistentGroups 沒有新添加連續關係

    // 於是就把“子候選組”全部拷貝到 vCurrentConsistentGroups， 用於更新mvConsistentGroups，連續性計數器設為0
    if(!bConsistentForSomeGroup)
    {
        ConsistentGroup cg = make_pair(spCandidateGroup,0);
        vCurrentConsistentGroups.push_back(cg);
    }
}// 遍歷得到的初級的候選關鍵幀

// 更新連續組
mvConsistentGroups = vCurrentConsistentGroups;

// 當前閉環檢測的關鍵幀添加到關鍵幀數據庫中
mpKeyFrameDB->add(mpCurrentKF);

if(mvpEnoughConsistentCandidates.empty())
{
    // 未檢測到閉環，返回false
    mpCurrentKF->SetErase();
    return false;
}
else 
{
    // 成功檢測到閉環，返回true
    return true;
}

// 多餘的代碼,執行不到
mpCurrentKF->SetErase();
return false;

### LoopClosing::ComputeSim3()
計算當前幀與閉環幀的Sim3變換
在[LoopClosing::Run()](#LoopClosing::Run())

#### 函式流程
1. 遍歷閉環候選幀集，初步篩選出與當前關鍵幀的匹配特徵點數大於20的候選幀集合，並為每一個候選幀構造一個Sim3Solver
1.1. 從篩選的閉環候選幀中取出一幀有效關鍵幀pKF
1.2. 將當前幀 mpCurrentKF 與閉環候選關鍵幀pKF匹配
1.3. 為保留的候選幀構造Sim3求解器
2. 對每一個候選幀用Sim3Solver 迭代匹配， 直到有一個候選幀匹配成功，或者全部失敗
2.1. 取出從 1.3. 中為當前候選幀構建的 Sim3Solver 並開始迭代
2.2. 通過上面求取的Sim3變換引導關鍵幀匹配，彌補Step 1中的漏匹配
2.3. 用新的匹配來優化 Sim3，只要有一個候選幀通過Sim3的求解與優化，就跳出停止對其它候選幀的判斷
3. 取出與當前幀閉環匹配上的關鍵幀及其共視關鍵幀，以及這些共視關鍵幀的地圖點
4. 將閉環關鍵幀及其連接關鍵幀的所有地圖點投影到當前關鍵幀進行投影匹配
5. 統計當前幀與閉環關鍵幀的匹配地圖點數目，超過40個說明成功閉環，否則失敗

#### 函式詳解

// 對每個（上一步得到的具有足夠連續關係的）閉環候選幀都準備算一個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);

// 存儲每一個候選幀的Sim3Solver求解器
vector<Sim3Solver*> vpSim3Solvers;
vpSim3Solvers.resize(nInitialCandidates);

// 存儲每個候選幀的匹配地圖點信息
vector<vector<MapPoint*> > vvpMapPointMatches;
vvpMapPointMatches.resize(nInitialCandidates);

// 存儲每個候選幀應該被放棄(True）或者 保留(False)
vector<bool> vbDiscarded;
vbDiscarded.resize(nInitialCandidates);

// 完成 Step 1 的匹配後，被保留的候選幀數量
int nCandidates=0;

1. 遍歷閉環候選幀集，初步篩選出與當前關鍵幀的匹配特徵點數大於20的候選幀集合，並為每一個候選幀構造一個Sim3Solver

// 遍歷閉環候選幀集
for(int i=0; i<nInitialCandidates; i++)
{
// 1.1. 從篩選的閉環候選幀中取出一幀有效關鍵幀pKF
    KeyFrame* pKF = mvpEnoughConsistentCandidates[i];

    // 避免在LocalMapping中KeyFrameCulling函數將此關鍵幀作為冗餘幀剔除
    pKF->SetNotErase();

    // 如果候選幀質量不高，直接PASS
    if(pKF->isBad())
    {
        vbDiscarded[i] = true;
        continue;
    }

// 1.2. 將當前幀 mpCurrentKF 與閉環候選關鍵幀pKF匹配
    // 通過bow加速得到 mpCurrentKF 與 pKF 之間的匹配特徵點
    // vvpMapPointMatches 是匹配特徵點對應的地圖點,本質上來自於候選閉環幀
    int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);

    // 粗篩：匹配的特徵點數太少，該候選幀剔除
    if(nmatches<20)
    {
        vbDiscarded[i] = true;
        continue;
    }
    else
    {
// 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++;
}

Sim3Solver :跳到[Sim3Solver::Sim3Solver()](#Sim3Solver::Sim3Solver())
pSolver->SetRansacParameters :跳到[Sim3Solver::SetRansacParameters()](#Sim3Solver::SetRansacParameters())

2. 對每一個候選幀用Sim3Solver 迭代匹配， 直到有一個候選幀匹配成功，或者全部失敗

// 用於標記是否有一個候選幀通過Sim3Solver的求解與優化
bool bMatch = false;

while(nCandidates>0 && !bMatch)
{
    // 遍歷每一個候選幀
    for(int i=0; i<nInitialCandidates; i++)
    {
        if(vbDiscarded[i])
            continue;

        KeyFrame* pKF = mvpEnoughConsistentCandidates[i];

        // 內點（Inliers）標誌
        // 即標記經過RANSAC sim3 求解後,vvpMapPointMatches中的哪些作為內點
        vector<bool> vbInliers; 

        // 內點（Inliers）數量
        int nInliers;

        // 是否到達了最優解
        bool bNoMore;

// 2.1. 取出從 1.3. 中為當前候選幀構建的 Sim3Solver 並開始迭代
        Sim3Solver* pSolver = vpSim3Solvers[i];

        // 最多迭代5次，返回的Scm是候選幀pKF到當前幀mpCurrentKF的Sim3變換（T12）
        cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

        // 總迭代次數達到最大限制還沒有求出合格的Sim3變換，該候選幀剔除
        if(bNoMore)
        {
            vbDiscarded[i]=true;
            nCandidates--;
        }

        // 如果計算出了Sim3變換，繼續匹配出更多點並優化。因為之前 SearchByBoW 匹配可能會有遺漏
        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];
            }

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

// 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，得到優化後的量gScm
            const 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=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;

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

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

pSolver->iterate : 跳到[Sim3Solver::iterate()](#Sim3Solver::iterate())
pSolver->GetEstimatedRotation() :跳到[Sim3Solver::GetEstimatedRotation()](#Sim3Solver::GetEstimatedRotation())
pSolver->GetEstimatedTranslation() :跳到[Sim3Solver::GetEstimatedTranslation()](#Sim3Solver::GetEstimatedTranslation())
pSolver->GetEstimatedScale() : 跳到[Sim3Solver::GetEstimatedScale()](#Sim3Solver::GetEstimatedScale())
matcher.SearchBySim3() :跳到[ORBmatcher::SearchBySim3()](#ORBmatcher::SearchBySim3())
Optimizer::OptimizeSim3 : 跳到[Optimizer::OptimizeSim3](#Optimizer::OptimizeSim3)

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

// 注意是閉環檢測出來與當前幀形成閉環的關鍵幀 mpMatchedKF
// 將mpMatchedKF共視的關鍵幀全部取出來放入 vpLoopConnectedKFs
// 將vpLoopConnectedKFs的地圖點取出來放入mvpLoopMapPoints
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;
            }
        }
    }
}

4. 將閉環關鍵幀及其連接關鍵幀的所有地圖點投影到當前關鍵幀進行投影匹配

// 根據投影查找更多的匹配（成功的閉環匹配需要滿足足夠多的匹配特徵點數）
// 根據Sim3變換，將每個mvpLoopMapPoints投影到mpCurrentKF上，搜索新的匹配對
// mvpCurrentMatchedPoints是前面經過SearchBySim3得到的已經匹配的點對，這裡就忽略不再匹配了
// 搜索範圍係數為10
matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);

跳到[ORBmatcher::SearchByProjection()sim3](#ORBmatcher::SearchByProjection()sim3)

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

### LoopClosing::CorrectLoop()
閉環校正
在[LoopClosing::Run()](#LoopClosing::Run())中引用

#### 函式流程

#### 函式詳解

cout << "Loop detected!" << endl;
// Step 0：結束局部地圖線程、全局BA，為閉環矯正做準備
// Step 1：根據共視關係更新當前幀與其它關鍵幀之間的連接
// Step 2：通過位姿傳播，得到Sim3優化後，與當前幀相連的關鍵幀的位姿，以及它們的MapPoints
// Step 3：檢查當前幀的MapPoints與閉環匹配幀的MapPoints是否存在衝突，對沖突的MapPoints進行替換或填補
// Step 4：通過將閉環時相連關鍵幀的mvpLoopMapPoints投影到這些關鍵幀中，進行MapPoints檢查與替換
// Step 5：更新當前關鍵幀之間的共視相連關係，得到因閉環時MapPoints融合而新得到的連接關係
// Step 6：進行EssentialGraph優化，LoopConnections是形成閉環後新生成的連接關係，不包括步驟7中當前幀與閉環匹配幀之間的連接關係
// Step 7：添加當前幀與閉環匹配幀之間的邊（這個連接關係不優化）
// Step 8：新建一個線程用於全局BA優化

// g2oSic： 當前關鍵幀 mpCurrentKF 到其共視關鍵幀 pKFi 的Sim3 相對變換
// mg2oScw: 世界坐標係到當前關鍵幀的 Sim3 變換
// g2oCorrectedSiw：世界坐標係到當前關鍵幀共視關鍵幀的Sim3 變換


// 結束局部地圖線程、全局BA，為閉環矯正做準備
// 請求局部地圖停止，防止在回環矯正時局部地圖線程中InsertKeyFrame函數插入新的關鍵幀
mpLocalMapper->RequestStop();

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

// 一直等到局部地圖線程結束再繼續
while(!mpLocalMapper->isStopped())
{
    std::this_thread::sleep_for(std::chrono::milliseconds(1));
}

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

// 因為之前閉環檢測、計算Sim3中改變了該關鍵幀的地圖點，所以需要更新
mpCurrentKF->UpdateConnections();

跳到[KeyFrame::UpdateConnections()](#KeyFrame::UpdateConnections())

2. 通過姿態傳遞，得到Sim3優化後，與當前幀相連的關鍵幀的姿態，以及它們的地圖點

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

// 取出當前關鍵幀及其共視關鍵幀，稱為“當前關鍵幀組”
mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
mvpCurrentConnectedKFs.push_back(mpCurrentKF);

// CorrectedSim3：存放閉環g2o優化後當前關鍵幀的共視關鍵幀的世界坐標系下Sim3 變換
// NonCorrectedSim3：存放沒有矯正的當前關鍵幀的共視關鍵幀的世界坐標系下Sim3 變換
KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
// 先將mpCurrentKF的Sim3變換存入，認為是準的，所以固定不動
CorrectedSim3[mpCurrentKF]=mg2oScw;
// 當前關鍵幀到世界坐標系下的變換矩陣
cv::Mat Twc = mpCurrentKF->GetPoseInverse();

// 對地圖點操作
{
    // 鎖定地圖點
    unique_lock<mutex> lock(mpMap->mMutexMapUpdate);

// 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); //r
            cv::Mat tic = Tic.rowRange(0,3).col(3);        //t

            // g2oSic：當前關鍵幀 mpCurrentKF 到其共視關鍵幀 pKFi 的Sim3 相對變換
            // 這裡是non-correct, 所以scale=1.0
            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優化後當前關鍵幀的共視關鍵幀的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.2. 得到矯正的當前關鍵幀的共視關鍵幀位姿後，修正這些共視關鍵幀的地圖點
    // 遍歷待矯正的共視關鍵幀（不包括當前關鍵幀）
    for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++)
    {
        // 取出當前關鍵幀連接關鍵幀
        KeyFrame* pKFi = mit->first;
        // 取出經過位姿傳播後的Sim3變換
        g2o::Sim3 g2oCorrectedSiw = mit->second;
        g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
        // 取出未經過位姿傳播的Sim3變換
        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) 內部做了相似變換 s*R*P +t  
            // 下面變換是：eigP3Dw： world →g2oSiw→ i →g2oCorrectedSwi→ world
            Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));

            cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
            pMPi->SetWorldPos(cvCorrectedP3Dw);
            // 記錄矯正該地圖點的關鍵幀id，防止重複
            pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
            // 記錄該地圖點所在的關鍵幀id
            pMPi->mnCorrectedReference = pKFi->mnId;
            // 因為地圖點更新了，需要更新其平均觀測方向以及觀測距離範圍
            pMPi->UpdateNormalAndDepth();
        }

        // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
// 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);
        // 設置矯正後的新的pose
        pKFi->SetPose(correctedTiw);

        // Make sure connections are updated
// 2.4. 根據共視關係更新當前幀與其它關鍵幀之間的連接
        // 地圖點的位置改變了,可能會引起共視關係\權值的改變 
        pKFi->UpdateConnections();
    }

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

}

4. 將閉環相連關鍵幀組mvpLoopMapPoints 投影到當前關鍵幀組中，進行匹配，融合，新增或替換當前關鍵幀組中KF的地圖點

// 因為 閉環相連關鍵幀組mvpLoopMapPoints 在地圖中時間比較久經歷了多次優化，認為是準確的
// 而當前關鍵幀組中的關鍵幀的地圖點是最近新計算的，可能有累積誤差
// CorrectedSim3：存放矯正後當前關鍵幀的共視關鍵幀，及其世界坐標系下Sim3 變換
SearchAndFuse(CorrectedSim3);

跳到[LoopClosing::SearchAndFuse()](#LoopClosing::SearchAndFuse())

5. 更新當前關鍵幀組之間的兩級共視相連關係，得到因閉環時地圖點融合而新得到的連接關係

// LoopConnections：存儲因為閉環地圖點調整而新生成的連接關係
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. 進行本質圖優化，優化本質圖中所有關鍵幀的位姿和地圖點

// LoopConnections是形成閉環後新生成的連接關係，不包括步驟7中當前幀與閉環匹配幀之間的連接關係
Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);

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

// 它在下一次的本質圖優化里面使用
mpMatchedKF->AddLoopEdge(mpCurrentKF);
mpCurrentKF->AddLoopEdge(mpMatchedKF);

8. 新建一個線程用於全局BA優化

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

// Loop closed. Release Local Mapping.
mpLocalMapper->Release();    

cout << "Loop Closed!" << endl;

mLastLoopKFid = mpCurrentKF->mnId;

### LoopClosing::SearchAndFuse()
通過將閉環時相連關鍵幀的地圖點投影到這些關鍵幀中，進行地圖點檢查與替換
在[LoopClosing::CorrectLoop()](#LoopClosing::CorrectLoop())中引用

#### 定義

void LoopClosing::SearchAndFuse(
    const KeyFrameAndPose &CorrectedPosesMap) //矯正的當前KF對應的共視關鍵幀及Sim3變換

#### 函式流程

#### 函式詳解
1. 遍歷待矯正的當前KF的相連關鍵幀

// 定義ORB匹配器
ORBmatcher matcher(0.8);

for(KeyFrameAndPose::const_iterator mit=CorrectedPosesMap.begin(), mend=CorrectedPosesMap.end(); mit!=mend;mit++)
{
    KeyFrame* pKF = mit->first;
    // 矯正過的Sim 變換
    g2o::Sim3 g2oScw = mit->second;
    cv::Mat cvScw = Converter::toCvMat(g2oScw);

2. 將mvpLoopMapPoints投影到pKF幀匹配，檢查地圖點衝突並融合

    // mvpLoopMapPoints：與當前關鍵幀閉環匹配上的關鍵幀及其共視關鍵幀組成的地圖點
    vector<MapPoint*> vpReplacePoints(mvpLoopMapPoints.size(),static_cast<MapPoint*>(NULL));
    // vpReplacePoints：存儲mvpLoopMapPoints投影到pKF匹配後需要替換掉的新增地圖點,索引和mvpLoopMapPoints一致，初始化為空
    // 搜索區域係數為4
    matcher.Fuse(pKF,cvScw,mvpLoopMapPoints,4,vpReplacePoints);

    // Get Map Mutex
    // 之所以不在上面 Fuse 函數中進行地圖點融合更新的原因是需要對地圖加鎖
    unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
    const int nLP = mvpLoopMapPoints.size();

3. 替換掉需要替換的地圖點

    // 遍歷閉環幀組的所有的地圖點，
    for(int i=0; i<nLP;i++)
    {
        MapPoint* pRep = vpReplacePoints[i];
        if(pRep)
        {
            // 如果記錄了需要替換的地圖點
            // 用mvpLoopMapPoints替換掉vpReplacePoints裡記錄的要替換的地圖點
            pRep->Replace(mvpLoopMapPoints[i]);
        }
    }
}