〇、代碼運作
運作ORBSLAM基本按照教程來就可以，這裡補充一下一個教程的錯誤，教程要求安裝pangolin的版本已經出現了更新，是以會與Eigen庫的版本不比對，主要的表現就是在編譯的時候，報無法找到Eigen庫的錯誤：
這不是因為Eigen庫的版本而導緻的，主要是pangolin庫按照教程使用的git指令，預設會安裝github上最新的版本，也就是0.6版本，而ORBSLAM運作需要用的是0.5版本，是以需要替換為舊版本。
詳細的替換參考連結：https://zhuanlan.zhihu.com/p/494294498
一、初始化部分
對于單目相機，檢視CMakeList可以看到，其對應的可執行檔案是mono_tum.cc，進入到這個檔案可以看出，這個程式相當于是單目相機情況下的總控制，負責讀取資料集、配置檔案等内容，這個檔案最重要的一句代碼就是下面這句：
這句代碼執行個體化了一個System對象，這個對象叫做SLAM，這個System對象實際上可以看作将ORBSLAM三大線程串在一起的樞紐，在這裡初始化這個對象，後續就隻需要按時間順序，将每一幀圖像讀取之後送到這個對象中就可以了，對于單目相機來說，就是調用TrackMonocular這個函數，傳入圖像和時間戳，上下兩句代碼隻是為了列印一幀的執行時間：
在進入TrackMonocular函數之前，我們先看一下System對象的構造函數，這個構造函數寫的巨大無比，簡單點來說就是初始化了整個SLAM過程中用得到的很多個對象，最重要的就是三大線程對象：
除此之外還包括一些SLAM過程中存儲關鍵内容的容器，比如存儲關鍵幀的容器mpKeyFrameDatabase。反正這個函數就當作初始化容器的大函數就好。裡面比較有意思的是函數最後的這一段：
這段函數主要是設定程序之間的指針，因為三大線程相當于是同時運作的，在運作過程中，免不了會遇見彼此之間的協作或者說互相通信，這種情況就需要用指針操作，而指針的指向關系就是在這一步裡面建立的。
回到原來的TrackMonocular函數，進入到這個函數，相當于開始了ORBSLAM的流程，順着這個函數，首先會到達System.cc裡的TrackMonocular函數，在這裡首先是一大堆互斥鎖的操作，這堆操作可以劃分為兩個鎖的内容，第一個鎖看不太懂，應該是在對線程做簡化，第二個鎖應該是在檢查是否進入了重新初始化的狀态，也就是三大政策都跟蹤失敗時進入的重新初始化狀态。
這兩個鎖的内容之後，是這個函數真正關鍵的地方，也就是位姿的估計，這裡首先是調用了mpTracker對象的GrabImageMonocular函數，用來獲得相機位姿的估計結果，之後擷取鎖，儲存目前幀的估計情況：
暫且不展開這裡的函數，我們按照這個函數的流程，将計算的相機位姿傳回，結果回到的應該是mono_tum裡面的main函數，但這裡在調用時并沒有一個資料量來接收這個位姿，這裡寫法上可能有些問題。回到main函數之後，主函數剩下的内容其實就沒什麼可說的了，這說明所謂的三大線程，和主函數的聯系其實就是通過Tracking連接配接的，在剩下的部分中，有點說頭的就是加載下一張圖像的部分，圖像的加載實際上是通過for循環實作的，而為了保證每一幀圖像到來的時間間隔的穩定，ORBSLAM在這裡使用了下面的寫法，讓循環等待一會再進入下一次循環，進而保證時間間隔的穩定。
那麼目前，主函數的基本執行順序就理清了，無外乎初始化一個System對象，然後按照一定的時間間隔向裡面傳送圖像。接下來我們就按照順序開始理順Tracking線程，回到System.cc的TrackMonocular函數中，最開始的一步是調用mpTracker的GrabImageMonocular函數去進行跟蹤，我們就從這裡開始。
GrabImageMonocular函數在Tracking.cc裡面，傳入函數的參數是目前幀的圖像和一個時間戳，進入這個函數後首先是對傳入圖像的處理，也就是将彩色圖像轉換為灰階圖像，之後會将目前幀的内容構造為一個幀，在這裡的代碼實作上是用了一個分支，判斷是不是初始化，無論是否進行初始化，都會構造一個幀對象，差別在于提取特征點數目，初始化初始的幀對象提取的數目要多：
這段代碼中ORB特征提取器其實是一個對象，mpIniORBextractor和mpORBextractorLeft隻有一個差別，就是ORB特征點提取的數目，這兩個對象是在Tracking對象初始化的時候進行的，也就是在System對象的構造函數裡，對應就是下面這段代碼，其中nFeatures就是提取特征點的數目，可以看見在初始化兩個特征提取器對象的時候，第一個參數差了兩倍，這一點對應的剛好就是特征點提取的數目。
在初始化兩個幀對象之後，就會調用Frame的構造函數，而在GrabImageMonocular函數的後面，就隻有一個調用Track進行跟蹤，是以關于幀的特征提取以及初始化等内容，應該都是在Frame的構造函數裡面實作的，是以這裡我們展開構造函數看一下内部實作。
構造Frame的過程中，首先是對一些關鍵内容的指派，之後利用傳入的内容，計算圖像金字塔的一些參數，圖像金字塔在之前看EDLine的時候記錄過，這裡不多介紹了。完成這些小參數的初始化之後，就可以提取ORB特征點，調用同一個類内部的ExtractORB函數，在這個函數中，出現了一個極其反人類的寫法，也就是重載了（），不知道作者為什麼在這裡要用炫技的寫法，反正看着很别扭，總之在這裡，對于單目相機，就通過mpORBextractorLeft對象進行特征提取，提取的細節在ORBextractor.cc裡面，ComputePyramid函數負責建構圖像金字塔，ComputeKeyPointsOctTree函數進行特征點的提取以及均勻化，這裡提取特征點并沒有直接使用OPENCV的ORB提取器，而是用了FAST提取器，關于ORB的實作是自己單獨寫的。提取好特征點之後計算描述子，具體實作在computeDescriptors函數裡，這裡用了一個很暴力的寫法，因為ORB描述子使用了brief描述子，也就是随機選取一定範圍内的像素，将灰階值大小換為01進而建構描述子，是以選取方法需要提前确定，這裡的選取方法寫在了bit_pattern_31_裡面。還有一點是在提取的時候涉及的旋轉，因為ORB在提取時會将方向轉到梯度方向，由于轉像素很麻煩，是以這裡采用的識将選取方法旋轉，也就是旋轉pattern。
回到Frame的構造函數，提取完ORB特征點之後，會利用OpenCV的矯正函數、内參對提取到的特征點進行矯正，矯正之後的三角化并沒有在這裡進行，主要是缺少三角化的第二幀圖像，在這個構造函數中隻定義了存儲本幀地圖點的資料結構，但是并沒有向裡面加入資料。
最後對于已經提取好的特征點，通過劃分網格的方法，将特征點放入了圖像網格中，這裡使用的是AssignFeaturesToGrid函數，這個函數會周遊每個特征點，用PosInGrid函數來确定特征點落入網格的坐标，之後将特征點的id放入對應網格的資料結構中：
到這裡Frame的構造函數執行完畢，在這個構造函數中，首先是完成一些參數的指派，之後建構圖像金字塔，之後對單目圖像提取特征點并計算描述子，對特征點矯正之後，放入劃分的網格。這個時候我們就得到了第一幀圖像的特征點資訊、特征點的描述子以及網格分布情況，由于沒有第二幀，是以還沒有進行三角化恢複3D資訊。
再繼續往回走，回到GrabImageMonocular函數，這時對于已經初始化好的第一幀，調用Track函數，進入到Track函數内部。track包含兩部分：估計運動、跟蹤局部地圖，進入Track之後會先判斷mState，這個量如果圖像複位過、或者第一次運作，則為NO_IMAGE_YET狀态，此時顯然是沒有運作過的狀态，會在下面進入地圖初始化的分支：
這裡直接進入MonocularInitialization函數，進行單目的初始化。此時由于單目初始化器在Tacking對象初始化時指派了一個空指針，是以這裡進入建立單目初始器的分支，在這裡必須保證初始幀的特征點數目多于100，否則會直接傳回，在第一幀特征點數目足夠多的情況下，會先用第一幀建立初始化的兩幀的對象，也就是讓第一幀先假裝第二幀去進行初始化。
之後記錄第一幀的所有關鍵點，利用第一幀去初始化一個Initializer對象，這裡就與前面對上了，進入這個分支是mpInitializer為一個空指針，空指針是在Tracking對象初始化的時候在構造函數裡賦了一個空指針，在這裡我們得到了第一幀圖像，是以可以用這幀圖像去初始化構造器，是以在這裡才會真正賦一個對象給mpInitializer。構造mpInitializer的過程其實沒有很複雜，主要是指派内參矩陣、特征點等内容，這段代碼位于Initializer.cc裡面：
之後第一次執行MonocularInitialization就結束了，因為沒有第二幀圖像，隻能初始化第一幀的特征點，需要等待第二幀來才能三角化，回到Track函數，将資訊傳給繪圖部分之後就相當于結束了第一幀的處理，傳回到TrackMonocular，将第一幀的跟蹤資訊儲存之後，傳回到主函數。
接下來等來第二幀圖像，按照前面的執行順序，會再次來到Track函數裡，這次還是走上次的分支，第一幀隻會讓mState從NO_IMAGES_YET換為NOT_INITIALIZED，隻有初始化徹底完成，也就是第二幀圖像也得到之後才會變為OK，按順序第二幀圖像進入MonocularInitialization裡面，這時mpInitializer已經初始化，進入另一個分支，在這個分支中處理第二幀。對于第二幀，隻有連續兩幀的特征點個數都大于100時，才能繼續進行初始化過程，在這裡如果第二幀特征點少于100個，就删除初始化對象，從新開始初始化的内容：
如果第二幀符合條件，就會初始化一個ORBmatcher對象，這個對象用于進行特征點的比對，需要給的兩個值是最佳的和次佳特征點評分的比值門檻值以及是否檢查特征點的方向，比值門檻值主要是用來剔除誤比對，最優次優的差别足夠大才可以接受最優比對，而點的方向則規定了在檢驗比對結果的時候，是否要依賴旋轉直方圖來篩除誤比對。之後利用這個對象搜尋比對結果：
這裡的最後一個參數，也就是搜尋視窗大小，指的是搜尋鄰域的範圍，由于幀于幀之間的運動偏移不會太大，是以搜尋時可以利用這種臨近搜尋的思路，減小搜尋的時間開銷。比對的結果存放在 mvIniMatches裡，比對對的數目為nmatches，隻有比對對數目多餘100才會繼續初始化，否則按照初始化失敗處理删除後重新開始。
如果比對數量足夠，那麼就可以利用兩幀比對的圖像進行初始化，這裡就對應了論文中兩種模型初始化的部分，通過H模型或F模型進行單目初始化，這部分的代碼實作在Initializer.cc的Initialize函數裡，這個函數裡面主要是通過RANSAC，每次随機取8組比對點放到vAvailableIndices裡面，開兩個線程分别運作FindHomography和FindFundamental，最後計算出兩個矩陣H和F：
之後通過論文提到的打分機制，為計算的結果賦分并選擇一個最優矩陣，利用矩陣還原第一幀和第二幀之間的旋轉和平移，如果計算正确就會傳回true。三角化的部分也是在這裡實作的，三角化的結果存放在了裡面。
在計算正确的情況下，傳回到MonocularInitialization函數中的值就是true，表示八點法位姿變換計算成功，之後首先會利用計算結果，删除一些無法進行三角化的誤差較大的比對點對，并且利用計算結果建立相機坐标系初始情況和世界坐标系的聯系。
在CreateInitialMapMonocular函數中進行了地圖的初始化，首先會将初始化用的兩幀全部作為關鍵幀存放進地圖中，雖然這裡可能會有些備援，但是在關鍵幀删除的部分會優化。之後就會将三角化的點構造為一個地圖點MapPoint并且為地圖點添加一些屬性，最後加入到地圖中，這裡對應的也就是ORBSLAM中地圖點的第一個添加的時機。
全部地圖點建立完成之後，還需要更新關鍵幀之間的連接配接關系，雖然這裡隻有兩個關鍵幀，但是依然需要維護關鍵幀之間的連接配接關系，這裡我們可以看作是在維護共視圖、生成樹等工具，這裡會調用UpdateConnections函數來維護連接配接關系，之後調用GlobalBundleAdjustemnt函數進行全局BA優化，同時優化所有位姿和三維點。
優化完成之後，利用平均觀測深度進行篩選，一個是平均深度要大于0,另外一個是在目前幀中被觀測到的地圖點的數目應該大于100。之後将兩幀之間的變換歸一化到平均深度1的尺度下，并将3D點的尺度也歸一化到1。最後将關鍵幀插入局部地圖，更新歸一化後的位姿、局部地圖點。初始化成功，至此，初始化過程完成。
最後對整個初始化過程做一個總結，在所有幀和計算都滿足條件的情況下，對于傳入的第一幀，提取特征并初始化一個初始化器對象，之後等待第二幀的傳入，幀間特征點比對之後，計算兩個模型H和F矩陣，利用打分機制選擇一個合适的矩陣并恢複幀間變換R和t并三角化恢複特征點，之後将特征點和兩個關鍵幀插入到初始化的地圖中，更新幀之間的連接配接關系後，對尺度做處理，進而完成整個初始化的過程。
二、Tracking線程
對于初始化完成的ORBSLAM，按照執行順序，接下來會在GrabImageMonocular函數中，進入正常運作時的分支，使用正常數目的ORB特征點提取器對目前幀進行特征點的提取，此時特征點的數目是初始化時候的一半。
進入Track函數之後，由于mState在初始化成功後已經變成了OK，是以在這裡也進入正常運作的分支。在正常的跟蹤過程中，首先會調用CheckReplacedInLastFrame函數，這個函數主要是負責檢查并更新上一幀中被替換掉的地圖點。由于跟蹤過程中會用到上一幀的地圖點，而在局部建圖線程中可能會将關鍵幀中的某些地圖點進行替換，是以在這裡需要進行一步更新，通過調用地圖點的GetReplaced函數，根據傳回值檢視是否需要更新。
通過這一步，目前地圖中的地圖點就是準确可靠的，之後按照論文的流程，會根據目前的運動狀态和上一幀的跟蹤資訊，判斷采用三種模型的哪一種：
在選擇三種模型的時候，是通過mState來判斷跟蹤的狀态的，如果這裡不是OK，就認為跟蹤出錯，轉而進行重定位，這種方法是避免重新初始化的最後的救命稻草，而在上一幀正常跟蹤的情況下，會有一個分支來判斷用哪一種方法，對應的就是上圖if語句中的兩個條件，第一個條件是判斷速度模型是不是空，因為速度一旦是空值，恒速模型就無法預測位姿變化，更談不上進行跟蹤，第二個條件貌似在論文裡面沒有提到，就是當目前幀與上一個重定位幀挨得很近的時候，也會用參考關鍵幀的方法進行跟蹤，個人感覺這樣考慮主要是因為目前幀與上一個重定位的幀挨得太近，速度資訊還沒有達到一個很準确的程度，這個情況下貿然使用恒速模型可能導緻估計出錯，是以幹脆就少了這步嘗試，直接用參考關鍵幀跟蹤。
恒速模型 TrackWithMotionModel
下面我們展開看一下這三種跟蹤的代碼實作。首先是最主要的恒速模型估計，對應的函數是TrackWithMotionModel。在這個函數中，首先會初始化一個特征比對對象ORBmatcher，這個對象和初始化時用的是一樣的，而且參數也是一樣的。之後調用UpdateLastFrame函數，對于單目相機而言，這個函數中會利用參考關鍵幀更新上一幀在世界坐标系下的位姿，這裡是利用上一幀的參考關鍵幀，更新上一幀的位姿，之後毫不猶豫直接傳回，而雙目或者rgbd相機則會生成一部分新的臨時地圖點。這部分的代碼實作如下所示：
可以看出，這部分首先取出了上一幀的參考關鍵幀，并且取出了存儲在mlRelativeFramePoses的上一幀的位姿，之後通過坐标變換轉換為世界坐标系下的位姿。這樣可以看出，mlRelativeFramePoses是一個存儲相對于上一個參考關鍵幀的位姿的資料結構，而對于每個幀，在剛剛執行完追蹤等内容後，沒有進行到下一幀之前，其内部儲存的應該是相對于上一個關鍵幀的位姿，而在下一幀到來之後，進行到Track函數的這部分之後，才會将内部的位姿轉換為世界坐标系下的位姿。
完成位姿轉換之後，回到TrackWithMotionModel函數，會根據估計的速度，利用恒速模型給目前幀一個初始化的位姿，實作上就是直接用恒速模型的速度乘以上一幀在世界坐标系下的相機位姿。在清空目前幀的地圖點之後就會開始進行搜尋，搜尋前會根據傳感器類型設定搜尋半徑，單目相機是15像素，雙目相機是7像素。
這裡的搜尋是調用了前面的ORBmatcher對象的SearchByProjection函數，與初始化時候不同，初始化的時候調用的是SearchForInitialization。在這個函數中主要是用上一幀的地圖點進行投影比對，如果比對點數目不足則擴大搜尋半徑再搜尋一次，實作起來也是很直接，用nmatches存儲比對點的數目，少于20就清空比對點然後将門檻值翻倍再調用一遍：
關于SearchByProjection函數内部，傳入的四個參數分别為目前幀、上一幀、搜尋範圍的門檻值以及一個用于标志傳感器是否為單目相機的bool值，傳回值則是成功比對的數目。進入函數之後，首先會建立旋轉直方圖，用于檢測旋轉的一緻性，在這裡代碼将旋轉的360°劃分為了30個區間，用HISTO_LENGTH來表示劃分的區間個數。在後面得到比對點之後，會利用這個角度直方圖做篩選，具體來說就是将比對點的角度變化進行統計，提出明顯旋轉不一緻的比對點對。
對于目前幀，首先計算目前幀和上一幀之間的平移向量，這裡采用的計算方法個人沒太看懂，先是去除目前幀的相機位姿，之後從中拆除旋轉矩陣R和平移向量t，利用這兩個量算出目前幀到世界坐标系的平移向量，再對上一幀的位姿做處理，進而得到兩幀之間的平移向量。主要的疑問點在于，計算平移向量的過程中為什麼要乘以一個旋轉矩陣的轉置，按道理平移表示的是坐标系原點之間的移動關系，那麼從相機到世界坐标系和從世界坐标系到相機，差别就應該隻有一個正負号，這裡乘了一個旋轉，屬實是有些沒法了解。
在計算出平移向量之後，就會周遊前一幀的每一個地圖點，如果地圖點有效（沒被删除且沒有标記為外點），就會利用剛剛計算的投影關系，将這個地圖點投影到目前幀上：
在代碼實作上，這裡的x3Dc就是地圖點在目前幀的相機坐标系下的3D坐标，後面是去除了xy以及逆深度資訊，根據逆深度篩選本身就是根據深度篩選，去除小于零的情況個人感覺應該是防止投影産生的錯誤點對後序造成影響，畢竟不是所有地圖點都可以正确投影過來。得到坐标之後利用相機模型投影到成像平面上，得到一個2D坐标，接下來判斷這個坐标是否落在了正确的成像平面範圍上，也就是檢查坐标值的範圍是否在合法範圍内。
得到投影位置後就要開始搜尋，在搜尋的時候，ORBSLAM采用的是同層搜尋，這裡會記錄上一幀的地圖點對應的二維特征點所在的金字塔層級，搜尋比對點的時候也會在同層進行搜尋，這對應這前面ORBexactor裡面的提取政策，建構金字塔是每層提取，層與層之間是完全獨立的，而不像EDLines那樣會産生層與層之間的比對與聯系。回到搜尋過程，找到所在的層之後，就在所在層劃定半徑進行搜尋，這裡再搜尋時，還考慮了相機的前後前進方向對搜尋尺度造成的影響：
如果可以根據這裡判斷相機的前後移動，那麼相當于在尺度變換上會産生一定的影響，表現在代碼上就是搜尋時金字塔的層數會發生變化：
是以這裡的搜尋政策其實是考慮到了尺度問題的搜尋，一開始ORBSLAM使用高斯金字塔進行特征的提取就是考慮到尺度的問題，如果通過相機移動沒法判斷前後的移動或者說相機的尺度沒法判斷發生了什麼變化的時候，就在臨近的層搜尋，認為是尺度一緻的情況下進行範圍的搜尋，而如果能夠判斷前後的移動，那麼在同層附近搜尋就不合适了，因為這時前後移動會導緻尺度變化，進而較大程度影響特征點出現在金字塔裡面的層數，是以像裡面代碼實作那樣，搜尋的層數會擴大很多。
搜尋的結果會放在vIndices2，之後會周遊這個向量裡面的候選比對點，尋找距離最小的最佳比對點，如果最佳比對點的描述子距離要小于設定的門檻值，就認為這個地圖點跟蹤到了目前幀的特征點，再次經過旋轉直方圖的篩選之後，傳回比對點的數目nmatches。
至此SearchByProjection函數就結束了，在這個函數中，主要任務就是根據投影找臨近點，首先得到目前幀和上一幀之間的位姿變換關系，利用這個變換關系，将上一幀的地圖點投影到目前幀，根據相機的前後移動方向，劃定金字塔的搜尋範圍，之後在對應範圍内搜尋關鍵點并計算描述子距離，如果最小距離小于門檻值，就認為找到了比對點，并對所有比對點進行角度直方圖的篩選，經過篩選後的比對點就是最終結果。
回到Track函數中，得到比對點的數目後，如果比對點太少就會将搜尋半徑擴大一倍然後再執行一次，如果再執行一次還滿足不了足夠的比對點，就認為跟蹤失敗。而跟蹤成功時，則會利用投影關系優化目前幀的位姿，調用Optimizer.cc中的PoseOptimization函數，在這個函數中采用最小化重投影誤差的方法，隻優化位姿，而不優化地圖點。
優化結束後，就利用優化的位姿剔除地圖中的外點，如果是外點，就清楚它的所有關系，如果不是外點，就累加到目前幀的比對地圖點的數目中。最終隻要比對地圖點的數目超過十個，就認為跟蹤成功。
這段代碼是對外點做篩選的具體實作，需要注意的是，如果比對點成功，會在目前幀的地圖點中儲存這個地圖點，表示觀測關系，也就是建立幀到地圖點的連線，而沒有反過來的連線。在優化之後，如果認為是外點，可以看見代碼的寫法是删除了目前幀到地圖點的指向關系，但并沒有删除地圖點，而是對地圖點的兩個屬性進行了修改。
至此TrackWithMotionModel函數就看完了，總結一下這個函數，作為三個跟蹤政策中最重要的，恒速模型的核心在于利用速度估計一個大體位姿，用這個位姿找比對點，如果比對點足夠多，認為這個估計出來的位姿大體準确，反過來再用比對點去優化位姿。在這個函數中，首先更新上一幀位姿，之後利用速度，估計目前幀的初始位姿，之後調用SearchByProjection函數，在幀間進行投影比對，一共兩次機會，如果數量不夠就再擴大範圍搜尋一次，兩次機會還滿足不了數目要求，就直接認為跟蹤失敗，跟蹤成功的情況下利用優化器最小化重投影誤差來優化位姿，之後再反過來剔除外點，剔除之後滿足比對點大于十個，認為真的跟蹤成功。
參考關鍵幀模型 TrackReferenceKeyFrame
跟蹤的第二種政策是用參考關鍵幀模型，作為恒速模型失效時的第一備選，對于單目相機，如果速度資訊為空或者恒速模型失效時，就會轉而使用這個模型進行跟蹤，其對應的函數為TrackReferenceKeyFrame。
進入這個函數後，首先會調用ComputeBoW函數，将目前幀的描述子轉化為詞袋向量，之後初始化一個ORBmatcher對象，用于進行目前幀和參考幀之間的特征點比對，這裡與前面初始化ORBmatcher對象的情況不同，在這裡所使用的比值門檻值要更嚴格，設定為了0.7，意味着最優和次優比對之間差距要足夠大才能認為是真正的比對。之後調用matcher.SearchByBoW函數，利用詞袋模型加速目前幀和參考幀的特征點比對，如果比對數目少于15則認為跟蹤失敗。
關于詞袋模型加速，首先詞袋模型并不是一個加速比對的工具，它是一個用來抽象關鍵幀的工具，是字典裡單詞的組合，簡單來說對于一張圖，如果車、人、樹木是字典裡的單詞，那麼“車出現了三次，人出現了五次，樹木沒有出現”就可以看作對目前這張圖的描述，也就是詞袋模型的結果，詞袋模型的建立個人感覺是一個NLP的任務，在SLAM過程中我們隻是單純拿訓練好的離線字典或者說模型直接使用。用訓練好的模型，對特征點做一個統計分類，就可以得到目前幀的詞袋。但是用詞袋模型加速比對，借助的其實是詞袋模型的存儲結構。具體的實作可能還不是很清楚，但是ORBSLAM建立了特征點到類别的一個反向索引，也就是說可以順着一個特征點，找到其所在的類别，進而找到所有類别的其它點。也就是說，對于一個特征點，我們可以直接找到參考關鍵幀中與其處在詞袋模型同類别的其它特征點，因為類别的存在，是以隻需要對這部分的點進行比對，由此減少了暴力比對的開銷，進而加速了尋找比對特征點的過程。
簡單看一下通過詞袋模型進行比對的函數内部，首先代碼會取出同屬于一個node的ORB特征點，這裡的實作是借助了讀取參考關鍵幀和目前幀的詞袋模型：
如果不考慮分支的内部實作，可以看出，代碼用四個疊代器指向了表示參考關鍵幀的類别和表示目前幀類别的向量的起點和終點，由于類别向量内部是從小到大來排列的，是以要找到同類節點，隻需要讓兩個向量中的node id相等即可。找到同類node之後，就會周遊同屬于一個node的特征點，尋找最優描述子距離和次優描述子距離，如果比值符合條件，就認為比對成功。除了最優次優的篩選方法，ORBSLAM在這裡還引入了兩次的篩選，一個是比對距離必須小于設定的門檻值，另一個是旋轉直方圖的變化必須符合一緻。這裡的兩次篩選是針對同類node的篩選，而在得到全部篩選之後，還會有一次利用角度直方圖的篩選，隻保留角度直方圖中前三多的區間，這裡個人感覺會稍微有點備援，但考慮到恒速模型已經失效，這裡提高一下比對的準确率也确實說得過去。
【補充】
看到回環檢測的時候發現也利用到了SearchByBoW，仔細看了一下，發現這兩個地方使用的搜尋是不一樣的，這裡使用的SearchByBoW比對的對象是一個關鍵幀和一個幀，用關鍵幀上已經有比對地圖點的特征點與目前幀的特征點進行比對，最後建立的是一個目前幀2D特征點與之前舊的地圖點之間的聯系，這樣也正好與後面3D-2D的優化相對應，并非一開始了解的2D-2D的對應關系。
回到TrackReferenceKeyFrame函數，通過SearchByBoW我們快速地得到了新的比對結果，之後判斷比對數目是否符合條件。在符合條件的情況下，就會利用這部分找到的比對點去優化，首先将上一幀的位姿作為目前幀的初始位姿，用這種方法加速收斂，之後通過優化3D-2D的重投影誤差來獲得準确的位姿，利用優化後的位姿剔除比對點中的外點，剔除之後如果比對點依然多與10個則認為采用參考關鍵幀的跟蹤是成功的。
總結一下這個函數，在采用參考關鍵幀進行跟蹤的實作中，其實難度相較于恒速模型已經降低了不少，其實作的思路簡單來說就是将恒速模型中的上一幀換為了參考關鍵幀，優化、剔除外點的實作其實沒有太大的變化。在實作細節上，比較關鍵的就是利用詞袋模型加速比對，這裡由于不能使用恒速模型的臨近搜尋方法，而直接暴力比對開銷又太大，是以引入了這種詞袋模型加速的方法。
重定位模型 Relocalization
如果進入到重定位模型的跟蹤，表示恒速模型以及參考關鍵幀都失效了，在這種情況下就需要更加可靠的方法來獲得位姿，依靠的不能僅僅用臨近的幀了，要擴大依賴的範圍，尋找更加可靠的對象，這裡也就是尋找候選關鍵幀。
在這個函數中，首先會對目前幀計算其詞袋向量，之後用這一幀的詞袋向量，從關鍵幀資料庫中尋找與其相似的候選關鍵幀，主要是通過調用mpKeyFrameDB->DetectRelocalizationCandidates函數，這個函數的傳回值是一個關鍵幀指針的向量，也就是說會傳回一堆與目前幀相似的關鍵幀，用于後續的跟蹤。
進入DetectRelocalizationCandidates函數看一下細節，這個函數主要負責在重定位中找到與該幀相似的候選關鍵幀組，簡單來說就是找到詞袋模型上足夠相似的關鍵幀組。在這個函數中首先會利用存儲的詞袋資訊，找出與目前幀具有公共單詞的所有關鍵幀，如果和目前幀具有公共單詞的關鍵幀數目為0，無法進行重定位，傳回空數組。這裡的代碼實作上應該也用到了前向索引和逆向索引來快速找到符合條件的關鍵幀。
這段代碼首先是讀取了目前幀擁有的所有單詞，根據單詞id，找出之前的關鍵幀中有過同樣單詞的關鍵幀，之後再周遊篩選出來的關鍵幀，通過修改mnRelocQuery的值，将這一幀标記為重定位候選幀，标記并送入lKFsSharingWords函數中。如果mnRelocQuery之前沒有标志，表示是第一次倍識别為重定位候選幀，此時除了标記還需要将mnRelocWords初始化為0，通過這個量來标記某個關鍵幀與目前幀的共享單詞的數量。
現在lKFsSharingWords向量中存放到實際上是大量的關鍵幀，這些關鍵幀都和目前幀存在至少一個共享的單詞id，也就是至少有一個單詞是二者共用的，共享單詞的數量由mnRelocWords來記錄。存在公共單詞的情況下，統計挑選出來的關鍵幀中與目前幀具有共同單詞最多的單詞數，這個單詞用于設定一個門檻值，其值的0.8倍将會作為最小公共單詞數這個門檻值，由此可以确定maxCommonWords和minCommonWords這兩個門檻值。
之後再次周遊篩選出來的重定位候選幀，利用設定好的兩個門檻值，篩選出單詞數大于minCommonWords的關鍵幀，并存儲目前幀與單詞數足夠大的關鍵幀的相似度得分。此時不僅存儲了相似度分數，還利用lScoreAndMatch存儲了比對對。
後面代碼又擴充了關鍵幀組的範圍，考慮到共視關系的存在，單計算目前幀和某一個關鍵幀的相似性是不全面的，這裡利用共視圖，将與關鍵幀共視程度最高的十個關鍵幀也納入了考慮，也就是将一個幀擴充為十個幀，将其歸為一組，計算總得分來作為相似程度。這裡在計算總得分的時候，隻有共視幀也在重定位候選幀中才能作為分數計算的一部分。
這裡個人感覺是在做一個增強的操作，這步操作顯然會增強連續候選幀的分數，即使是考慮了共視關系，能加分的關鍵幀本身也必須存在在重定位候選幀中，是以要求距離不能太遠，這樣做其實是削弱單獨的重定位候選幀的價值，讓連續的重定位候選幀的分數更高。在這個過程中，記錄最高得分值bestAccScore，并将最高值的0.75倍記為minScoreToRetain，這兩個值将作為門檻值再進行後續的篩選，隻有大于minScoreToRetain的關鍵幀才能作為最終被傳回的候選關鍵幀。
也就是說，在DetectRelocalizationCandidates函數中，通過詞袋模型做了三次的篩選，第一次根據共享單詞篩選，沒有共享單詞的關鍵幀被篩除，第二次根據共享單詞數目進行篩選，數目少于最大值0.8倍的被篩除，第三次則根據考慮了共視關系的相似度得分來篩選，得分小于最高得分0.75倍的被篩除。
在得到了候選關鍵幀之後，就會初始化一個用于特征比對的ORBmatcher和一個用于求解PnP問題的vpPnPsolvers，周遊所有的候選關鍵幀，通過調用SearchByBoW函數，對候選關鍵幀和目前幀做一個快速比對，比對的特征點足夠多就是用EPnP來估計目前幀的位姿，這裡實際上是計算了多次的EPnP，每一個候選關鍵幀都會計算一次。展現在代碼上實際上是分了兩次循環，第一次循環做比對然後在比對數目足夠多的情況下，初始化PnPsolver對象并單獨存儲一個PnPsolver對象給對應的候選關鍵幀，第二次循環則是利用比對點和前面存儲的PnPsolver對象進行疊代求解。
疊代求解之前會再初始化一個比值門檻值為0.9的ORBmatcher，這裡相當于下調了最優比對和次優比對的差異接受度，再次周遊候選關鍵幀，調用PnPsolver對象，疊代五次計算EPnP問題得到一個位姿，通過位姿對内點進行BA優化，優化之後如果内點數目小于10，就直接跳過，如果内點數目大于10但是小于50，就通過投影的方式将關鍵幀中未比對的地圖點投影到目前幀中, 生成新的比對，具體來說就是調用恒速模型裡使用過的SearchByProjection，用求解EPnP得到的位姿作為輸入來尋找比對。如果通過投影過程新增了比較多的比對特征點對使得總體比對大于50，就根據比對結果再次優化位姿，優化後縮小搜尋視窗再次SearchByProjection并進行優化。也就是說如果當内點數目在10到50之間，就用兩次篩選進行内點的補充，采用如此複雜的方法，主要是為了用更加靠譜的方法去補充内點，如果内點補充時不小心謹慎一點，很容易産生誤閉環。
對于内點數目大于50的情況，不管是一開始就大于50還是說通過内點補充之後大于了50，隻要有一個就認為候選關鍵幀重定位成功，其餘的就不再考慮直接跳出。此時就将重定位的結果作為最終結果，并記錄成功重定位幀的id，防止短時間多次重定位。
總結一下這個函數，對于重定位的跟蹤，它其實是三種跟蹤政策裡面的最後的救命稻草，是以在使用這種政策的時候，使用了很多小心謹慎的政策，首先是在搜尋候選關鍵幀組的時候，用了三種篩選來優化候選關鍵幀組，之後通過對候選關鍵幀組求解EPnP得到位姿，對于得到的位姿，根據内點數目來判斷，對于内點數目不多不少的情況，還采用了嚴格的内點補充方法，最後根據最終的内點數目來判斷位姿的準确與否。
局部地圖跟蹤 TrackLocalMap
根據Track函數的正常執行順序，在正常跟蹤成功的情況下，也就是利用三種跟蹤政策成功得到了目前幀的位姿之後，會将最新的關鍵幀作為目前幀的參考關鍵幀，之後就進入到論文的流程圖裡面的下一步，即TrackLocalMap的子產品，這部分在看論文的時候就感覺很難了解，因為它太容易和後面局部建圖那裡混淆了。
局部地圖跟蹤對應的就是TrackLocalMap函數，進入這個函數之後，首先調用UpdateLocalMap更新局部關鍵幀和局部地圖點，局部關鍵幀和局部地圖點包括兩層共視關系包含進來的關鍵幀以及這些關鍵幀觀測到的地圖點，更新的過程我們放在Track線程的最後再看。更新完局部關鍵幀和局部地圖點之後，就調用SearchLocalPoints函數，篩選局部地圖中新增的在視野範圍内的地圖點，将這些地圖點投影到目前幀并搜尋比對關系，以此擴大優化的依據。
個人感覺了解局部地圖跟蹤的關鍵就在SearchLocalPoints裡面，這個函數中，首先會周遊目前幀的所有地圖點，而這些地圖點是之前存放在mCurrentFrame.mvpMapPoints裡面的，這部分地圖點來自于三種模型跟蹤的結果，拿恒速模型來看，進入恒速模型的函數之後，首先有一步清空地圖點，那裡就是在将目前幀的mvpMapPoints清空，之後相這個資料機構中添加的時機，是在調用SearchByProjection的時候，将上一幀的地圖點利用估計的位姿投影到目前幀之後，如果搜尋到了比對的特征點，就建立這個特征點和地圖點之間的聯系，我們可以認為是比對的特征點對應的地圖點。
而在函數執行之前，調用UpdateLocalMap補充了地圖點，也就是說在執行SearchLocalPoints的時候，函數一開始知道的地圖點實際上有兩部分：目前幀跟蹤得到的以及根據共視關系補充進來的。在這裡代碼首先周遊了目前幀全部的地圖點，将他們在地圖點标記出來，因為他們本身就是投影得到的，不需要再參與投影搜尋比對。
之後周遊全部的地圖點，如果有目前幀标記就不再檢驗，如果沒有，則判斷地圖點是不是在視野範圍内，也就是調用isInFrustum來判斷投影點的情況，如果地圖點的視野範圍出現問題，也視為地圖點不符合條件。
如果檢驗到符合條件的地圖點，就進行投影比對，也就是調用SearchByProjection，根據投影關系搜尋臨近比對點，進而增加比對關系。總的來說SearchLocalPoints函數就是在篩選擴充以後的地圖點是否符合要求，這部分補充的地圖點是根據共視關系得到的，是暴力地将共視幀的地圖點湊在了一起，是以一些地圖點實際上在目前幀是看不見的或者說不符合條件的，這個函數會根據投影關系去篩選，如果地圖點的投影符合幾何關系而且能夠在鄰近區域産生比對，就認為這個地圖點是可視的，進而真正認為這個地圖點是可以補充進來的。
回到TrackLocalMap函數，在補充了地圖點之後，我們就可以利用新增加的比對關系，進行一次BA優化進而獲得更加準确的位姿。完成後做一步更新，更新地圖點的觀測程度以及比對數目。
函數的最後會根據跟蹤比對數目及重定位情況決定是否跟蹤成功，如果最近剛剛發生了重定位,那麼至少成功比對50個點才認為是成功跟蹤，如果是正常的狀态話隻要跟蹤的地圖點大于30個就認為成功了。
這裡再稍微理順一下，我們假設新的一幀到來，沒有出現因為門檻值出現問題的情況，而且可以根據恒速模型順利跟蹤。這種情況下，我們看一下這一幀的地圖點是怎麼産生出來的，也就是向量mvpMapPoints的變化情況。
首先進入到恒速模型的函數TrackWithMotionModel中，在投影比對之前會清空目前幀的地圖點，由于地圖點是一個指針向量，是以清空的方法就是讓指針全部指向空，實作代碼為：
清空之後，目前幀一個地圖點也沒有，而插入地圖點的位置是在SearchByProjection中，進入這個函數後，有一步是周遊上一幀的所有地圖點，投影到目前幀并在臨近範圍内搜尋，這裡我們假設在臨近範圍内找到了符合要求的最佳比對點，看搜尋最佳比對點的過程可以發現，mvpMapPoints同時也起着标記的作用，如果對應位置上的指針指向的是空，表明特征點和地圖點之間的聯系還沒有建立。
在篩選得到最優比對點之後，就會建立地圖點與特征點之間的聯系，也就是修改mvpMapPoints的元素值。
在看這段代碼的時候，突發了一個疑問，mvpMapPoints是一個向量，而在這段代碼裡，對于第一個最優比對點，其插入時整個mvpMapPoints是空空如也的，那麼這個時候直接通過一個非0的下标bestIdx2通路，按道理是通路不到的。這裡涉及的是vector的初始化的寫法，在之前打競賽的時候，很少會注意到向量大小的初始化，因為向量大小什麼都很靈活，但也有一種是初始化向量的時候就将大小定下的寫法，這裡我也寫了一段極其簡單的代碼來對比，首先固定大小初始化的時候，我們初始化了5個元素的向量，隻給第五個元素指派，那麼在這種情況下，依然可以保證剩下的元素保持為初始值0而隻修改最後一個元素。
而在不初始化大小的情況下，采用同樣的指派方法是不行的。
可以看見，這種情況下，不僅指派失敗，而且程式運作的傳回值也是一個一看就有問題的值。這裡個人猜測是和向量内部實作有關，忘記在哪裡看見過，向量的實作實際上有一個數組複制的過程，向量初始化的時候，就算不指定也會劃定一個很小的長度，如果加入的資料超過了這個長度，就會重新劃分一個更長一點的記憶體空間，并把這堆資料複制過去，在這裡我們指定了長度，是以可以直接給長度範圍内的元素指派并讀取。而在ORBSLAM的源代碼裡面，使用的都是這種指定長度的初始化寫法，比如地圖點和外點的初始化寫法：
是以投影搜尋那裡，就算是不連續的下标，也可以正常進行指派和讀取。那麼回到最開始的問題，地圖點在投影搜尋之前清空，根據投影搜尋符合條件的被重新加入到地圖點集合中，後續經過BA優化之後，會重新剔除一遍外點，但是地圖點的加入，在跟蹤的過程中，隻有投影搜尋的時候會加入。而投影是根據上一幀的地圖點進行的投影，也就是說正常情況下，地圖點确實是在一點一點減少的，因為總有一部分地圖點會因為相機向前移動而投影不到或者找不到對應的比對點。正因為地圖點會減少，是以才在跟蹤局部地圖的子產品又增加了地圖點的補充，試圖通過參考關鍵幀，将一些跟蹤過程遺失的地圖點補充進來，保證下一幀跟蹤的時候有足夠的地圖點來進行投影。
對這個函數做一個總結，TrackLocalMap個人現在感覺像是對目前幀地圖點的一個補充，可以這樣想，相機是向前移動的，而地圖點是固定在現實環境中不變的，如果我們什麼都不做，按照三大模型的跟蹤政策，地圖點實際上是越來越少的，因為總有一些舊的地圖點會因為投影過去投影到了邊界而被抛棄掉，采用TrackLocalMap會利用臨近的一些關鍵幀，将它們所擁有的一些三角化出來的地圖點根據共視關系補充進來，進而一定程度上延緩地圖點的減少。從實作的角度來看，TrackLocalMap函數先利用共視關系擴充目前幀的地圖點，之後做篩選，對符合條件的地圖點，再次使用一次BA優化校正位姿，并且更新觀測資訊最後判斷是否跟蹤成功。是以說TrackLocalMap的作用就是兩點：擴充地圖點以及進一步優化位姿。
關鍵幀判斷與生成
在Track函數中，在跟蹤成功并利用局部地圖跟蹤校正之後，就進入到關鍵幀的判斷的部分，在跟蹤狀态正常的情況下，首先會更新恒速模型，如果更新不成功，則會将速度設定為空，那麼在下一幀将會無法使用恒速模型進行跟蹤。
之後會周遊目前幀的所有地圖點，清除觀測不到的地圖點，這裡個人的感覺是在清除TrackLocalMap裡面增加的地圖點但是本身上是觀測不到的，但在增加地圖點的時候按道理沒有被觀測的地圖點應該都被去掉了，這裡增加這一步有些不清楚是為什麼。也可能這一步是隻針對雙目相機和RGBD相機的，後面還删除了臨時地圖點，而臨時地圖點隻用在雙目和RGBD相機裡面，用來增強跟蹤的效果。
在執行了一系列的更新和替換操作之後，就會調用NeedNewKeyFrame函數來判斷目前幀是否可以作為一個新的關鍵幀，如果符合條件，将會調用CreateNewKeyFrame函數建立一個新的關鍵幀。在經過這一步之後，Track函數其實就不剩多少了，後面如果mState也就是跟蹤狀态正常，就儲存目前幀資訊。如果跟蹤失敗，就執行Reset函數重新進行初始化。關于這部分的代碼就不看了，這裡主要看一下關鍵幀相關的函數。
關鍵幀的判斷 NeedNewKeyFrame
首先是判斷目前幀是否可以作為一個關鍵幀的函數NeedNewKeyFrame，一般來說關鍵幀的判斷都是依據移動的幅度，如果移動了較大一段的距離，就認為可以将目前幀作為新的關鍵幀了。而在ORBSLAM裡面，有好多判斷的标準，首先會檢測局部地圖線程是否在運作，如果局部地圖線程被閉環檢測使用，則不插入關鍵幀，這裡應該是一種互斥鎖，雖然沒有使用正常寫法的信号量，但是其作用依然是保證局部地圖一次隻被一個線程使用。其次是判斷距離上一次重定位的距離，如果離上一次重定位太近或者目前關鍵幀的數目已經超過了最大門檻值，就不插入新的關鍵幀。最後還要查詢局部地圖線程是否繁忙，目前能否接受新的關鍵幀。這三步都通過，才會進入決策是否需要插入關鍵幀的部分，決策的标準有四個，對于單目相機，決策标準有三個：
其中涉及到的門檻值為：
之後根據這四個條件判斷目前幀是否可以作為一個新的關鍵幀，但是，如果僅僅判斷為一個新的關鍵幀是不足以證明可以插入的，這裡還有一個參量bLocalMappingIdle，是用來local mapping線程是否空閑，隻有該線程空閑時才可以直接插入，如果線程不是空閑，就需要看情況來判斷是否可以插入，代碼這裡實作得很暴力，如果是非單目相機，就判斷關鍵幀隊列的長度，長度不長就可以插入，但是對于單目相機，直接就不能插入。
關鍵幀的建立 CreateNewKeyFrame
建立關鍵幀的函數，首先會利用目前幀作為參數，建立一個關鍵幀，之後将目前最新的關鍵幀更新為參考關鍵幀，這裡也和前面參考關鍵幀的跟蹤那裡對上了，我們在了解時直接将參考關鍵幀看作是最新的一個關鍵幀是可行的，本身在代碼實作上就是這麼設定的，雖然在後面有可能會修改參考關鍵幀的指向，但是這裡并不影響我們這樣去了解。
對于單目相機來說，完成這一步之後就将建立好的關鍵幀插入到關鍵幀清單中，更新相關資訊就算完成了。
Tracking線程總結
跟蹤線程這樣就算是基本看完了，這個線程處理的對象是每一幀，不僅包含了初始化的内容，含包含了ORBSLAM裡面最重要的部分之一：跟蹤。三種跟蹤政策用于保證跟蹤的實作，恒速模型利用速度預測一個位姿，利用位姿做投影搜尋，根據比對結果反過來再優化位姿，能夠實作時間開銷與準确性的雙赢；參考關鍵幀跟蹤依賴于目前幀的參考關鍵幀，将地圖點的來源由上一幀換為參考關鍵幀，采用的方法不再是投影搜尋了，而是用詞袋模型加速過的暴力比對，雖然是暴力比對，單一由于詞袋模型的劃分功能，比對的範圍已經小了很多；重定位作為最後的救命稻草，将特征來源換為了資料庫中和目前幀比對程度足夠大的候選關鍵幀組，篩選過後利用從關鍵幀組裡挑選出來的特征點進行詞袋加速比對，在更加嚴格的門檻值下得到目前幀的位姿。跟蹤成功之後考慮到地圖點的不斷減少，是以增加了一個局部地圖跟蹤子產品來增加比對關系，将參考關鍵幀的地圖點投影到目前幀并嘗試增加地圖點。一切結束後，檢測目前幀是否可以作為新的關鍵幀，在符合關鍵幀要求且其餘線程允許的情況下，就可以作為新的關鍵幀加入到關鍵幀隊列中。最後經過跟蹤線程的處理，新的關鍵幀作為後續的基本機關參與到ORBSLAM的後續建圖與回環檢測中，同時保留地圖點的資訊友善後續的建圖與投影，但是幀的資訊就不再繼續保留。
三、LocalMapping線程
按照論文裡面的流程圖，從跟蹤線程出來的關鍵幀就進到了局部建圖線程裡面，在這個線程裡面，處理的基本機關就不再是幀了，而是建圖所更需要的關鍵幀。按照ORBSLAM的代碼風格，負責這個線程的實作的必然也是個對象，這個對象的初始化是在System對象初始化的時候，初始化的時候傳入了一個新的Map對象以及表示傳感器類型的參數。
Map對象的結構其實沒有想象的那麼複雜，本質上隻有兩部分：關鍵幀和地圖點，分别用向量mspKeyFrames和mspMapPoints存儲，并用mnMaxKFid存儲最大關鍵幀的id，而在這個構造函數裡面，幾乎也沒有操作，就是将mnMaxKFid設定為0，畢竟兩個向量和一些其它參量都是作為成員變量，地圖為空的時候，确實也不需要什麼額外的操作。
在局部建圖線程的構造函數裡面也沒有複雜的内容，全是一些參數的初始化，這裡不再贅述。從線程的建立的寫法來看，在單獨開辟線程的時候，實際上是調用了mpLocalMapper的Run函數，是以我們局部建圖線程的入口就是LocalMapping.cc裡面的Run函數。
在正式開始Run函數之前，需要搞明白線程之間的通信是靠什麼實作的，當初看PLSLAM源代碼的時候，它内部的線程通信使用的是信号量，對應的就是大學作業系統裡面必考的線程通信，采用線程之間的互斥其實是很有必要的，舉個例子，回環檢測線程和局部建圖線程，假設某一個關鍵幀的時候出現了回環，回環檢測線程的優化運算到一半，但是局部建圖線程剛好因為沒有滿足三幀連續觀測的要求，删除了一部分的地圖點，而這部分在沒被删除之前剛好用于了回環檢測的優化，這時候就會出現問題，局部建圖線程删除了但是回環檢測線程還在使用，是以在這裡引入線程互斥的機制。互斥機制有好幾種實作方法，之前PLSLAM的代碼使用一個信号量的寫法，簡單來說就是一個int型或者bool型的變量，某個互斥量被使用時就調整為真，不使用的時候就調整為假，如果想要使用就先檢驗這個量，能用就用不能用就等待。在ORBSLAM裡面，互斥機制的實作是用的隊列，簡單來說就是利用清單list内元素的數量，跟蹤線程将每次建立的新關鍵幀放入存放關鍵幀的list裡面，局部建圖線程利用死循環去監督這個隊列，一旦隊列裡有了元素，就取一個出來進行處理，如果沒有元素就一直等待。是以ORBSLAM的一些線程互斥的實作并沒有使用單獨的信号量，而是結合隊列直接做了互斥，當然，這些隊列本身也是涉及互斥的，屬于是生産者消費者問題，隊列的互斥是需要信号量的。
局部建圖線程的主函數就是Run()函數，這個函數内部最主要的就是一個死循環while(1)，一旦跳出循環意味着線程已經結束，隻需要調用結束函數，将一些資料結構劃分的記憶體清空即可。
下面展開看這個死循環内部，首先調用SetAcceptKeyFrames函數，将mbAcceptKeyFrames調整為false，相當于調整了一個信号量，告訴其它線程局部建圖線程已經開始了一個關鍵幀的處理，暫時不接受請求。這個信号量主要是和跟蹤線程裡面保證互斥，在Tracking線程裡面在判斷是否可以作為一個新的關鍵幀的時候就查詢過這個信号量，如果這個信号量設定為false就認為不可以作為關鍵幀，換句話說如果局部建圖線程在工作，就不會産生關鍵幀，關鍵幀的産生隻能是在局部建圖線程不工作的時候才可以。
那麼在循環一開始調用SetAcceptKeyFrames，其實就是告訴其它線程，局部建圖開始工作了，讓其它線程不要打擾自己。但問題在于，這種寫法個人感覺有點問題，暫且不看局部建圖内部的處理，我們可以明确的是，進入局部建圖時，必須要有關鍵幀，一旦有關鍵幀，就會開始處理，而處理完成之前是不會調用SetAcceptKeyFrames來将mbAcceptKeyFrames改為true的，也就是說一旦局部建圖開始處理關鍵幀，那麼就不會有新的關鍵幀在這個過程中插入進來，那對于關鍵幀隊列來說，是不是可以認為常年就隻有一個關鍵幀在内部，按照線程之間的輪詢機制，一旦有了關鍵幀插入進來，那麼局部建圖就開始執行了，處理完這個之前不會有新的進來，那樣是不是不會有第二個關鍵幀進入隊列，這裡個人感覺可能有點問題。
【思路更正】
這裡的個人了解的互斥邏輯有點錯誤，修改mbAcceptKeyFrames并不會完全停止Tracking線程産生新的關鍵幀，在判斷是否能産生時，mbAcceptKeyFrames隻會作為其中一個條件，而不是唯一條件，除此之外的條件還包括距離上次插入關鍵幀的時間、插入關鍵幀的時間間隔、跟蹤點的數目（僅雙目與RGBD情況）以及與參考幀相比的跟蹤點數目。這些條件會利用一個邏輯判斷來決定是否能夠插入新的關鍵幀。是以對于單目相機，即使局部建圖線程處理不完，如果長時間沒有新的關鍵幀插入，也會産生新的關鍵幀送入隊列，也就是說，理想情況隊列确實是隻有一個關鍵幀，而如果出現問題，局部建圖線程的處理速度大幅度下降，這種情況隊列一開始會因為mbAcceptKeyFrames而不讓插入隊列，而等一段時間之後，滿足長時間沒有新的關鍵幀插入的條件之後，就會允許新的關鍵幀插入隊列。
抛開這個問題暫且不談，在完成互斥量的更改之後，會調用CheckNewKeyFrames函數判斷清單中是否有等待被插入的關鍵幀，也就是Tracking線程新産生的關鍵幀，如果有就會進入到處理的分支裡面，在這個分支中首先調用ProcessNewKeyFrame函數，處理清單中的關鍵幀，包括計算BoW、更新觀測、描述子、共視圖，插入到地圖等。之後調用MapPointCulling剔除觀測情況不好的地圖點，完成後執行對于單目相機至關重要的一步，調用CreateNewMapPoints函數通過三角化建立新的地圖點。下面分别展開看一下這三個函數。
處理關鍵幀 ProcessNewKeyFrame
ProcessNewKeyFrame函數的功能和它的名字一樣，就是處理新的關鍵幀，進入這個函數之後，首先會從新關鍵幀清單裡取出最前面的關鍵幀，之後計算關鍵幀的詞袋模型，完成後會調用GetMapPointMatches函數，擷取目前關鍵幀的所有地圖點，需要注意，這裡的地圖點本身就是一個索引，是一個和舊地圖點的聯系，自身由于沒有進行過三角化，是以本身是沒有新建立的地圖點，存儲的實際上是與舊地圖點之間的聯系。
之後周遊所有舊的地圖點，也就是GetMapPointMatches的傳回結果，對于所有符合條件的地圖點，執行下面的判斷分支：
這個分支的判斷标準是IsInKeyFrame函數，這個函數負責檢查該地圖點是否在關鍵幀中有對應的二維特征點，可以看見，這個函數實際上是一個MapPoint對象的成員函數，它的作用是檢驗目前的地圖點在傳入的幀中是否有對應的二維點。說實話這段代碼實在是很難看懂，這裡記錄一下思路，感覺可能也不太正确。看懂這段代碼，關鍵是要理順清楚這裡是在對誰做判斷，判斷的對象是pMP，它的來源是vpMapPointMatches，也就是mpCurrentKeyFrame->GetMapPointMatches()，這個函數傳回的是mvpMapPoints這個向量，而在看Tracking部分的代碼時我們遇見過這個資料結構，它相當于一個索引，一端是目前幀的2D關鍵點，另一端是地圖點，表示目前幀的2D特征點是否對應一個之前的地圖點，可以确定的是，我們使用投影比對得到的對應關系，都會儲存在這裡面，那麼這裡取了一個pMP，就是取了一個地圖點，檢查地圖點和目前這一幀之間是否添加了觀測關系。這裡其實是ORBSLAM挖的一個坑，地圖點到目前幀以及目前幀到地圖點在ORBSLAM裡面是分開的兩個索引，前面Tracking的部分的索引隻是目前幀到地圖點的索引，而到這一步才是補充地圖點到目前關鍵幀的索引。
對于單目相機來說，其實在運作起來之後是不可能進入到下面這個分支的，因為單目相機的地圖點添加，隻有在初始化時或者三角化完成後才能進行，這個分支對應的是雙目相機或者RGBD相機建立臨時地圖點的部分。而上面的分支就是取出目前關鍵幀的一個地圖點，為地圖點增加觀測資訊。作為分支判斷的标準是“!pMP->IsInKeyFrame(mpCurrentKeyFrame)”，如果進入上面分支，那麼條件必須為真，去掉！相當于條件剩下部分必須為假，進入函數内部看，傳回值是mObservations對象的count，而對于一個新的關鍵幀而言，這裡的傳回值顯然是0，是以上面的推理是成立的。
是以這個地方對于單目相機而言，就直接了解為更新地圖點到關鍵幀的索引即可。完成這一步之後，會利用完善好的觀測資訊，更新共視圖，之後将該關鍵幀插入到地圖中。
對于ProcessNewKeyFrame這個函數來說，最大的作用就是完善觀測關系，我們這裡隻讨論單目相機，這個函數首先會取出一個新的關鍵幀，計算詞袋向量後周遊地圖點，為目前關鍵幀的地圖點增加觀測資訊，之後利用增加的觀測資訊去更新共視圖，最後插入到地圖中。
地圖點剔除 MapPointCulling
新的關鍵幀插入到地圖中後，進入到MapPointCulling函數中根據觀測情況剔除品質不好的地圖點。這個函數的實作并不複雜，主要的對象就是上個函數裡面的mlpRecentAddedMapPoints清單，這個清單存儲的是新添加的地圖點，需要明确一點，對于單目相機，地圖點的建立一定是在三角化的過程中實作的，而對于ORBSLAM裡面，三角化的實作是放在CreateNewMapPoints函數裡，也就是MapPointCulling的後面，是以對于單目相機，在這個函數中，處理的實際上是上一次執行CreateNewMapPoints添加的地圖點，而對于深度相機和RGBD相機來說，這一部分處理的是新添加的地圖點和局部地圖跟蹤子產品裡面增加的地圖點，這裡的代碼沒有區分傳感器類型，是以有些地方看起來有些混亂。
反正無論哪種傳感器，都會經過這個函數來剔除不合适的地圖點，是以這裡也就不再區分了。傳感器類型在這裡影響的是觀測門檻值，也就是最近多少幀能夠觀測到這個地圖點的門檻值，單目相機要求在建立的3幀内觀測數目不小于2，雙目和RGBD則要求不小于3，也就是說如果一個地圖點經過三幀還沒有被删除，那就會被認為是一個好的地圖點。
确定好門檻值之後，就會周遊檢查新添加的地圖點，對于每個地圖點，首先通過pMP->isBad()檢測地圖點是不是壞點，如果是，那麼壞點将會在這一步裡面統一删除。這裡補充一下ORBSLAM的代碼裡面一個很嚴謹的地方，地圖點的删除隻有MapPointCulling這個函數裡面會進行，在其它時刻認為要删除的地圖點，會通過SetBadFlag來暫時标記為壞點，一旦标記為壞點就不會納入優化或者投影之類的操作，但并沒有被删除，等再次執行到MapPointCulling的時候才會統一删除。個人感覺這裡也是考慮到了地圖點的互斥問題，如果一個線程在優化，另一個線程認為要删除，那麼一旦删除之後優化的結果就沒有接收的對象了，就會出現位址問題。應該是考慮到這個問題，才使用了标記壞點的方法。
對于暫時還不是壞點的地圖點，則會通過一些列的檢驗去判斷是不是合格的地圖點。第一個檢驗會檢查跟蹤到該地圖點的幀數相比預計可觀測到該地圖點的幀數的比例，如果小于25%就從地圖中删除。這裡計算的比例，用代碼裡面的變量名來表示就是mnFound/mnVisible，其中mnFound表示地圖點被多少幀（包括普通幀）看到，而mnVisible表示地圖點應該被看到的次數。對于mnFound這個量，能夠修改這個量的函數隻有IncreaseFound，采用函數來修改protected量這種寫法是很符合C++的要求的，全局搜尋調用過這個函數的位置，如下圖所示：
第二個和第三個分别對應IncreaseFound函數的聲明和執行個體化，這裡就不做考慮。剩下的三個裡面，第一個是在Replace裡面調用，也就是在融合地圖點的時候，簡單來說就是一個新發現的地圖點實際上和之前的某個地圖點是一個點，這種情況需要将兩個地圖點融合，将目前地圖點的觀測資料等其他資料都"疊加"到新的地圖點上。而在Tracking.cc裡面調用的兩個位置，一個是在局部地圖跟蹤的時候，一個地圖點如果通過局部地圖跟蹤與目前幀建立了聯系，那麼就會補充一個觀測關系；另一個是在跟蹤tracking，局部地圖不工作的情況，這種情況相當于SLAM隻工作一部分，我們就不看了。也就是說正常情況下，能修改mnFound這個量的隻有兩個情況：局部地圖跟蹤和地圖點融合。
同理我們再檢視mnVisible這個量的情況，它在IncreaseVisible函數中被修改，也是在五個地方被修改，差別的是IncreaseVisible的調用要比IncreaseFound寬松一些，除了在地圖點融合部分和定義聲明部分出現過，還有兩次出現是在SearchLocalPoints裡面，分别給目前幀的地圖點增加觀測關系和給投影過來沒有超出範圍的地圖點增加觀測關系。個人感覺這時因為mnVisible表示的是會被觀測到的幀的數目，不管有沒有産生比對，隻要按照位姿産生的投影沒有超出範圍，那就算會被觀測到，隻不過沒有找到合适的比對關系，那是2D特征提取的問題，與觀測無關。
那麼回到MapPointCulling那裡，在地圖點不是壞點的情況下，調用GetFoundRatio去檢驗這兩個量的比值，其實就是在檢測地圖點的被觀測情況，也就是說如果地圖點建立了但是好多幀按說能觀測到但是實際上沒有觀測，這種點就很難讓人相信是一個好的地圖點，是以我們在這裡就認定地圖點是壞點。還有一個問題，這裡檢驗的地圖點，都是mlpRecentAddedMapPoints裡面的，也就是建立好沒超過3個關鍵幀的地圖點，一定要搞清楚，mnVisible和mnFound是針對幀的，如果是針對關鍵幀，那這裡兩個值最大才是3，必然不對。
第二個檢驗和第三個檢驗個人感覺根本就是一個，這裡檢驗了目前幀到地圖點第一次出現的幀的距離，這種寫法應該是考慮了傳感器不同門檻值差異不同，是以用了這個寫法，順便提一嘴，這裡的代碼和前面應該不是一個人寫的，前面Tracking部分經常會根據傳感器參數來選擇不同的分支，而這裡幾乎不用這種寫法，而是用了一些可讀性稍微差但是一體性很好的寫法。
是以對于MapPointCulling這個函數，它最主要的任務，就是剔除新加入的地圖點中不靠譜的那部分，剔除的标準有兩個，一個是觀測情況，另一個是距離最早發現的幀數，如果不符合條件，就會被标記為壞點，在下次執行到這裡的時候被删除掉。
地圖點建立 CreateNewMapPoints
剔除品質不好的關鍵幀之後，就會調用CreateNewMapPoints函數，利用目前關鍵幀與相鄰關鍵幀之間的比對點關系，三角化産生新的地圖點。進入函數之後，首先會根據傳感器類型，确定搜尋最佳共視關鍵幀的數目，單目為20個，雙目和RGBD相機為10個，這裡就需要稍微拓寬一下思路，地圖點的三角化，并不一定必須是兩幀之間，準确地說是一個地圖點的三角化必須要兩幀，而對于好多的地圖點，可以利用多幀來各自三角化各自的。這裡的這個量就是這個用處，用于找出共視關系最好的一部分臨近關鍵幀。
得到臨近關鍵幀的門檻值之後，就調用GetBestCovisibilityKeyFrames函數來查詢共視圖找到共視程度最高的一部分關鍵幀。
這裡需要注意的是，我們調用的這個GetBestCovisibilityKeyFrames函數，實際上是關鍵幀對象的成原函數，而它傳回的查詢對象，也就是mvpOrderedConnectedKeyFrames，也是關鍵幀的一個成員變量，仔細檢視一下關鍵幀的頭檔案KeyFrame.h可以看到這些量的定義，在這堆定義裡面，也包含了共視圖的定義，代碼如下：
也就是說，我們了解的共視圖是一個很大的圖，但實際上在代碼裡面，共視圖是一個局部的結構，每一幀維護各自的共視圖，而并不像衆多的講解裡面提到的那樣是一個很大很完整的結構。
回到建立地圖點的函數，得到了共視關系很好的關鍵幀之後，就會初始化一個特征點比對對象ORBmatcher，不同的是這次初始化的權值要求更加嚴格，之前初始化和跟蹤時都利用過這個對象，但是它們的權值都沒有這裡的嚴格。之後取出目前幀從世界坐标系到相機坐标系的變換矩陣，得到目前關鍵幀光心在世界坐标系中的坐标、内參，對于雙目相機則是得到左目光心在世界坐标系中的坐标、内參。
接下來就開始處理每一個關鍵幀，周遊前面篩選出來的共視關系好的關鍵幀，這裡為了友善叙述我們用相鄰幀來統一稱呼。首先得到相鄰幀的光心在世界坐标系下的位姿，利用相鄰幀和目前幀的光心坐标，得到兩幀之間相機的位移，也就是基線向量，并記錄它的長度。之後判斷相機運動的基線是不是足夠長，對于單目相機來說，就需要比較基線與景深的關系。景深在這裡取相鄰幀的場景的深度中值，也就是對當相鄰幀下所有地圖點的深度進行從小到大排序，取深度值的中值作為景深。這裡計算了基線長度與景深的比例，如果比例特别小，就認為恢複的3D點不準确，跳過目前這個臨近幀不進行地圖點的建立。
這裡比較基線與景深的比例，顯然是為了讓更加準确的幀參與到地圖點的恢複中，問題是這裡采用這個比例的合理性。這裡我們稍微化簡一下，跳過目前幀的條件是基線長度與平均深度的比例小于0.01，也就是基線長度遠小于平均深度，這樣的情景下，兩個光心與物體形成的三角形是一個十分尖銳的三角形。個人感覺，這裡采用這個比例，主要是因為三角化的一些不足，三角化的時候，我們的任務是利用幀之間的位姿變化和兩個比對點，得到地圖點的3D資訊，具體來說是将兩個比對點沿着光心延長，按理說兩條延長線會交于一點，這個點就是地圖點，但這是理論情況，一般會因為噪聲的存在，延長線之間會産生一些偏差，實際計算時采用的一般是最小二乘法。但對于景深遠大于基線的場景來說，很可能會出現的問題就是這兩個延長線，偏差不僅大，甚至可能産生平行的情況，本身地圖點景深就很大，而運動又很小，也就是三角形的兩個底部的頂點離得很近，形成的三角形及其尖銳，加上測量都因素的噪聲，會讓這個尖出現不相交的情況，進而讓地圖點的恢複效果變差。從另一個方面考慮，由于基線的長度遠小于深度，就算不是平行的狀态，那麼由于誤差給延長線造成的交點位置變化會更明顯，進而讓計算的深度誤差很大。順便提一嘴，三角化的過程中，基線的長度過小過大都是不合适的，基線長度過小會出現這種平行的問題，基線長度過大，兩張圖像的内容變化會很大，比對特征點的數目會下降進而影響效果。
對于符合條件的幀，會根據兩個關鍵幀的位姿計算基礎矩陣，這裡計算基礎矩陣的方法是用旋轉和平移來拼出基礎矩陣，而不是像一般計算時那樣利用八點法計算，這裡是一個很巧妙的地方。
計算好後，利用詞袋模型加速對沒有比對的特征點進行快速比對，并利用極線限制來抑制離群點，進而生成新的比對點對。這一步的實作是用的SearchForTriangulation函數，函數需要傳入兩個關鍵幀的資訊、基礎矩陣、存放比對結果的向量以及一個僅用于雙目和RGBD相機的參數，傳回值則是比對的數量。
對于這個函數，主要的問題是如何了解用極線限制來抑制離群點。為了友善叙述，我們用KF1和KF2表示兩個關鍵幀。在這個函數中，首先計算了KF1的光心在KF2圖像中的像素坐标，也就是下圖的e2點，這個點也叫做極點，表示的是O1在O2坐标系的成像位置。
極線限制展現在對比對點的篩選過程中，對于一對通過了描述子距離等内容篩選的候選點對，在正式确定比對之前會有一步篩選，就是利用極線限制。對于單目相機，首先會檢查像素點到極點的距離，也就是上圖l2的長度，如果這個距離太小，就認為這個地圖點距離KF1的光心太近。
關于這段代碼，個人的了解是作者想借用深度資訊做一個篩選，首先能達到這一步的點對，都是經過了描述子距離篩選的點對，其本身的差距就已經很小了，頂多存在一些誤比對，而且這個情況下，KF1的資訊是準确的，KF2是才獲得的或者說是待比對的，這一步實際上是在利用KF1的深度資訊對比對對做篩選，按照對極幾何的限制，如果l2長度很小，意味着PO1的距離很短，一旦超過了KF1地圖點的深度分布，就可以認為是錯誤比對。
經過這一步的篩選之後，下一步是計算特征點p2到p1對應極線的距離是否小于門檻值，這裡調用的是CheckDistEpipolarLine函數，函數的輸入是兩個2D特征點、前面計算好的基礎矩陣以及目前幀的資訊，傳回值是一個布爾值，用來表示這對點是否符合對極限制。具體實作上，這裡計算了KF1中的特征點投影到KF2上産生的極線，之後用KF2上的待比對特征點p2計算p2到投影基線的距離，這裡為了計算友善是直接計算了距離的平方，不過一樣用。之後用這個距離和門檻值比較，門檻值是參考了卡方分布的一個變化量，其值與金字塔的層數有關。
是以這裡所謂的極線限制，實際上就隻有兩點，一個是由對極幾何反推的深度資訊是否與金字塔的深度資訊一緻，另一個是點到投影極線的距離是否足夠小。其本質都是用更加可靠的KF1來篩選待比對的KF2，兩種方式都是依賴于對極幾何，是以叫對極限制。
在經過對極限制的檢測之後，比對對會再利用一次旋轉直方圖來篩選，篩選通過之後就認為真正比對，會存儲比對資訊并傳回比對數量。回到CreateNewMapPoints函數，在得到了比對點關系之後，就可以利用比對關系進行三角化了。三角化的過程這裡就不做記錄了，按照流程進行就可以，三角化成功的點就直接初始化為一個地圖點，添加相應的觀測屬性之後，放入mlpRecentAddedMapPoints，等待下一次執行到局部建圖線程的時候檢驗其觀測情況并确定去留。
對于CreateNewMapPoints函數，其主要作用就是建立新的地圖點，而在建立之前，采用了一些列的步驟來篩選和擴充地圖點。擴充指的是利用具有比較好共視關系的臨近幀來做三角化，而篩選則是指擷取比對點的時候的一系列篩選誤比對的措施。其中篩選過程使用的極線比對是一個很巧妙的篩選方法，利用對極幾何延伸出來的一些限制篩除誤比對。
地圖點融合 SearchInNeighbors
回到局部建圖線程的Run函數，按照執行順序，在建立好地圖點之後，就會檢測關鍵幀隊列是不是空的，如果空了，那就進行一步融合，也就是調用SearchInNeighbors函數。如果沒有空，就将目前處理好的關鍵幀送入回環檢測隊列，轉而開始隊列裡下一幀的處理。
這裡我們直接看融合的部分，也就是SearchInNeighbors函數，它的主要作用就是檢查并融合目前關鍵幀與相鄰關鍵幀幀（兩級相鄰）中重複的地圖點，就是把一些重複的地圖點融合。在這裡需要明确兩個概念，在共視圖中，我們将目前關鍵幀的鄰接關鍵幀稱為一級相鄰關鍵幀，将與一級相鄰關鍵幀相鄰的關鍵幀稱為二級相鄰關鍵幀，簡單來說一級相鄰關鍵幀就是目前幀的鄰居，而二級相鄰關鍵幀就是鄰居的鄰居。
進入這個函數之後，首先會利用共視圖，找出在共視圖中權重排名靠前的nn個一級相鄰關鍵幀，放入vpNeighKFs向量中，這個nn參數，對于傳感器為單目相機的情況為20個，而雙目和RGBD為10個。之後周遊vpNeighKFs，其中的每個元素都是和目前關鍵幀具有較好共視關系的關鍵幀，周遊的過程中将這些關鍵幀展開來獲得二級相鄰關鍵幀，每個一級相鄰關鍵幀取共視關系最好的五個相鄰關鍵幀作為二級相鄰關鍵幀，這樣我們就獲得了目前幀的一級相鄰關鍵幀和二級相鄰關鍵幀。
接下來就需要兩種方式進行投影融合。首先是将目前幀的地圖點分别向前面得到的兩級相鄰關鍵幀做投影并查找是否可以進行融合，這一步稱作正向投影融合。具體的實作上來說，就是周遊存放兩級關鍵幀的向量，調用ORBmatcher裡面的Fuse函數來檢測一個相鄰關鍵幀和目前幀地圖點的關系。
對于Fuse函數，其傳入的參數有三個：關鍵幀、待投影地圖點以及搜尋視窗的門檻值，傳回值則是更新地圖點的數量。在這個函數中，會逐一周遊所有待投影的地圖點，對于有效的地圖點，将其變換到關鍵幀所在的相機坐标系下，投影得到該地圖點在關鍵幀上的二維圖像坐标，對于這個圖像坐标需要滿足幾個要求，一是需要在有效的範圍之内，也就是不能超過圖像範圍，二是地圖點到關鍵幀相機光心距離需滿足在有效範圍内，也就是地圖點的深度資訊要與前面維護的關鍵幀的深度資訊一緻，此外地圖點到光心的連線與該地圖點的平均觀測向量之間夾角要小于60°，這三個要求用來保證地圖點投影之後的合理性。
經過這三個篩選就可以認為投影點是合理的正确的，接下來就需要在投影點附近搜尋得到候選比對點，這個搜尋是在圖像上進行搜尋，會參考特征點所在金字塔的層數對搜尋半徑進行調整。獲得鄰域内的特征點後就需要進行特征點比對，這裡的特征點是二維特征點的比對，首先會檢測所在金字塔的層級，要求層級接近，同層或者小一層。對于單目相機來說，之後計算投影點與待檢驗特征帶你的距離，如果偏差無法滿足卡方檢驗，就認為是不比對的。滿足卡方檢驗還不夠，需要選擇描述子距離最小的作為最優比對結果，如果最優比對距離小于門檻值，就認為确實找到了一個可以融合的地圖點。
這裡需要注意，我們建立的隻是投影點到特征點之間的聯系，而在這裡，特征點并不一定擁有與之對應的地圖點，是以在融合時就有兩種選擇，如果特征點已經有與之對應的地圖點，那麼就選擇被觀測次數最多的那個進行替換，反之則将特征點加入到目前地圖點的觀測資訊中。這裡根據觀測次數進行替換，就是比較目前地圖點和對應地圖點之間的觀測次數，将觀測次數小的那個整合進觀測次數大的那個地圖點中。調用的函數是地圖點對象的Replace函數，在這個函數中會将目前地圖點的觀測資料等其他資料都"疊加"到新的地圖點上，同時将觀測到目前地圖點的關鍵幀的資訊進行更新。融合之後地圖中就隻存在一個融合後的地圖點，所有的觀測資訊都将被更新。
到這裡正向投影融合就已經完成了，在正向投影融合的過程中，我們将目前幀的地圖點向前面獲得的相鄰關鍵幀投影，尋找比對點并進行比對融合。接下來進行的融合是翻過來的部分，将兩級相鄰關鍵幀的地圖點向目前幀投影，尋找比對點并進行地圖點的融合，這種融合方式因為方向和前面那種正好相反，是以被稱為反向投影融合。
進行這一步，首先需要取出兩級相鄰關鍵幀中的所有地圖點，之後還是調用Fuse函數，但是參數變了。這裡就要提一下ORBSLAM作者寫法上的巧妙，Fuse函數的前兩個參數是關鍵幀和地圖點，是以對于這裡的兩個方向的融合，正向融合時隻需要周遊所有相鄰關鍵幀，對每一個關鍵幀調用一次Fuse函數，反向融合的時候就更友善了，直接對目前關鍵幀和所有相鄰關鍵幀的地圖點調用一次Fuse函數。一個函數同時滿足了兩個方向的融合，代碼的複用得到了很大的提升。
經過兩個方向的融合之後，就可以更新目前幀的地圖點的一些資訊了，這裡更新的内容主要是地圖點的描述子、深度以及平均觀測方向等内容，個人感覺在融合的時候，像點的觀測次數等内容是針對于一個點的，是以完全可以在融合時修改，而對于描述子這類需要用全部點重新計算的，就需要放在SearchInNeighbors函數的最後，等全部處理完之後再做更新。最後更新目前幀與其它幀的共視連接配接關系，整個融合地圖點的函數就結束了。
總的來看，SearchInNeighbors函數負責進行地圖點的融合，融合的對象是目前幀的地圖點和兩級相鄰關鍵幀的地圖點，融合的政策有兩個，一個是将目前關鍵幀的地圖點向相鄰地圖點投影并查找可以融合的地圖點對，另一個是将兩級相鄰關鍵幀的地圖點向目前幀投影并查找可以融合地圖點，分别稱為正向和反向地圖融合。融合過程中，如果地圖點能比對關鍵幀的特征點，并且該點有對應的地圖點，那麼選擇觀測數目多的替換兩個地圖點。反之，如果地圖點能比對關鍵幀的特征點，并且該點沒有對應的地圖點，那麼為該點添加該投影地圖點。完成地圖融合之後更新地圖資訊，融合的步驟就算完成。
備援關鍵幀剔除 KeyFrameCulling
回到局部建圖的Run函數，按照程式的執行流程，當處理完關鍵幀隊列中的最後一個關鍵幀之後，首先調用SearchInNeighbors函數對地圖點進行一個融合，之後在已經處理完隊列中的最後的一個關鍵幀，并且閉環檢測沒有請求停止LocalMapping的情況下，就會進行備援關鍵幀的剔除，這個函數算是局部建圖線程最後一個函數。
在進入這個函數之前，首先調用了一個局部優化，當局部地圖中的關鍵幀大于兩個的時候進行一次局部BA優化。優化後就進入到KeyFrameCulling函數裡。這個函數的主要功能，就是剔除備援關鍵幀，而備援的定義，就是指某個關鍵幀，它的地圖點有90%能夠被至少三個關鍵幀觀測到。
對于要檢驗的對象，可以确定的是如果一個關鍵幀要被剔除，那麼肯定是因為目前這一個關鍵幀的插入所導緻的，是以就需要根據共視圖将目前幀的所有共視關鍵幀提取出來，逐一檢查每個共視關鍵幀。之後周遊這個共視關鍵幀的所有地圖點，其中能被其它至少3個關鍵幀觀測到的地圖點為備援地圖點，通過nRedundantObservations這個量來記錄備援點的個數。周遊完成後，對于一個共視關鍵幀，如果90%以上的有效地圖點被判斷為備援的，則認為該關鍵幀是備援的，需要删除該關鍵幀。
删除時采用的也是ORBSLAM裡面使用次數很多的标記壞點機制，也就是調用SetBadFlag将認為要删除的關鍵幀打一個壞标記，在下一次運作到MapPointCulling的時候删除這個點。而在其它時刻由于标記了壞點，是以要被删除的點并不會産生影響。
關鍵幀處理完成之後，将關鍵幀插入到閉環檢測線程的隊列中，局部建圖的過程就算是完成了。
LocalMapping線程總結
回顧一下整個局部建圖線程，這個線程依托于主函數Run，通過關鍵幀隊列與Tracking線程保持互斥，互斥的實作依賴于信号量mbAcceptKeyFrames，但并不完全依賴于這個量，如果局部建圖線程因為一定原因速度變慢導緻和跟蹤線程不同步，局部建圖線程會持續處理關鍵幀隊列中的關鍵幀，并保持mbAcceptKeyFrames的值為false，告知跟蹤線程無法産生新的關鍵幀，但長時間沒産生關鍵幀依然會短路mbAcceptKeyFrames的互斥效果。到這個線程以後，處理的基本對象就變成了關鍵幀，已經沒有幀的概念了，在關鍵幀隊列中存在未處理的關鍵幀時，首先會調用ProcessNewKeyFrame處理新進來的關鍵幀，具體包括計算關鍵幀的詞袋向量、為目前關鍵幀的地圖點增加觀測資訊、更新共視圖等，之後調用MapPointCulling函數，将品質不好的地圖點删除，這裡删除的内容包括上一輪循環中标記為壞點的地圖點、被觀測效果不好的地圖點以及地圖點被關鍵幀觀測不足的地圖點。到這裡舊的地圖點就已經算是處理完成，之後調用CreateNewMapPoints建立新的地圖點，建立的過程是利用共視圖中共視程度最高的一部分相鄰關鍵幀，以較為嚴格的比對誤差來尋找比對點，同時還增加了基于對極幾何的極線限制來對比對點做更詳細的篩選，得到比對點後三角化進而恢複出地圖點的資訊，這部分新建立的地圖點将被打上待檢測的标簽，如果三個連續關鍵幀内沒有被觀測到就會被剔除。當隊列中的關鍵幀都被處理完之後，就會執行一個地圖點的融合，融合是為了将一些本來就是一個地圖點，但是因為觀測誤差等因素讓地圖點建立成了兩個，這種情況将會在SearchInNeighbors函數中修正，通過雙向融合政策，将地圖點與二級臨近關鍵幀中的地圖點做融合。除此之外，在隊列已經處理完且閉環檢測沒有要求停止的時候，還回調用KeyFrameCulling函數，對備援關鍵幀進行剔除，也就是将共視關鍵幀中超過90%的地圖點都可以被别的關鍵幀觀測到的關鍵幀從地圖中剔除，保證地圖結構的簡潔。
四、LoopClosing線程
按照論文裡面的流程圖，從局部建圖線程出來之後，處理完的關鍵幀就進入了回環檢測的部分，對應到代碼實作的部分，System.cc裡面初始化了一個mpLocalMapper對象用于回環檢測，并單獨開了一個線程用于回環檢測，從配置設定線程的代碼可以看出，LoopClosing線程的主函數也是Run()，進入這個主函數，可以看出其内部結構也是一個死循環來檢查回環檢測隊列有沒有新加入的關鍵幀，如果有則進行處理，沒有則循環等待。
進入這個循環，可以看出其互斥保持的基本結構也是和局部建圖線程類似，而且這個循環要簡單得多，檢查回環檢測隊列有沒有新的關鍵幀，如果有，對關鍵幀檢測是否有回環，如果檢測到了回環，就計算Sim3模型并進行回環校正，反之則直接跳過。
檢驗新關鍵幀 CheckNewKeyFrames
下面我們展開看一下這些函數，首先是進入循環後的第一個函數，也就是用來檢查回環檢測隊列mlpLoopKeyFrameQueue内部是否有局部建圖線程送進來的新關鍵幀，這個函數無比的簡單，一個互斥鎖保證對回環檢測隊列的互斥，傳回值則是mlpLoopKeyFrameQueue隊列元素是否為空，隻要不為空一直傳回真。
回環檢測 DetectLoop
這個函數負責檢驗是否産生了回環的候選，并根據檢測結果傳回true或者false。首先函數會從回環檢測隊列中取出一個關鍵幀，由于采用的是隊列結構存儲關鍵幀，是以每個關鍵幀隻會作為候選幀檢測一次回環。
由于回環出現的頻率不會太高，是以ORBSLAM采用了用時間濾除回環的方法，也就是認為每次回環間隔至少是10幀，如果不到10幀就直接認為不會出現，具體來說就是計算距離上次出現回環的幀數，如果小于10幀就不進行檢驗。這種方法還可以避免連續的回環幀出現，因為一旦回環出現，很可能的一種情況就是好幾幀回環持續出現，因為場景的重複性可能不止一對關鍵幀就可以結束的，一旦出現就有可能是好幾對關鍵幀，采用這種方法可以避免這種情況的出現。
之後我們需要利用共視關鍵幀來計算一個最低得分門檻值，首先周遊目前關鍵幀共視圖中所有與其有連接配接的關鍵幀，計算目前關鍵幀與每個共視關鍵幀的詞袋相似度得分，得到一個相似度的最低分minScore，這個最低分将作為回環關鍵幀檢測的一個門檻值。這裡得分計算的方法是直接調用DBoW2庫中計算相似度的函數score，傳入兩幀的詞袋模型即可，這裡用到的就是ProcessNewKeyFrame函數中一開始調用的ComputeBoW的計算結果。
之後正式進入到回環檢測的過程中，這裡我們的目标就是從所有的關鍵幀中檢驗是否産生了回環，對于這個目标，我們可以确定的是，與目前幀有共視關系的關鍵幀是不可能産生回環的，除此之外，前面計算的minScore也可以派上用場，我們知道minScore是目前關鍵幀和其共視關鍵幀中相似度得分的最小值，那麼如果要産生回環，如果要追求準确，那麼回環幀與目前幀的相似度得分不應該少于這個最小得分minScore。
具體的候選回環幀篩選在DetectLoopCandidates函數中，這個函數是關鍵幀資料庫KeyFrameDatabase類的一個成員函數，而一個關鍵幀資料庫對象會儲存所有的關鍵幀，這樣在檢索時會友善一些。在篩選候選回環幀的時候，首先找出和目前幀具有公共單詞的所有關鍵幀，不包括與目前幀連接配接的關鍵幀，對應的就是前面的第一個限制，之後隻和具有共同單詞較多的（最大數目的80%以上）關鍵幀進行相似度計算，這種方式是在利用詞袋模型縮小一一比對的耗時，可以看見，這裡使用的門檻值相當于一個相對門檻值，采用的是最大共同單詞數目的0.8作為門檻值，用這種方法來防止數量過少。
這樣我們就可以得到兩個門檻值：minCommonWords和minScore，前者用來根據共同單詞數進行篩選，後者則利用單詞比對度進行篩選。将符合這兩個門檻值限制的關鍵幀篩選出來之後就可以直接調用score函數來計算相似度得分。
但是相似度得分的計算到這裡還沒有結束，對照論文裡面回環檢測的部分，這裡實際上還需要有一個成組檢驗。隻用一個幀來檢驗回環是不完全的，ORBSLAM裡面選擇将與關鍵幀共視程度最高的十個幀作為一個組，用這個組來計算得分進而比較是不是真的回環。具體來說，對于上一步通過門檻值檢驗的候選回環關鍵幀，它們全部被存放在lScoreAndMatch這個清單裡，周遊這個清單，檢驗其中每個候選回環關鍵幀，利用GetBestCovisibilityKeyFrames函數取出它們共視關系最好的10幀，我們稱之為候選回環關鍵幀的共視關鍵幀，周遊這10幀計算得分。
從代碼可以看出，對于這10個共視關鍵幀，并不是它們都會給相似度産生貢獻，這些共視關鍵幀必須滿足兩個要求，對于這兩個要求可以看裡面的if分支，對于這個符合條件，前者對應的是第一步篩選，就是将沒有共視關系但是有公共單詞的關鍵幀篩選出來這一步，後者則對應的是公共單詞大于最小要求。也就是說在這一步篩選的關鍵幀，除了minScore的限制，别的限制都應該滿足，這裡不對相似度做限制，主要是因為我們不是要細緻地篩選回環幀，我們隻是要将臨近資訊納入做一個整體考核，是以不需要做這麼細節的篩選。
對于符合條件的共視關鍵幀，統計最高得分和累計得分并進行存儲。之後将所有組中最高得分的0.75倍，作為最低門檻值，隻取組得分大于門檻值的組，也就是前25%，它們将被作為候選回環關鍵幀傳回到Run函數中參與後續的篩選。
現在我們得到了候選回環幀，它們不僅相似度足夠，而且在成組檢驗、單詞數上都符合要求。但它們依然是候選回環幀，帶着個候選的帽子，接下來需要從它們中再次檢測是否真的出現了回環。這個檢測的過程出現了很多的概念：
①組（group）：一個關鍵幀和與其具有共視關系的關鍵幀組成一個組；
②候選組（candidate group）：某個候選回環關鍵幀和與其具有共視關系的關鍵幀組成的“組”，本身也是一個組，但對關鍵幀加上了候選回環幀的限制；
③連續（consistent）：不同的組之間如果共同擁有一個及以上的關鍵幀，那麼稱這兩個組之間具有連續關系，也就是兩個組之間可以通過一個關鍵幀來建立連續關系；
④連續性（consistency）：表示連續持續的長度。如果AB兩個組之間存在連續關系，那麼其連續性為1，對應的就是與初始組之間存在連續關系的組的個數，同理，如果是ABC三個組，那麼其連續性為2；
⑤連續組（consistent group）：新的被檢測出來的具有連續性的多個組的集合，本身是一個集合，集合中的元素是許多具有連續關系的組。在此基礎上，連續組的子集被稱為子連續租；
确定這幾個概念之後，就可以進行檢測，檢測的過程會用到一個很重要的資料結構ConsistentGroup，這個結構是typedef的pair<set<KeyFrame*>,int>，也就是說建立了一個關鍵幀集合與整形數字之間的聯系，關鍵幀的集合對應的就是連續組，而整形數則表示前面提到的連續性長度，而在代碼實作中，建立的由ConsistentGroup組成的向量vCurrentConsistentGroups則用于存儲所有的連續組。
除此之外，還有一個資料結構vbConsistentGroup，它是用來标記後選擇中是否有和目前組相同的一個關鍵幀，本身是一個bool類型的向量，其長度為mvConsistentGroups的長度，而mvConsistentGroups表示的是上一次循環時的連續組，也就是上次循環的vCurrentConsistentGroups，是以這個資料結構實際上存儲的就是目前的候選組中在上一次循環中是否有與之連續的連續組。
下面來看具體的檢測過程，檢測的過程本身就是對候選回環關鍵幀的内容做一個周遊，對于每個候選回環幀，根據共視關系将自身加上與自身有共視關系的其它幀組合成一個候選組，實作上調用的是GetConnectedKeyFrames，檢視這個函數的代碼可以看出，這裡函數的傳回值實際上就是和候選回環幀具有共視關系的全部關鍵幀。在傳回值的基礎上再加上自身，進而構成了候選回環幀的候選組。
之後周遊mvConsistentGroups，也就是上一輪循環中的連續組，每次取出一個連續組，利用這個連續組和候選回環幀組成的候選組進行檢驗，如果二者存在交集，就說明二者連續，就将它們建立一個連續關系。
如果這裡跳出循環，就認為上一輪循環中的某一個連續組和目前候選回環幀的候選組之間存在連續關系，接下來就需要檢驗這個連續關系是否能夠真正達到認為産生了回環的程度，在看這個分支之前，我們需要理順一下這個檢測的過程。
代碼的流程是從最右邊開始的，首先取出一個候選回環幀，利用其共視關系，組成候選回環幀組成的連續組，之後周遊上一次循環的連續組，也就是中間豎着的這個結構，每次的對象都是一個關鍵幀組成的集合，也就是橫向的一個框，用這裡面的元素和候選回環幀組成的連續組做比對，如果發現在某個集合和連續組有交集，就認為二者存在連續關系，之後檢查最左側的标記，如果标記為false，表示在這一輪的連接配接檢查中，目前這個連續組之前沒有發現連接配接關系，那麼就可以進行連接配接，将目前這個候選回環幀組成的連續組組成對之後加入到vCurrentConsistentGroups，表示是目前這一次檢測的連續組，通過pair裡面的第二個值來表示連續持續的長度。
理順清楚流程再看代碼就容易一些了，代碼裡面利用vbConsistentGroup[iG]進行篩選，其實就是為了防止多個候選回環幀的連續組連接配接在上次循環的同一個連續組上，采用這個量保證一對一的連接配接，并且連接配接關系隻維護上一次循環的情況，長度隻通過nCurrentConsistency來記錄，每次都通過加一來記錄長度的增加。
如果連續長度滿足要求，那麼目前的這個候選關鍵幀是足夠靠譜的，對應論文上的連續三幀出現回環，這個時候我們可以認為不僅有回環，而且回環的持續性也得到了保證，這種情況認為回環已經足夠靠譜，就将目前這個候選回環幀進行單獨存儲，放入mvpEnoughConsistentCandidates中。
當然，如果上一輪循環中所有的連續組都不能建立連續關系，這就意味着目前的候選回環幀沒能和舊的建立聯系，是以建立一個長度為0的連續組放入vCurrentConsistentGroups。
在函數的最後，判斷mvpEnoughConsistentCandidates内是否有元素，如果有，說明确實是出現了回環，就會傳回true之後進行後續的檢驗。
回顧一下整個函數，DetectLoop函數的唯一一個任務就是檢驗是否出現了回環，我們可以将其拆分為兩部分：回環候選幀的擷取和回環檢測。候選回環幀部分利用門檻值和最小成組得分篩選，得到的候選回環幀再利用連續性檢測進行篩選，對應論文上的持續性檢測，僅有一幀出現回環是不夠的，要連續三幀出現回環才能認為是真的出現了回環，具體來說就是用幾個向量來記錄連續持續的長度以及連續組的資訊，進而保證這個連續關系可以伴随着關鍵幀的産生不斷向下延續，最終根據延續長度傳回是不是真的出現了回環。
變換相似度計算 ComputeSim3
這一個函數的主要功能，就是計算目前關鍵幀和上一步閉環候選幀之間的Sim3變換，也可以了解為是一個在已經篩選過好幾次的候選回環幀上再做一次篩選，由于回環校正這個東西是一個很嚴肅的問題，一旦出現誤矯正，造成的影響是難以估計的，是以這裡由利用Sim3做了一次篩選。
首先周遊上一步中計算出來的優秀候選回環幀，初步篩選出與目前關鍵幀的比對特征點數大于20的候選幀集合，搜尋的過程也是利用ORBmatcher對象中的SearchByBoW對象，利用詞袋模型進行快速比對，在特征點比對數目足夠的情況下，利用一個Sim3求解器進行疊代計算。可以從下面的代碼看出，在這裡隻是初始化好優化器對象，對于一個優化器對象，在new的時候就傳入兩個關鍵幀和一個用于儲存結果的向量。
值得一提的是，這裡調用的SearchByBoW，實際上和前面跟蹤時使用的是不同的，這裡使用了一個函數的重載，如果傳入參數是兩個關鍵幀，那麼調用的是另一套代碼，但本質差別并不是太大，在這裡調用的SearchByBoW會增加比對點的限制，在前面調用的時候，我們是利用已經有對應地圖點的2D特征點與目前幀的2D特征點進行比對，而在這裡，由于換用了關鍵幀，是以待比對的2D特征點都必須是有與之對應地圖點的特征點。
初始化好優化器之後，需要的就是重複疊代來進行優化，直到有一個候選幀比對成功，或者全部失敗。這裡會周遊每一個候選幀，調用優化器的pSolver->iterate函數進行計算。這個函數本身就是使用RANSAC進行計算的一個過程，首先我們需要明确這個函數計算的Sim3是什麼，對于兩張圖像的許多比對點，需要求取一組合适的s,R,t能夠滿足這些已知點的對應變換關系，這三個計算的量就是Sim3的結果，詳細的推導過程可以參考連結。
檢視這個計算函數，其輸入是最大疊代次數、表示求解是否成功的參量、标記是否是内點的向量以及内點的數目，傳回值則是計算得到的Sim3矩陣。其内部的實作其實就是一個重複随機取比對點然後調用函數計算Sim3的過程，乍一看和RANSAC是一模一樣。
計算的關鍵在于調用ComputeSim3這個函數，函數傳入的參數就是随機的三對比對點的坐标，這裡計算Sim3的方法，參考的是Closed-form solution of absolute orientataion using unit quaternions這篇論文中提出的方法，具體實作沒有仔細看，經過這個函數，計算出了旋轉、平移和尺度。接下來利用這個計算結果，通過投影來進行内點的檢測，具體來說就是将兩個坐标系下的比對地圖點分别向彼此進行投影并得到2D的重投影坐标，利用比對點互相投影産生的重投影誤差來進行檢驗，隻有都小于門檻值才認為是真的比對。這裡的比對點來自于之前進行過的地圖點的詞袋模型加速比對，在初始化Sim3優化器的時候存儲了進去，作為優化器中的一部分被單獨儲存了起來。
計算Sim3完成之後，就根據這次計算的内點數目進行一次評價，内點數本身就可以衡量目前這次Sim3計算的優劣程度，是以内點數最多的那次Sim3計算就是最優結果，在每次調用CheckInliers函數檢測内點的時候，都會将這個量設為0，之後在每次對比對點進行檢測的過程中都會對這個量做修改。是以對于這個過程來說，内點的數目肯定小于等于比對結束後的點的數目，這也就對應了iterate函數開始的地方的這段代碼，顯然如果比對點的數目要小于RANSAC的門檻值，那麼Sim3的計算必然不可能成功。
對于每次Sim3計算的結果，根據内點數量我們取最優值并進行結果的儲存，如果超過了門檻值，就認為找到了一個合格的Sim3變換，傳回計算結果。對于沒有計算出來的，則會傳回空矩陣，代碼在這裡還維護了一個最多循環次數，外界調用的時候是每次最多5次循環就傳回，除了這個5次的限制，代碼還對最多循環次數做了限制，也就是總的循環次數不能超過300，以此來保證不會出現重複無效的計算，一旦超過300次，就會将bNoMore改為true進而告訴外邊的函數不要再循環計算了，已經算不出來該放棄了。
回到ComputeSim3函數，如果成功計算出Sim3，那麼就可以根據這個結果，擴充比對點的範圍，因為詞袋模型加速比對完全有可能存在漏網之魚。這部分的代碼首先是将經過Sim3檢驗過後的内點儲存出來，因為這部分是原本在計算中就符合條件的，不需要額外的計算。
在這之後，我們就利用ORBmatcher裡面的第三種比對方法SearchBySim3來進行一次比對，在ORBSLAM前面的部分中，我們使用了兩種搜尋比對的方法：SearchByProjection和SearchByBoW，這裡用的則是之前沒用過的第三種方法。簡單看過這個函數之後，個人感覺這個函數和SearchByProjection其實是一個思路，都是投影之後在鄰近區域内進行搜尋，差別在于SearchByProjection使用的是從目前幀的成員變量中獲得位姿，并通過計算來實作幀間位姿變化，而是用SearchBySim3則是直接将Sim3的計算結果作為參數傳了進來，省了在函數中計算的那部分，除此之外在一些比對細節上還稍微有差别，比如SearchBySim3沒有使用最優次優篩選，但本質上思路是相同的。
通過這種方式補充比對點之後，就可以利用新的比對點對Sim3的計算結果進行優化，調用優化器中的OptimizeSim3函數，傳回是優化後比對點中内點的個數，至此我們認為利用Sim3計算出來的結果已經可以用來，如果這時内點數目超過20個，認為優化成功，停止Sim3的疊代計算，将此次計算對應的候選回環關鍵幀認為是真的回環關鍵幀，将相關的資訊儲存下來用于後續的回環校正。
到這裡，ComputeSim3裡第一個超長的大循環就結束了，在這個大循環裡，主要完成的就是一個任務，循環去對候選回環幀中嘗試計算Sim3，沒算出來就重複計算，算出來了就優化檢驗，直到某個結果符合内點數目的要求或者完全計算不出來。程式會因為兩種情況從循環出來，一種是确實查到了回環，這種情況bMatch會改為true，另一種則是因為循環次數超過限制不得不跳出循環，這種情況表明Sim3的計算失敗，直接傳回失敗即可，當然，由于計算失敗，目前這一幀也就不會産生回環，相應地也要清空之前計算的優秀候選回環幀。
我們的重點放在計算成功的時候，我們這裡嘗試計算的Sim3，實際上是建立了兩幀之間的回環關系，其依據是兩幀之間通過計算出來的Sim3能夠建立至少20個内點的對應關系，但是這個對應關系僅僅是兩幀之間的，考慮到ORBSLAM使用的共視關系，這個對應關系是可以擴充到共視幀上去的。
是以在成功計算出Sim3之後，我們還需要利用共視關系，對我們确定的這對回環做一次檢查。這裡我們取出回環幀的共視幀，将其和共視幀放在一起組成一個vpLoopConnectedKFs向量，之後周遊這個向量，取出所有的有效地圖點放入mvpLoopMapPoints向量。我們後續的檢驗将建立在這兩個向量上，直白點就是從這兩個向量中尋找更多的比對點。
這裡實作的過程使用的是ORBmatcher裡面的SearchByProjection函數，這個函數在ORBmatcher裡面重載了4次，這裡調用的SearchByProjection是專門用在Sim3檢測的那一個，它的作用是将回環幀及其所有共視幀中的地圖點投影到目前關鍵幀，在其中尋找新的比對點，傳回值是比對點的數目。函數的實作大體思路都是一樣的，差別在于這裡的R和t是由Sim3矩陣分解而來的，而不是幀對象裡面直接讀取，擷取了位姿變換之後，後面的投影搜尋方式就一樣了。在獲得比對點的數目之後，如果超過40個，認為回環真的出現，否則依然認為回環沒有出現。
回過頭來看整個ComputeSim3函數，不難發現，僅僅在Sim3這裡的篩選就已經重複到離譜了，更别說除了Sim3之外還有别的回環檢測的篩選。這個函數首先用一個篇幅極大的while循環疊代計算優秀候選回環幀的Sim3，如果能夠計算出來，就利用Sim3模型查找詞袋模型比對沒能找到的其它比對點，擴充内點數目之後如果超過20認為這個候選回環幀的可能性是很高的，直接跳出循環，将候選回環幀的地圖點和其共視幀的地圖點放到一起在做一次投影檢測，如果比對點個數超過40，認為這個幀确實是回環幀。
可以說在這個函數中，核心就是利用Sim3檢測有沒有回環，個人的感覺是回環檢測的過程太複雜了，一方面，在得到内點超過20個回環幀之後的利用共視關系擴充，思路确實是沒錯的，但是在進去ComputeSim3函數之前檢測回環的時候，成組檢測本身就用了共視關系，這裡再用是不是稍微有些重複；另一方面，在計算過程中如果發現一個内點數量超過20的幀之後，代碼會直接跳出while循環，之後進入投影點的擴充，問題在于，在這一步擴充時，目前這個回環候選幀是有可能被篩掉的，如果被篩掉，程式的執行不會再傳回到上一步，也就是說如果這個時候如果一個誤回環被檢測出來内點超過20個，但是在40内點檢測的時候被篩掉了，程式會認為沒有發生回環，并不會再繼續檢測剩餘的候選回環幀而是直接清空，也就是說真的回環有可能被忽略了。程式的這種寫法，個人感覺确實保證了回環檢測的嚴格，但是有些嚴格過頭了，為了保證時間開銷不得不在一些地方做一些剪枝操作，但這又帶來了一些遺漏回環的潛在影響。
回環校正 CorrectLoop
經過了成組檢驗和Sim3檢驗的雙重考察之後，我們認為确實是出現了回環，這種情況下，就進入到CorrectLoop函數進行回環的校正。第一個問題在于互斥的保持，考慮到回環校正是一個涉及全身的問題，是以必須要保持互斥，這裡函數的一開頭就對互斥做了限制，如果在進行全局BA，就停止BA等待自己給出全新的BA，此外還要等局部建圖線程結束再進行回環校正。
正式得到線程的處理權之後，代碼會根據共視關系更新目前關鍵幀與其它關鍵幀之間的連接配接關系，也就是調用UpdateConnections函數更新目前關鍵幀的連接配接關系，更新好之後取出目前關鍵幀及與其存在共視關系的其它關鍵幀放在一起，這裡我們稱為目前關鍵幀組。接下來周遊整個目前關鍵幀組，對于共視關鍵幀，計算出其到目前關鍵幀的位姿變換，進而将所有優化前的位姿取了出來。除此之外，在這段代碼中應該也完成了共視關鍵幀位姿的校正，從708行的變量名可以看出這一點，其儲存的結果是在705行計算出來的，具體的優化過程屬實是看不懂了。
之後周遊存放的共視關鍵幀，應該是在根據校正的位姿再一次優化地圖點，對于待矯正共視關鍵幀的每個地圖點，首先取出其世界坐标系下的坐标，利用Converter對象轉換為一個Eigen庫的向量，對這個向量連續進行了一波轉換，最後得到了校正後的結果并儲存了起來。
主要的校正過程，其實就是在做一個坐标的轉換，從上圖中的代碼可以看出，在749行取出了未校正的世界坐标，經過一堆轉換，在756行得到了矯正結果并在757行将校正後的結果重新覆寫了進去，如果我們将751行和756行看作是形式的轉換而不是校正的過程，那麼校正的實作就是754行這段代碼，點進來看map這個函數，隻有一行代碼，用于對這個傳入的向量進行旋轉縮放和平移的轉換，是以這裡的第一層轉換，就是将地圖點的世界坐标映射到還沒有矯正的這個共視關鍵幀上，之後再在第二層的轉換中，将位姿轉換到校正後的世界坐标系中，經過這一系列的轉換，将地圖點轉換到了校正後的世界坐标系下，進而完成了地圖點的校正過程。
校正之後會調用UpdateConnections去更新共視關系和權值的變化，除此之外由于地圖點發生了變化，是以需要檢查目前幀的地圖點與經過閉環比對後該幀的地圖點是否存在沖突，對沖突的進行替換或填補。
到這裡我們完成了對地圖點的操作，但是因為回環産生的新觀測關系還是沒有校正的，接下來就是在進行這一部分，但是這部分的代碼實在是太難看懂了，首先是将之前計算的回環關鍵幀的共視組裡面的地圖點取出來，由于它們在地圖中時間比較久經曆了多次優化，認為是準确的，将它們向目前關鍵幀組進行投影，尋找替換校正之後的地圖點，這裡感覺像是在進行一個額外的地圖點融合，前面局部建圖部分也有地圖點融合，但是那個融合的範圍要小得多，僅僅局限于目前關鍵幀和兩級共視範圍内的關鍵幀，到了這裡由于檢測出了回環，是以按道理也是有部分局部地圖點其實是同一個，這裡就需要做一個融合的操作。
之後更新目前關鍵幀組之間的兩級共視相連關系，得到因閉環時地圖點融合而新得到的連接配接關系，這是說地圖點融合之後，原本的共視圖其實也會因為回環産生新的邊，要将這部分邊加入進去。代碼上使用的是一種做減法的方法，首先調用UpdateConnections更新共視圖的連接配接關系，之後将原本一級共視和二級公式産生的連接配接關系給扣掉，那麼剩下的就是因為回環産生的共視關系。
接下來對這部分新增的邊進行一個校正，使用的方法是本質圖優化，優化本質圖中所有的關鍵幀的位姿。完成之後儲存一下一些此次運算的資訊，供下一次使用，這樣一次回環校正就算完成了。
回過頭來看一下整個CorrectLoop函數，進入到這一步我們認為确實是産生了回環，需要采用校正的方法來消除累積誤差，校正的過程可以分為地圖點的校正和位姿的校正，對地圖點的校正是利用一系列的坐标轉換來完成的，将未校正的地圖點從世界坐标系映射到相機坐标系，在映射到校正後的世界坐标系下，由于這個過程動了地圖點，是以又跟上了一系列的對地圖連接配接關系的更新，對位姿的校正則是依賴于因為共視關系産生的新的邊，在地圖點的融合之後，回環幀和目前幀之間會因為融合而産生新的共視邊，篩選出這部分邊并利用圖優化進行一次本質圖優化，進而得到校正後的位姿。
LoopClosing線程總結
對于ORBSLAM的最後一個線程，進入到這個線程的就隻有局部建圖線程處理好的關鍵幀了，這個線程的任務就隻有一個：發現并校正回環。對于回環這個現象，ORBSLAM采用了極為嚴格檢測方法，甚至讓人覺得有些嚴格得過分，在有新的關鍵幀的情況下，首先通過DetectLoop函數來尋找候選的回環幀，在這個函數中我們可以認為候選回環的檢測包括兩部分：回環幀擷取和回環檢測，前者可以了解為一個最基本的篩選，這裡隻考慮了共同單詞等因素，在完成這部分之後，後者回環檢測将對其采用連續性和最小相似度的檢測，利用很巧妙的成組檢驗的方法來篩選回環，伴随着新關鍵幀的到來不斷向後延伸。DetectLoop函數産生的候選回環幀依然是候選，這裡我們隻考慮了特征資訊以及連續性資訊來檢測回環，而後面跟的ComputeSim3則是利用回環的位姿變換來進行深層次篩選。簡單來說就是根據比對點，重複對候選回環幀計算Sim3模型，如果有一個模型能夠讓内點數目足夠多，就認為極有可能是真的回環，如果再次使用計算出來的Sim3模型增加比對點後能夠超過40個，就認為這個Sim3模型計算的很對，并且這兩幀之間也确實是真正的回環，這樣就可以進入到CorrectLoop進行回環的校正。對于CorrectLoop函數，最大的問題在于将誤差利用回環和共視，均攤到每一個個體上，這個函數可以拆分成兩部分：地圖點校正和位姿校正，地圖點校正依托于一系列的坐标轉換，考慮到回環出現時一般會伴随地圖點的重複，在校正後引入了地圖點的融合，而因為一部分地圖點融合，在回環幀和目前幀之間會産生新的共視關系，這部分共視關系是因為回環所産生的，在之前并沒有進行校正過，是以在最後補一個本質圖優化并對位姿進行校正，進而完成整個回環的校正。
五、總結
經過兩個多周的摸魚時光，終于把ORBSLAM2代碼中主要的部分看完了，一些細節上還沒有看，比如說關鍵幀維護、共視圖修正等内容，在看的過程中都是直接作為函數整體來了解的，具體内部的實作就先不研究了。
對于整個ORBSLAM2的源代碼，按照個人的了解從功能上可以劃分為四部分：初始化、Tracking、LocalMapping以及LoopClosing。
初始化的部分用于整個SLAM流程的建立，處理對象是開始時候的兩幀品質比較好的圖像，對這兩幀進行特征點的提取和比對，采用雙線程計算基本矩陣和單應矩陣，根據評分機制選擇二者中的一個并拆分得到旋轉和平移，之後通過三角化創造出第一批地圖點。初始化的部分在于開一個好頭，這個階段由于隻有兩幀，是以基本沒有什麼可以操作的空間，當然，如果後續出現了跟蹤失敗的情況，還是要乖乖回到這裡重新初始化。
跟蹤部分Tracking屬于是ORBSLAM2能夠領先于其它視覺SLAM架構的一個很關鍵的地方，個人感覺甚至可以說是整個ORBSLAM裡面最重要的部分。使用三種跟蹤模型各有特點，恒速模型是利用目前的速度來估計一個位姿，用這個位姿去進行投影比對，如果效果很好就接一個優化，在相機運動速度不快而且穩定的情況下，可以說絕大多數的跟蹤使用的都是這個模型，它的優點在于快，因為采用鄰域搜尋的思想，是以根本不需要進行大範圍的比對，隻在投影位置的附近進行比對就足以，這裡節省了大量的時間。參考關鍵幀模型是跟蹤的後勤保障，當恒速模型失效時，說明上一幀已經不靠譜了，這個時候我們将參考提前，換成上一個關鍵幀，但這個時候由于距離已經變遠了，就不能再用估計的方法了，既然暴力比對必不可少，我們要做的就是盡可能優化減少次數，這裡就使用了詞袋模型來加速比對，雖然沒有恒速模型那麼快的速度，但是已經比暴力比對要好不少了，得到比對點之後用上一幀的位姿作為初值來進行一個優化。重定位模型則是跟蹤的最後防線，正因為是最後防線，是以這個時候上一幀和上一個關鍵幀都不靠譜了，其參考來源換成了關鍵幀資料庫中的其它關鍵幀，不僅如此，在這種模型中處處展現了小心謹慎的考慮，首先是在搜尋候選關鍵幀組的時候，用了三種篩選來優化候選關鍵幀組，之後通過對候選關鍵幀組求解EPnP得到位姿，對于得到的位姿，根據内點數目來判斷，對于内點數目不多不少的情況，還采用了嚴格的内點補充方法，最後根據最終的内點數目來判斷位姿的準确與否。三種模型可以說都是在盡可能偷懶進而提升速度，但有一個不可忽略的問題就是，采用這三種模型會讓地圖點逐漸減少，因為相機是在不斷移動的，總會讓一部分地圖點移動出視野範圍，也可能因為誤比對導緻跟蹤丢失，這種情況就需要局部地圖跟蹤來救場，TrackLocalMap函數先利用共視關系擴充目前幀的地圖點，之後做篩選，對符合條件的地圖點，再次使用一次BA優化校正位姿，并且更新觀測資訊最後判斷是否跟蹤成功。
局部建圖部分LocalMapping處理的對象，就是跟蹤部分産生的關鍵幀了，在這個函數中，包含了SLAM流程的絕大多數對地圖的操作。一個新的關鍵幀到來，首先會調用ProcessNewKeyFrame處理新進來的關鍵幀，具體包括計算關鍵幀的詞袋向量、為目前關鍵幀的地圖點增加觀測資訊、更新共視圖等，重頭戲在于對地圖點的操作，操作可以分為剔除、建立和融合。剔除依賴于MapPointCulling函數，它會将品質不好的地圖點删除，這裡删除的内容包括上一輪循環中标記為壞點的地圖點、被觀測效果不好的地圖點以及地圖點被關鍵幀觀測不足的地圖點。建立則是依賴于CreateNewMapPoints函數，建立的過程是利用共視圖中共視程度最高的一部分相鄰關鍵幀，以較為嚴格的比對誤差來尋找比對點，同時還增加了基于對極幾何的極線限制來對比對點做更詳細的篩選，得到比對點後三角化進而恢複出地圖點的資訊，這部分新建立的地圖點将被打上待檢測的标簽，如果三個連續關鍵幀内沒有被觀測到就會被剔除。對于單目相機，這裡的地圖點建立時除了初始化之外唯一的建立機會，單目相機隻有三角化才會創造新的地圖點。最後的融合則是在當隊列中的關鍵幀都被處理完之後，融合是為了将一些本來就是一個地圖點，但是因為觀測誤差等因素讓地圖點建立成了兩個，這種情況将會在SearchInNeighbors函數中修正，通過雙向融合政策，将地圖點與二級臨近關鍵幀中的地圖點做融合。為了保證關鍵幀結構的簡潔，在隊列已經處理完且閉環檢測沒有要求停止的時候，還回調用KeyFrameCulling函數，對備援關鍵幀進行剔除，也就是将共視關鍵幀中超過90%的地圖點都可以被别的關鍵幀觀測到的關鍵幀從地圖中剔除。
回環檢測部分LoopClosing依然是在對關鍵幀做處理，但是目标已經換成檢索回環了。這部分可以說采用了及其嚴格甚至嚴格到變态的回環檢測政策，共同單詞檢驗、成組檢驗、連續性檢驗還不夠，還加了Sim3檢驗，而且還在Sim3檢驗的基礎上給它加了共視關系的擴充，這複雜的檢驗方法讓回環的出現變得很準确，但是造成的時間開銷也很大，一旦檢測出回環，就調用CorrectLoop來進行校正，通過共視關系和回環關系，将累積誤差均攤到其中，進而實作更加準确的地圖估計。
ORBSLAM不愧是目前最典型的視覺SLAM架構，它不僅保證了準确性，還極大程度提高了速度，也正是因為這個優點，大量的視覺SLAM架構在其上做修改，誕生了一些列的新架構。這樣整個ORBSLAM2的代碼記錄就算完成了，如果後續有需要，可能也會再把ORBSLAM2使用到的一些資料結構以及維護方法記錄一下。整篇部落格是根據程式執行流程來記錄的，裡面的一些内容是參考衆多資料和網課得到的，也包括個人的了解，有錯誤是很正常的，畢竟本人也不是什麼大佬，如果有錯希望大佬能指出。