ORB-SLAM（十）LoopClosing

 構造函數

LoopClosing(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale);

 主要分兩部分，迴環檢測，以及GlobalBundleAdjustment

 

LoopClosing中的關鍵幀都是LocalMapping中送過來的：送過來一幀，就檢查一幀。

// in LocalMapping::Run()
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);

// in LoopClosing.cpp
void LoopClosing::InsertKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexLoopQueue);
    if(pKF->mnId!=0)
        mlpLoopKeyFrameQueue.push_back(pKF);
}

所以咱們須要在KeyFrameDataBase中尋找與mlpLoopKeyFrameQueue類似的閉環候選幀。主要過程包括：

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

尋找回環候選幀，檢查候選幀連續性；計算Sim3；閉環（地圖點融合，位姿圖優化）。

一. DetectLoop()

在共視關係中找到與當前關鍵幀Bow匹配最低得分minScore，在除去當前幀共視關係的關鍵幀數據庫中，檢測閉環候選幀：

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

閉環候選幀篩選過程：

1. Bow得分>minScore；

2. 統計知足1的關鍵幀中有共同單詞最多的單詞數maxcommonwords；

3. 篩選出共同單詞數大於mincommons(=0.8*maxcommons)的關鍵幀；

4. 相連的關鍵幀分爲一組，計算組得分（總分），獲得最大總分bestAccScore，篩選出總分大於minScoreToRetain(=0.75*bestAccScore)的組，用組中得分最高的候選幀lAccScoreAndMatch表明該組。計算組得分的目的是剔除單獨一幀得分較高，可是沒有共視關鍵幀，做爲閉環來講不夠魯棒。

對於經過了閉環檢測的關鍵幀，還須要經過連續性檢測（連續三幀都經過上面的篩選），才能做爲閉環候選幀。

mvpEnoughConsistentCandidates

二. ComputeSim3()

計算當前關鍵幀和閉環候選幀之間的Sim3，這個Sim3變換就是閉環前積累的尺度和位姿偏差，該偏差也能夠幫助檢驗該閉環在空間幾何姿態上是否成立。

1. Bow

經過SearchByBow搜索當前關鍵幀中和閉環候選幀匹配的地圖點。(Bow經過將單詞聚類到樹結構node的方法，加快搜索匹配速度)

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

若nmatches<20，剔除該候選幀，將匹配好的地圖點放入當前候選幀對應的vvpMapPointMatches[i]中。

注意這裏使用Bow匹配速度較快，可是會有漏匹配。

2. RANSAC

利用上面匹配上的地圖點（雖然匹配上了，可是空間位置相差一個Sim3），用RANSAC方法求解Sim3位姿。（這裏有可能求解不出Sim3，也就是雖然匹配知足，可是空間幾何姿態不知足）。

這裏Sim3的求解須要參考論文 Horn B K P. Closed-form solution of absolute orientation using unit quaternions. JOSA A, 1987, 4(4): 629-642.

Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
pSolver->SetRansacParameters(0.99,20,300);// 至少20個inliers 300次迭代

cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

3. SearchBySim3

根據計算出的Sim3（s，R，t），去找更多的匹配點（SearchBySim3），更新vpMapPointMatches

matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);

4. Optimize

使用更新過的匹配，使用g2o優化Sim3位姿，這時內點數nInliers>20，才說明經過。一旦找到閉環幀mpMatchedKF，則break，跳過對其餘候選閉環幀的判斷。

const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);

5. SearchByProjection

獲取mpMatchedKF及其相連關鍵幀對應的地圖點。將這些地圖點經過上面優化獲得的Sim3（gScm-->mScw）變換投影到當前關鍵幀進行匹配，若匹配點>=40個，則返回ture，進行閉環調整，不然，返回false，線程暫停5ms後繼續檢查Tracking發送來的關鍵幀隊列。

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

注意這裏獲得的當前關鍵幀中匹配上閉環關鍵幀共視地圖點（mvpCurrentMatchedPoints），將用於後面CorrectLoop時當前關鍵幀地圖點的衝突融合。

到這裏，不只確保了當前關鍵幀與閉環幀之間匹配度高，並且保證了閉環幀的共視圖中的地圖點和當前關鍵幀的特徵點匹配度更高（20--->40），說明該閉環幀是正確的。

三. CorrectLoop()

閉環糾正時，LocalMapper和Global BA必須中止。注意Global BA使用的是單獨的線程。

分爲兩步，第一步LoopFusion，第二步Essential Graph優化。

其中Essential Graph包含三部分：1. 共視關係很好的關鍵幀；2. spanning tree鏈接關係（父子關係）；3. 閉環關鍵幀鏈接關係。

使用計算出的Sim3對當前位姿進行調整，而且傳播到當前關鍵幀相連的關鍵幀（相連關鍵幀之間相對位姿是知道的，經過當前關鍵幀的Sim3計算相連關鍵幀的Sim3）。這樣迴環的兩側關鍵幀就對齊了，利用調整過的位姿更新這些相連關鍵幀對應的地圖點。而後將閉環幀及其相連幀的地圖點都投影到當前幀以及相連幀上，投影匹配上的和Sim3計算過的地圖點進行融合（就是替換成質量高的），涉及融合的關鍵幀還須要更新其在共視地圖中的觀測邊關係，這是爲了剝離出由於閉環產生的新的鏈接關係LoopConnections，用於優化Essential Graph。添加當前幀與閉環匹配幀之間的邊，該邊不參與優化。

記住地圖點是鏈接關鍵幀之間的樞紐，每次調整地圖點位置後都須要更新關鍵幀的鏈接關係。

下面新開一個線程進行全局優化。OptimizeEssentialGraph只是優化一些主要關鍵幀，全局優化能夠優化全部的位姿與MapPoints。