構造函數
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。