ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras(論文原始碼解讀)

這是我根據看泡泡機器人吳博的ORB-SLAM2原始碼詳解視訊做的一些筆記，因為主要是作為自己的筆記使用，所以會有點亂。大家可以看吳博在B站上面的視訊：https://www.bilibili.com/video/BV1h441177pY?from=search&seid=17501489641807485361
講的還是不錯的。
大家也可以參考劉國慶師兄的註釋，寫的也是非常仔細，連結是：https://github.com/DreamWaterFound/self_commit_ORB-SLAM2

如果我寫的有什麼不對，還希望大家批評指正。

四個主要執行緒的程式碼主要放置的位置：Tracking.cpp、LocalMapping.cpp、LoopClosing.cpp、Viewer.cpp

變數命名規則：
P：表示指標資料型別、n：表示int型別、b：表示bool型別、s表示set型別、v：表示vector資料型別、l：表示list資料型別、m：表示類成員變數member、KF表示KeyPoint資料型別。

ORB-SLAM的入口是System.cpp程式，這也是主程式的實現檔案，這個檔案主要的程式包括：1.系統的建構函式System()，將會啟動其他的執行緒；2.雙目輸入時的跟蹤介面TrackStereo(左側影像，右側影像，時間戳)；3.當輸入影像為RGBD時進行追蹤TrackRGBD()；4.輸入位單目影像時的追蹤器介面TrackMonocular()；5.ActivateLocalizationMode()為啟用定位模式，DeactivateLocalizationMode()為取消定位模式，MapChanged()判斷是否地圖有較大的變化，Reset()準備執行復位，Shutdown()退出；6.SaveTrajectoryTUM()按照TUM格式儲存相機執行軌跡並儲存到指定的檔案之中（只有當感測器為雙目或者RGBD時才可以工作）;7.SaveKeyFrameTrajectoryTUM()儲存關鍵幀軌跡，SaveTrajectoryKITTI()按照KITTI資料集的格式將相機的運動軌跡儲存到檔案中；8.GetTrackingState()獲取跟蹤器狀態，GetTrackedMapPoints()獲取追蹤到的地圖點（其實實際上得到的是一個指標），GetTrackedKeyPointsUn()獲取追蹤到的關鍵幀的點。

而整個程式實現主要的步驟是：
1.（從該步起程式碼主要位於Tracking.cpp中）輸入影像(分別針對立體相機、RGBD相機和單目相機)(括號裡為一些主要的引數)：
GrabImageStereo(ImRectLeft,imRectRight,timestamp)：將RGB或者RGBA影像轉換為灰度影像；構造Frame；跟蹤。
GrabImageRGBD(imRGB,imD,timestamp)：與上類似，加一步將深度相機的disparity轉為Depth , 也就是轉換成為真正尺度下的深度。
GrabImageMonocular(im,timestamp)
2.轉換為灰度影像得到一些主要的引數：
Stero:mImGray,imGrayRight
RGBD:mImGray,imDepth
Mono:mImGray
3.構造Frame(mpIniORBextractor相比mpORBextractorLeft提取的特徵點多一倍)：
Stero:Frame(mImGray.imGrayRight,mpORBextractorLeft,mpORBextractorRight)
RGBD:Frame(mImGray,imDepth,mpORBextractorLeft)
Mono(未初始化):Frame(mImGray,mpIniORBextractor)、Mono(已初始化):Frame(mImGray,mpORBextractorLeft)
4.Track:資料流進入Tracking執行緒(VO執行緒開始)（將Frame傳到下一個步驟）
5.初始化(Tracking執行緒開始)：
SteroInitialization():雙目和RGB-D初始化：設定初始位姿；將當前幀構造為初始關鍵幀；在地圖中新增該初始關鍵幀；為每個特徵點構造MapPoint；在區域性地圖中新增該初始關鍵幀。
MonocularInitialization()：單目初始化：得到用於初始化的第一幀，初始化需要兩幀；如果當前幀特徵點數大於100，則得到用於單目初始化的第二幀，如果當前幀特徵點太少，重新構造初始器；在mInitialFrame與mCurrentFrame中找匹配的特徵點對；如果初始化的兩幀之間的匹配點太少，重新初始化；通過H模型或F模型進行單目初始化，得到兩幀間相對運動、初始MapPoints；刪除那些無法進行三角化的匹配點；將三角化得到的3D點包裝成MapPoints。
6.相機位姿跟蹤（出現兩種情況，跟蹤失敗和成功）（mbOnlyTracking預設為false，使用者可以通過執行介面選擇跟蹤定位模式）：
（1）mbOnlyTracking(False):會出現兩種情況，一種情況是位姿跟蹤，主要有TrackWithMotionModel()和TrackRerenceKeyFame()兩個方法，這兩種方法的主要區別見下面的圖：

