現在開始學習下閉環檢測線程。
LocalMapping線程把關鍵幀送到了mlploopKeyFrameQueue隊列中，我們檢查該隊列是否為空，如果有先的關鍵幀進來，那么就開始進行回環檢測。LoopClosing中的關鍵幀都是LocalMapping中送過來的：送過來一幀，就檢查一幀.
Funtion1:DetectLoop()
從隊列中取出一個關鍵幀，作為當前關鍵幀，並設為不被擦除的特性，接下來判斷距離上一次閉環檢測是否超過10幀，如果是的話那么把當前關鍵幀加入mpKeyFrameDB中，並擦除當前關鍵幀。
遍歷所有共視關鍵幀，計算當前關鍵幀與每個共視關鍵的bow相似度得分，並得到最低得分minScore，這是一個閾值，如果得分比這個最低得分還要低的話，那么就是說肯定不會成為閉環關鍵幀。
Funtion1.1:pKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore)
在所有關鍵幀(pKeyFrameDB)中找出閉環備選幀。
* 1. 找出和當前幀具有公共單詞的所有關鍵幀（不包括與當前幀相連的關鍵幀），統計所有閉環候選幀中與pKF具有共同單詞最多的單詞數，並以這個最多單詞數的0.8倍作為一個閾值。
* 2. 只和共同單詞大於最多單詞數的0.8倍這個閾值的關鍵幀進行相似度計算，選出相似度大於 minScore的關鍵幀。
* 3.單單計算當前幀和某一關鍵幀的相似性是不夠的，這里將與關鍵幀相連（權值最高，共視程度最高）的前十個關鍵幀歸為一組，計算累計得分，選出最高的組得分，然后以此的0.75 倍作為閾值，得分大於這個閾值的組，作為得分較高的組。
* 4. 只返回累計這些得分較高的組中分數最高的關鍵幀。
ok，這樣我們就得到了閉環候選關鍵幀組。
如果檢測結束后，得不到這個候選關鍵幀組，那么在關鍵幀數據庫里添加此幀，返回false。
接下來檢查關鍵幀的連續性。計算組得分的目的是剔除單獨一幀得分較高，但是沒有共視關鍵幀，作為閉環來說不夠魯棒。對於通過了閉環檢測的關鍵幀，還需要通過連續性檢測（連續三幀都通過上面的篩選），才能作為閉環候選幀。
最終要達到的目的就是如上圖所示，接連3幀檢測到的閉環關鍵幀是聯系的。
在程序上的實現：先是進來一組候選關鍵幀，每一幀將自己以及與自己相連的關鍵幀構成一個“子候選組”。當系統剛開始，或者未檢測到有連續時，”子連續組“里的每 個”子候選組“的序號都為0。之后的關鍵幀的候選關鍵幀組來的時候，每個組里的候選關鍵幀組成自己的”子候選組“，與之前的”子連續組“里的”子候選組“依次比較。如果相連的話，把相連的”子候選幀組“和序號壓入當前連續組，這個序號比前一組”子連續組“的序號+1。如果nCurrentConsistency大於等於3，那么該”子候選組“代表的候選幀過關，進入mvpEnoughConsistentCandidates。最后，循環結束，當前連續組送給系統連續組用於下一次檢測。並把當前幀壓入關鍵幀數據庫。
Funtion2:ComputeSim3()
依次從篩選的閉環候選幀中取出一幀關鍵幀pKF。
Funtion2.1:nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i])
將當前幀mpCurrentKF與閉環候選關鍵幀pKF匹配，通過bow加速得到mpCurrentKF與pKF之間的匹配特征點，vvpMapPointMatches和GetMapPointMatches在同一個索引下對應的是同一個mappoint，這部分需要和跟蹤線程的這個函數做區別。
如果返回的匹配數量多於20個的話，那么就給當前幀和這個關鍵幀構造sim3求解器。
Funtion2.2:nmatches = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale)
將上一步得到的匹配的mappoint對，表達成各自坐標系下的空間點，並變化為像平面下的坐標。
Funtion2.3:SetRansacParameters(0.99,20,300)
RANSAC的相關內容可以參考：隨機抽樣一致。
Funtion2.4:Scm = pSolver->iterate(5,bNoMore,vbInliers,nInliers)
對有較好的匹配的關鍵幀求取Sim3變換，最多迭代5次，Ransac求解mvX3Dc1和mvX3Dc2之間Sim3，函數返回mvX3Dc2到mvX3Dc1的Sim3變換，也就是候選幀pKF到當前幀mpCurrentKF的Sim3變換（T12）。當這個函數迭代的次數不超過5次或者總的迭代次數不超過300次，那么就任意取三組點根據兩組匹配的3D點，計算之間的Sim3變換。
Funtion2.4.1: ComputeSim3(P3Dc1i,P3Dc2i)
這里記錄下求解sim3的算法：參考論文
論文中提到：公式11變換后的式子中的D越大，剩余誤差會越小，所以我們就必須找到旋轉矩陣讓式子
盡量越大越好。
也就是
，所以D就可以表示成
=
，然后：
，
最后指出四元數的解就是對應的N的最大特征值的列向量。證明見論文中附錄A3。
在程序的的設計上也是先根據去心的匹配點對構造出了矩陣M，然后構造矩陣N，N矩陣最大特征值（第一個特征值）對應特征向量就是要求的四元數死（q0 q1 q2 q3）， 將(q1 q2 q3)放入vec行向量，vec就是四元數旋轉軸乘以sin(ang/2)，計算旋轉的角度和旋轉向量，再計算出旋轉矩陣。再求出尺度（用的非對稱模式）和平移量，最后給出位姿變換矩陣。
每次得到計算的sim3變換后，都能前幾次變換成功后的mappoint集合起來得到更多的maoppoint。
Funtion2.5:matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5)
查找更多的匹配（成功的閉環匹配需要滿足足夠多的匹配特征點數，之前使用SearchByBoW進行特征點匹配時會有漏匹配）,通過Sim3變換，確定pKF1的特征點在pKF2中的大致區域，同理，確定pKF2的特征點在pKF1中的大致區域。在該區域內通過描述子進行匹配捕獲pKF1和pKF2之前漏匹配的特征點，如果成功對上了，則認為是互相匹配上了。更新匹配vpMapPointMatches。
Funtion2.7:nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale)
如果mbFixScale為true，則是6DoFf優化（雙目 RGBD），如果是false，則是7DoF優化（單目），優化mpCurrentKF與pKF對應的MapPoints間的Sim3，得到優化后的量gScm。
如果優化后，通過卡方檢驗的內點數超過20的話，那么mpMatchedKF就是最終閉環檢測出來與當前幀形成閉環的關鍵幀，得到從世界坐標系到該候選幀的Sim3變換，Scale=1， 得到g2o優化后從世界坐標系到當前幀的Sim3變換。
只要有一個候選幀通過Sim3的求解與優化，就跳出停止對其它候選幀的判斷。
如果沒有一個閉環匹配候選幀通過Sim3的求解與優化，清空mvpEnoughConsistentCandidates。
取出閉環匹配上關鍵幀的相連關鍵幀，得到它們的MapPoints放入mvpLoopMapPoints，在執行上： 將mpMatchedKF相連的關鍵幀全部取出來放入vpLoopConnectedKFs，將vpLoopConnectedKFs的MapPoints取出來放入mvpLoopMapPoints。
Funtion2.8:matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10)
將閉環匹配上關鍵幀以及相連關鍵幀的MapPoints投影到當前關鍵幀進行投影匹配。投影用的是mscw。根據投影查找更多的匹配（成功的閉環匹配需要滿足足夠多的匹配特征點數），根據Sim3變換，將每個mvpLoopMapPoints投影到mpCurrentKF上，並根據尺度確定一個搜索區域，根據該MapPoint的描述子與該區域內的特征點進行匹配，如果匹配誤差小於TH_LOW即匹配成功，更新mvpCurrentMatchedPoints。 mvpCurrentMatchedPoints將用於SearchAndFuse中檢測當前幀MapPoints與匹配的MapPoints是否存在沖突。
判斷當前幀與檢測出的所有閉環關鍵幀是否有足夠多的MapPoints匹配（40個），最后清空mvpEnoughConsistentCandidates。
Funtion3:CorrectLoop
* 1. 通過求解的Sim3以及相對姿態關系，調整與當前幀相連的關鍵幀位姿以及這些關鍵幀觀測到的MapPoints的位置（相連關鍵幀---當前幀）
* 2. 將閉環幀以及閉環幀相連的關鍵幀的MapPoints和與當前幀相連的關鍵幀的點進行匹配（相連關鍵幀+當前幀---閉環幀+相連關鍵幀）
* 3. 通過MapPoints的匹配關系更新這些幀之間的連接關系，即更新covisibility graph
* 4. 對Essential Graph（Pose Graph）進行優化，MapPoints的位置則根據優化后的位姿做相對應的調整
* 5. 創建線程進行全局Bundle Adjustment
Funtion3.1:mpLocalMapper->RequestStop()
請求局部地圖停止，防止局部地圖線程中InsertKeyFrame函數插入新的關鍵幀,如果全局優化線程還在運行，那么就終止它。
Funtion3.2:mpCurrentKF->UpdateConnections()
根據共視關系更新當前幀與其它關鍵幀之間的連接
通過位姿傳播，得到Sim3優化后，與當前幀相連的關鍵幀的位姿，以及它們的MapPoints。 當前幀與世界坐標系之間的Sim變換在ComputeSim3函數中已經確定並優化，通過相對位姿關系，可以確定這些相連的關鍵幀與世界坐標系之間的Sim3變換。 取出與當前幀相連的關鍵幀，包括當前關鍵幀,先將mpCurrentKF的Sim3變換存入，固定不動。為了得到閉環g2o優化后各個關鍵幀的位姿，相連幀的位姿是從世界坐標系到相機坐標系的，當前幀的位姿（從世界坐標系到相機坐標系）固定不動，傳播是世界到當前幀在傳播到相連幀。然后不經過優化后的當前幀的話就直接得到世界坐標系到相連坐標系的位姿。
得到調整相連幀位姿后，修正這些關鍵幀的MapPoints。將該未校正的eigP3Dw先從世界坐標系映射到未校正的pKFi相機坐標系（×g2oSiw），然后再反映射到校正后的世界坐標系下（×g2oCorrectedSwi）。
將Sim3轉換為SE3，根據更新的Sim3，更新關鍵幀的位姿。把sim3的尺度信息去掉就是SE3位姿。
根據共視關系更新當前幀與其它關鍵幀之間的連接。這時候的連接已經包括當前幀和閉環幀直接的連接。
檢查當前幀的MapPoints與閉環匹配幀的MapPoints是否存在沖突，對沖突的MapPoints進行替換或填補。如果有重復的MapPoint（當前幀和匹配幀各有一個），則用匹配幀的代替現有的，如果當前幀沒有該MapPoint，則直接添加。
Funtion3.3: SearchAndFuse(CorrectedSim3)
通過將閉環時相連關鍵幀的mvpLoopMapPoints投影到這些關鍵幀中，進行MapPoints檢查與替換。
更新當前關鍵幀之間的共視相連關系，得到因閉環時MapPoints融合而新得到的連接關系。首先，遍歷當前幀相連關鍵幀（一級相連），得到與當前幀相連關鍵幀的相連關鍵幀（二級相連），更新一級相連關鍵幀的連接關系（這個是在修正MapPoint之后得到的連接關系）。取出該幀更新后的連接關系，從連接關系中去除閉環之前的二級連接關系，剩下的連接就是由閉環得到的連接關系，從連接關系中去除閉環之前的一級連接關系，剩下的連接就是由閉環得到的連接關系。
Funtion3.4:OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale)
進行EssentialGraph優化，LoopConnections是形成閉環后新生成的連接關系，不包括步驟7中當前幀與閉環匹配幀在MapPoints融合之后而新得到的連接關系。1.將地圖中所有keyframe的pose作為頂點添加到優化器（盡可能使用經過Sim3調整的位姿）。2.1、LoopConnections是閉環時因為MapPoints調整而出現的新關鍵幀連接關系；2.2、添加跟蹤時形成的邊、閉環匹配成功形成的邊（1.只添加擴展樹的邊（有父關鍵幀）2.添加在CorrectLoop函數中AddLoopEdge函數添加的閉環連接邊）;2.3、最有很好共視關系的關鍵幀也作為邊進行優化。最后一次取得優化后的各個位姿和空間點。
Funtion3.5:mpMatchedKF->AddLoopEdge(mpCurrentKF); mpCurrentKF->AddLoopEdge(mpMatchedKF)
添加當前幀與閉環匹配幀之間的邊,這兩句話應該放在OptimizeEssentialGraph之前，因為OptimizeEssentialGraph的步驟2.2.2(青色部分)中有優化.
Funtion3.6:new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId)
這是整個程序真正意義上在全局地圖的后端優化。優化后要更新所有的地圖點和關鍵幀的位姿。因為地圖管理線程（localmaping）在全局BA優化的過程中仍然在工作，這就有可能導致新的關鍵幀沒有被優化，所以我們需要通過spanning tree來進行調整（就是調整沒有別優化的新進來的點和關鍵幀）。mvpKeyFrameOrigins放的是原始關鍵幀（在初始化的時候插入的關鍵幀），然后往下找其子節點，如果字節點沒有經過BA優化，根據子節點的位姿算出父節點的從世界坐標系到相機坐標系的位姿，如果優化后就直接壓入列表。然后把子節點送入列表，刪除父節點，再取出列表中的剛剛插入的子節點作為新的父節點，這樣反復下去。對於地圖點，如果經過BA優化就保持原來的，如果沒有經過優化就利用生成它的關鍵幀的位姿修正為成世界坐標系里的點。