TrackReferenceKeyFrame()：將當前幀的描述子轉化為BoW向量；通過特徵點的BoW加快當前幀與參考幀之間的特徵點匹配；將上一幀的位姿態作為當前幀位姿的初始值；通過優化3D-2D的重投影誤差來獲得位姿；剔除優化後的outlier匹配點。
TrackWithMotionModel()：更新上一幀的位姿，對於雙目或rgbd攝像頭，還會根據深度值為上一關鍵幀生成新的MapPoints；根據勻速度模型進行對上一幀的MapPoints進行跟蹤, 根據上一幀特徵點對應的3D點投影的位置縮小特徵點匹配範圍；優化位姿；優化位姿後剔除outlier的mvpMapPoints。
另一種情況就是Relocalization()重定位，主要的方法實現見下面的圖：

Relocalization()：計算當前幀特徵點的Bow對映；找到與當前幀相似的候選關鍵幀；通過BoW進行匹配；通過EPnP演算法估計姿態；通過PoseOptimization對姿態進行優化求解；如果內點較少，則通過投影的方式對之前未匹配的點進行匹配，再進行優化求解。
（2）mbOnlyTracking(true):同時跟蹤與定位，不插入關鍵幀，區域性地圖不工作。
7.區域性地圖跟蹤：
（1）UpdateLocalMap():主要分為UpdateLocalKeyframes()和UpdateLocalPoints()，
UpdateLocalPoints()：清空區域性MapPoints；遍歷區域性關鍵幀；將區域性關鍵幀的MapPoints新增到mvpLocalMapPoints。
UpdateLocalKeyFrames()：遍歷當前幀的MapPoints，記錄所有能觀測到當前幀MapPoints的關鍵幀；更新區域性關鍵幀。
（2）SearchLocalPoints()：獲得區域性地圖和當前幀的匹配：遍歷當前幀的mvpMapPoints，標記這些MapPoints不參與之後的搜尋；將所有區域性MapPoints 投影到當前幀，判斷是否在視野範圍內，然後進行投影匹配。
（3）PoseOptimization():最小化投影誤差優化位姿
8.是否生成關鍵幀：
在很長時間內沒有插入關鍵幀、區域性地圖空閒、跟蹤快要跪的時候都要插入關鍵幀
跟蹤地圖MapPoints的比例比較少
9.生成關鍵幀（生成m1NewKeyFrames向下一級傳）：
KeyFrame(mCurrenFram,mpMap,mpKeyFrameDB)
對於雙目或者RGBD攝像頭構造一些MapPoints,為MapPoints新增屬性
10.(以下步驟的程式碼位於LocalMapping.cpp執行緒)檢查佇列：CheckNewKeyFrames()：檢視列表中是否有等待被插入的關鍵幀。
11.處理新關鍵幀：
ProcessNewKeyFrame():更新MapPoints與KeyFrame的關聯；UpdateConnections()
ProcessNewKeyFrame()：從緩衝佇列中取出一幀關鍵幀；計算該關鍵幀特徵點的Bow對映關係；跟蹤區域性地圖過程中新匹配上的MapPoints和當前關鍵幀繫結；更新關鍵幀間的連線關係；將該關鍵幀插入到地圖中。
12.刪除MapPoints:MapPointCulling()，刪除地圖中新新增的但是質量不好的MapPoints:a.IncreasFound/IncreaseVisible<25%;b.觀測到該點的關鍵幀太少。
13.生成MapPoint:運動過程中和共檢視程度比較高的關鍵幀通過三角化恢復出一些MapPoints，CreateNewMapPoints()：在當前關鍵幀的共視關鍵幀中找到共視程度最高的nn幀相鄰幀vpNeighKFs；遍歷相鄰關鍵幀vpNeighKFs；判斷相機運動的基線是不是足夠長；根據兩個關鍵幀的位姿計算它們之間的基本矩陣；通過極線約束限制匹配時的搜尋範圍，進行特徵點匹配；對每對匹配通過三角化生成3D點。
14.MapPoints融合：檢查當前關鍵幀和相鄰（兩極相鄰）重複的MapPoints
15.Local BA:和當前關鍵幀相鄰的關鍵幀以及MapPoints做區域性BA優化，SearchInNeighbors()：獲得當前關鍵幀在covisibility圖中權重排名前nn的鄰接關鍵幀；將當前幀的MapPoints分別與一級二級相鄰幀(的MapPoints)進行融合；將一級二級相鄰幀的MapPoints分別與當前幀（的MapPoints）進行融合；更新當前幀MapPoints的描述子，深度，觀測主方向等屬性。
16.關鍵幀刪除：其中90%以上的MapPoints能被其他共視關鍵幀（至少三個）觀測到的關鍵幀（生成mlploopKeyFrameQueue送到下一級中）
17.（以下步驟的程式位於LocalClosing.cpp執行緒，閉環檢測）從佇列中取一幀作為mpcurrentKF，並判斷距離上一次閉環檢測是否超過10幀，計算當前幀與u相連關鍵幀的Bow最低得分，由此得到mpcurrentKF和minscore.
18.檢測得到閉環候選幀：
(pKF,minscore)找出與當前幀有公共單詞的關鍵幀，但不包括與當前幀相連的關鍵幀（1KFsharingWords）；統計候選幀中與pKF具有共同單詞最多的單詞數（maxcommonwords）;得到閥值（mincommons=0.8maxcommonwords）(maxcommonwords,mincommnts,minscore),篩選共有單詞大於mincommons且Bow得分大於mincore的關鍵幀（lscoreAndMatch）；將存在相連的分為一組，計算組最高得分bestAccScore,同時得到每組中得分最高的關鍵幀（IsAccScoreAndMatch,bestAcoScore）;得到閥值minSoreToRetain=0.75bestAccScore(IsAccScoreAndMatch,minSoreToRetain);vpLoopGandidates.
20.（vpLoopCandidates）檢測候選幀的連續性
21.Sim3計算
SearchByBow(vvpMapointMatches);構造Sim3求解器，對匹配成功的關鍵幀進行Sim3求解（mpCurrentKF,pKFScm–>(R,t,s),vpMapointMatches）;SearchBySim3得到更多的匹配；優化Sim3;將MatchedKF共視幀取出;將MatchedKF共視幀取出；SearchByProjection得到更多的匹配點。
22.correctLoop:
暫停LocalMapping防止插入新的關鍵幀（mpCurrentKF）;更新當前幀的共視相連關係（mpCurrentKF）;得到與當前幀相連的關鍵幀（mvpcurrentConnectedKFs（包括當前關鍵幀），mpCurrentKF的mg2oScw）;根據傳播得到與關鍵幀相連關鍵幀閉環後的Sim3；調整當前相連關鍵幀的MapPoints;將Sim3轉為SE3，並調整關鍵幀位姿；更新關鍵幀連結關係(mvpCurrentMatchedPoints–>ploopMapPoint,mpCurrentKF的MapPoint–>pCurrentMapPoint);用pLooMapPoints替換pCurrentMapPoints(correctedSim3,mvpLooopMapPoints);將這些關鍵幀的MapPoints與閉環MapPoints融合（mvpCurrentConnectedKFs）;更新關鍵幀連線關係（LoopConnections）;得到由閉環形成的連線關係（LoopConnections,mpMap,mpMatchedKF,mpCurrentKF,correctedSim3,NocorrectedSim3）;
OptimizeEssentialGraph;新建一個執行緒進行全域性BA優化

Frame.cpp
一、雙目立體匹配：
1.為左目每個特徵點建立帶狀區域搜尋表，限定搜尋區域。（已提前極線校正）
2.通過描述子進行特徵點匹配，得到每個特徵點最佳匹配點scaleduR0
3.通過SAD滑窗得到匹配修正量bestincR
4.(bestincR.dist)(bestincR-1,dist)(bestincR+1,dist)三個點擬合出拋物線，得到亞畫素修正量deltaR
5.最終匹配點位置為：scaleduR0+bestincR+deltaR
二、Disparity與Depth（對於RGB-D來說）:

單目Initializer.cpp

ORBmatcher.cpp
1.ORB-SLAM2中特徵點匹配均採用各種技巧減小特徵點匹配範圍
2.ORB-SLAM2中特徵點通過描述子匹配後會進行旋轉一致性檢測，並且最佳匹配點要明顯優於次匹配點；
3.ORB-SLAM2中特徵點提取仍然是非常耗時的地方。
SearchByProjection函式和SearchBySim3函式利用將相機座標系下的MapPoints投影到影像座標系，在投影點附近搜尋匹配。
SearchByBoW函式通過判斷特徵點的word的node是否相同可以加速匹配過程。
SearchForTriangulation函式利用對極幾何約束：左目一個點對應右目一條線。將左影像的每個特徵點與右影像同一node節點的所有特徵點一次檢測，判斷是否滿足對極幾何約束，滿足約束就是匹配的特徵點。
Fuse和SearchByProjection函式差不多，只不過是判斷特徵點p的MapPoint是否與MapPoint點P衝突。