int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
   

nmatches表示匹配到的對應點對數。

二、同時計算兩個模型

  在找到對應點之後，開始呼叫Initializer.cc中的Initializer::Initialized函式進行初始化工作。為了計算R和t，ORB_SLAM為了針對平面和非平面場景選擇最合適的模型，同時開啟了兩個執行緒，分別計算單應性矩陣

和基礎矩陣

。如下所示：

1. thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));

2. thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));

這裡的H和F分別滿足下列關係： 

執行緒threadH呼叫Initializer::FindHomography函式，計算單應性矩陣H，採用歸一化的直接線性變換（normalized DLT）。執行緒threadF呼叫Initializer::FindFundamental函式計算基礎矩陣F，使用歸一化8點法。為了評估哪個模型更合適，文中使用和來計算各自的分值。分別呼叫Initializer::CheckHomography函式和Initializer::CheckFundamental函式進行計算，計算的方法如下所示，其中統一表示和： 
其中，和表示對稱的轉換誤差，分別是從當前幀到參考幀的變換誤差和參考幀到當前幀的變換誤差。這裡： 

三、模型選擇

  文中認為，當場景是一個平面、或近似為一個平面、或者視差較小的時候，可以使用單應性矩陣H，而使用基礎矩陣F恢復運動，需要場景是一個非平面、視差大的場景。這個時候，文中使用下面所示的一個機制，來估計兩個模型的優劣： 

當大於0.45時，選擇從單應性變換矩陣還原運動。不過ORB_SLAM2原始碼中使用的是0.4作為閾值，如下：

1. // Compute ratio of scores

2. float RH = SH/(SH+SF);

3.  

4. // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)

5. if(RH>0.40)

6. return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

7. else //if(pF_HF>0.6)

8. return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

四、運動恢復（sfm）

  選擇好模型後，就可以恢復運動。 
（1）從單應性變換矩陣H中恢復 
  在求得單應性變化H後，本文使用FAUGERAS的論文[1]的方法，提取8種運動假設。這個方法通過視覺化約束來測試選擇合理的解。但是如果在低視差的情況下，點雲會跑到相機的前面或後面，測試就會出現錯誤從而選擇一個錯誤的解。文中使用的是直接三角化8種方案，檢查兩個相機前面具有較少的重投影誤差情況下，在檢視低視差情況下是否大部分雲點都可以看到。如果沒有一個解很合適，就不執行初始化，重新從第一步開始。這種方法在低視差和兩個交叉的檢視情況下，初始化程式更具魯棒性。程式中呼叫Initializer::ReconstructH函式恢復運動。

(2)從基礎矩陣F中恢復 
  在得到基礎矩陣F，並且一直攝像機內參K的情況下，可以計算得到本質矩陣E，然後使用[2]中的方法，恢復出4個運動假設的解。這一部分的理論推導在之前部落格做過介紹。其中基礎矩陣F得到本質矩陣E的公式如下所示： 

同樣的，這4個解中只有一個是合理的，可以使用視覺化約束來選擇，本文使用與單應性矩陣做sfm一樣的方法，即將4種解都進行三角化，然後從中選擇出最合適的解。這裡使用的是Initializer::ReconstructF函式。

五、集束調整

  最後使用一個全域性集束調整（BA)，優化初始化結果。這一部分是在Tracking.cc中的CreateInitialMapMonocular()函式中，使用瞭如下語句：

Optimizer::GlobalBundleAdjustemnt(mpMap,20);
 

  如下圖所示，PTAM和LSD_SLAM在這個資料集中，會將所有點初始化在一個平面上，而ORB_SLAM會一直等到有足夠的視差，才使用基礎矩陣，得到最正確的初始化。由於ORB-SLAM對初始化的要求較高，因此初始化時可以選擇一個特徵豐富的場景，移動攝像機給它提供足夠的視差。另外，由於座標系會附著在初始化成功的那幀影像的位置，因此每次初始化不能保證在同一個位置。

跟蹤

這一部分是ORB_SLAM系統中最基本的一步，會對每一幀影像進行跟蹤計算。Tracking執行緒執行在主執行緒中，主要思路是在當前幀和（區域性）地圖之間尋找儘可能多的對應關係，來優化當前幀的位姿。每次新採集到一幀影像，就是用下列介面將影像傳入SLAM系統就行處理。該程式碼位於主程式中：

1. // Pass the image to the SLAM system

2. SLAM.TrackMonocular(im,tframe);

在檢查完系統將模式切換為跟蹤模式後，是用下面介面進入功能：

mpTracker->GrabImageMonocular(im,timestamp);
 

• 1

一、ORB提取

  本文做匹配的過程中，是用的都是ORB特徵描述子。先在8層影像金字塔中，提取FAST特徵點。提取特徵點的個數根據影像解析度不同而不同，高解析度的影像提取更多的角點。然後對檢測到的特徵點用ORB來描述，用於之後的匹配和識別。跟蹤這部分主要用了幾種模型：運動模型（Tracking with motion model）、關鍵幀（Tracking with reference keyframe）和重定位（Relocalization）。

二、從前一幀初始化位姿估計

  在成功與前面幀跟蹤上之後，為了提高速率，本文使用與之前速率相同的運動模式來預測相機姿態，並搜尋上一幀觀測到的地圖點。這個模型是假設物體處於勻速運動，例如勻速運動的汽車、機器人、行人等，就可以用上一幀的位姿和速度來估計當前幀的位姿。使用的函式為TrackWithMotionModel()。這裡匹配是通過投影來與上一幀看到的地圖點匹配，使用的是matcher.SearchByProjection()。

1. if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)

2. {

3. bOK = TrackReferenceKeyFrame();

4. }

5. else

6. {

7. bOK = TrackWithMotionModel();

8. if(!bOK)

9. bOK = TrackReferenceKeyFrame();

10. }

當使用運動模式匹配到的特徵點數較少時，就會選用關鍵幀模式。即嘗試和最近一個關鍵幀去做匹配。為了快速匹配，本文利用了bag of words（BoW）來加速。首先，計算當前幀的BoW，並設定初始位姿為上一幀的位姿；其次，根據位姿和BoW詞典來尋找特徵匹配，使用函式matcher.SearchByBoW()；最後，利用匹配的特徵優化位姿。

三、通過全域性重定位來初始化位姿估計

  假如使用上面的方法，當前幀與最近鄰關鍵幀的匹配也失敗了，那麼意味著需要重新定位才能繼續跟蹤。重定位的入口如下：

bOK = Relocalization();
 

此時，只有去和所有關鍵幀匹配，看能否找到合適的位置。首先，計算當前幀的BOW向量，在關鍵幀詞典資料庫中選取若干關鍵幀作為候選。使用函式如下：

1.  

2. // Relocalization is performed when tracking is lost

3. // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation

4. vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);

其次，尋找有足夠多的特徵點匹配的關鍵幀；最後，利用RANSAC迭代，然後使用PnP演算法求解位姿。這一部分也在Tracking::Relocalization() 裡。

四、區域性地圖跟蹤

  通過之前的計算，已經得到一個對位姿的初始估計，我們就能透過投影，從已經生成的地圖點中找到更多的對應關係，來精確結果。函式入口為：

bOK = TrackLocalMap();
 

為了降低複雜度，這裡只是在區域性圖中做投影。區域性地圖中與當前幀有相同點的關鍵幀序列成為K1，在covisibility graph中與K1相鄰的稱為K2。區域性地圖有一個參考關鍵幀Kref∈K1，它與當前幀具有最多共同看到的地圖雲點。針對K1, K2可見的每個地圖雲點，通過如下步驟，在當前幀中進行搜尋: 
（1）將地圖點投影到當前幀上，如果超出影像範圍，就將其捨棄； 
（2）計算當前視線方向向量v與地圖點雲平均視線方向向量n的夾角，捨棄n·v < cos(60°)的點雲； 
（3）計算地圖點到相機中心的距離d，認為[dmin, dmax]是尺度不變的區域，若d不在這個區域，就將其捨棄； 
（4）計算影像的尺度因子，為d/dmin； 
（5）將地圖點的特徵描述子D與還未匹配上的ORB特徵進行比較，根據前面的尺度因子，找到最佳匹配。 
  這樣，相機位姿就能通過匹配所有地圖點，最終被優化。

五、關鍵幀的判斷標準

  最後一步是確定是否將當前幀定為關鍵幀，由於在Local Mapping中，會剔除冗餘關鍵幀，所以我們要儘快插入新的關鍵幀，這樣才能更魯棒。這個部分程式碼為：

1. // Check if we need to insert a new keyframe

2. if(NeedNewKeyFrame())

3. CreateNewKeyFrame();

確定關鍵幀的標準如下： 
（1）在上一個全域性重定位後，又過了20幀； 
（2）區域性建圖閒置，或在上一個關鍵幀插入後，又過了20幀； 
（3)當前幀跟蹤到大於50個點； 
（4）當前幀跟蹤到的比參考關鍵幀少90%。

六、程式碼架構

區域性建圖

在之前的Tracking中，我們得到了新的關鍵幀Ki。如下圖所示，Local Mapping這部分 包括插入關鍵幀，剔除冗餘的地圖點和關鍵幀，還有進行區域性集束調整。接下來按照順序介紹各部分。 

這一部分通過之前例項化SLAM系統物件時，例項化了一個LocalMapping的物件，並且開啟一個執行緒，執行LocalMapping::Run()函式。整個程式碼邏輯如下： 

一、關鍵幀插入

  首先將新的關鍵幀Ki作為新的節點Ki加入Covibility Graph，並且更新與那些能夠共享地圖點的關鍵幀節點相連線的邊。同時更新關鍵幀Ki的生長樹，並計算表示關鍵幀的詞袋BOW。這一部分的介面是在LocalMapping.cc中的

1. // BoW conversion and insertion in Map

2. ProcessNewKeyFrame();

計算詞袋，整合地圖點到新的關鍵幀，計演算法線和描述子的介面如下：

1. // Compute Bags of Words structures

2. mpCurrentKeyFrame->ComputeBoW();

3.  

4. // Associate MapPoints to the new keyframe and update normal and descriptor

5. const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();

更新Covisibility Graph，將關鍵幀插入Map中的介面如下：

1. // Update links in the Covisibility Graph

2. mpCurrentKeyFrame->UpdateConnections();

3.  

4. // Insert Keyframe in Map

5. mpMap->AddKeyFrame(mpCurrentKeyFrame);

二、當前地圖點剔除

  為了儲存地圖點，必須在建立該點雲的前三幀測試通過約束，才能真正被儲存，這樣才能保證可跟蹤且不容易在三角化時出現較大誤差。這部分的介面如下：

1. // Check recent MapPoints

2. MapPointCulling();

一個點要被加入Map，需要滿足下面條件： 
（1）這個點要在可預測到能夠觀察到該點的關鍵幀中，有超過25%的關鍵幀能夠跟蹤到這個點； 
（2）如果一個地圖點被構建，它必須被超過三個關鍵幀觀察到（在程式碼中，可以發現如果是單攝像頭，這個閾值被設定為2）。

  一旦地圖點被建立了，就只有在少於3個關鍵幀能夠觀察到該點時才會被剔除。而要剔除關鍵幀，通常是在區域性集束調整剔除外點或者在後面剔除關鍵幀時才會發生。這樣就保證了地圖點很少存在外點影響效果。

三、新的地圖點建立

  通過將檢測到的ORB特徵點，找到Covisibility Graph中與之相連的關鍵幀Kc，進行特徵匹配，然後將匹配到的特徵點進行三角化。對於沒有匹配上的點，本文又與其他關鍵幀中未被匹配的特徵點進行匹配。匹配方法使用的是之前的方法，並且將不滿足對極幾何約束的匹配點捨棄。ORB特徵點對三角化後，檢查正向景深、視差、反投影誤差和尺度一致性，這時才得到地圖點。一個地圖點是通過兩個關鍵幀觀察到的，而它也可以投影到與之相連的其他關鍵幀中，這個時候可以使用Tracking部分的跟蹤區域性地圖來在附近的關鍵幀中找到匹配。得到更多的地圖點。這部分介面為：

1. // Triangulate new MapPoints

2. CreateNewMapPoints();

3.  

4. if(!CheckNewKeyFrames())

5. {

6. // Find more matches in neighbor keyframes and fuse point duplications

7. SearchInNeighbors();

8. }

四、區域性集束調整

  區域性集束調整（local BA）會將當前處理的關鍵幀Ki進行優化，優化時如下圖所示：現在優化Pos3位置的關鍵幀。同時參與優化的還有： 
所有在Covibility Graph中與該關鍵幀相連的關鍵幀Kc，即下圖中的Pos2； 
所以被這些關鍵幀觀察到的地圖點，即X1和X2。 
另外還有能觀察到地圖點的但並未與當前處理的關鍵幀相連的關鍵幀，即下圖中的Pos1。 
  但要注意的是，諸如Pos1的關鍵幀，參與優化中的約束，但不作為變數去改變它們的值。優化時得到的外點會在優化的中期或後期被剔除。 

這部分的介面如下：

1. // Local BA

2. if(mpMap->KeyFramesInMap()>2)

3. Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

五、區域性關鍵幀剔除

  為了控制重建的緊湊度，LocalMapping會去檢測冗餘的關鍵幀，然後刪除它們。這樣的話會有利於控制，隨著關鍵幀數目增長後，集束調整的複雜度。因為除非視角改變了，否則關鍵幀的數量在相同的環境中不應該無休止地增長。本文將那些有90%的點能夠被超過三個關鍵幀觀察到的關鍵幀認為是冗餘關鍵幀，並將其刪除。這個部分的介面如下：

1. // Check redundant local Keyframes

2. KeyFrameCulling();

最後，在所有步驟結束後，會將關鍵幀記錄到資料庫列表中。完成LocalMapping工作，將標誌設為SetAcceptKeyFrames(true)，以允許Tracking執行緒繼續得到關鍵幀。

閉環檢測

毋庸置疑的是，隨著相機的運動，我們計算的相機位姿，三角化得到的點雲位置，都是有誤差的，即使我們使用區域性的或全域性的BA去優化，仍然會存在累積誤差。而消除誤差最有效的辦法是發現閉環，並根據閉環對所有結果進行優化。閉環是一個比BA更加強烈、更加準確的約束，所有如果能夠檢測到閉環，並對其優化，就可以讓結果更加準確。 
  整個LoopClosing模組是線上程中完成，並在建立執行緒時呼叫LoopClosing::Run()函式讓其執行。整個框架如下圖所示： 

一、閉環條件檢測

  首先我們計算關鍵幀Ki和在Covisibility Graph中與其相連的關鍵幀之間的詞袋(BOW)之間的相似度。本文中，作者離線訓練了大量的基於ORB描述的詞袋，在程式執行時載入進去。這裡的詞袋作為對該關鍵幀的描述，將閉環檢測轉變為一個類似於模式識別的問題。當相機再次來到之前到過的場景時，就會因為看到相同的景物，而得到類似的詞袋描述，從而檢測到閉環。這裡的介面是

1. // Detect loop candidates and check covisibility consistency

2. if(DetectLoop())

ORBSlam2中的閉環檢測和後端優化LoopClosing

LoopClosing在現在的Slam系統中是非常重要的一個部分，VO總是會有累計誤差，而LoopClosing通過檢測是否曾經來過此處，進行後端優化，可以將這個累計誤差縮小到一個可接受的範圍內。從而使得Slam系統應對大範圍場景時，擁有更高的魯棒性和可用性。

ORBSlam2中的LoopClosing閉環檢測執行緒主要進行閉環檢測，並在檢測到閉環的時候計算Sim3變換，進行後端優化。

檢測閉環 DetectLoop

首先我們會檢測當前關鍵幀在Covisibility圖中的附近關鍵幀，並會依次計算當前關鍵幀和每一個附近關鍵幀的BoW分值，通過我們所得到分數的最低分，到資料庫中查詢，查詢出所有大於該最低分的關鍵幀作為候選幀，用以檢測閉環。

1. 如果地圖中的關鍵幀數小於10，那麼不進行閉環檢測

2. 獲取共視關鍵幀，並計算他們和當前關鍵幀之間的BoW分數，求得最低分

3. 通過上一步計算出的最低分數到資料庫中查詢出候選關鍵幀，這一步相當於是找到了曾經到過此處的關鍵幀們

這一步是非常重要的，相當於是在為閉環檢測做前期的預處理

1. 首先，得到與當前幀的連結的關鍵幀。 
   set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();

2. 然後，在地圖中搜尋與當前關鍵幀共享一個BOW word的關鍵幀，並排除上一步蒐集到的附近關鍵幀，得到候選幀，這些候選幀基本上都是曾經來到此處看到的。 
   

3. 統計這些幀中，與當前關鍵幀的Bow共有Word最多的單詞數maxCommonWords。 
   

4. 計算最低共有單詞數閾值minCommonWords = maxCommonWords*0.8f，並搜尋候選幀中，共有單詞數大於minCommonWords的關鍵幀，並計算它與當前幀的score分值。 
   

5. 將這些候選幀進行聚類，相連的候選幀聚為同一類，並計算每一組的累計得分，得到最高累計分的組，並得分數最高組的最高分的關鍵幀。 
   這樣的話，會把一些擁有很高分數的獨立出來的關鍵幀給去掉，因為他並沒有跟其他關鍵幀相連，沒有連續性，所以這一步就是去除這些得分很高的錯誤關鍵幀。 
   

6. 得到最後一個閾值 minScoreToRetain = 0.75f*bestAccScore，再通過這個最低閾值，計算出這些候選幀中比這個分值高的關鍵幀，並儲存起來返回。 
   

4. 對候選關鍵幀集進行連續性檢測(有的文章也翻譯為一致性檢測)

這一步相當於是檢測上一步得到的關鍵幀集是否是真的可以使用的。 
（由於這段網路上很少有介紹，所以這部分我是按照我自己的理解寫的，如果有錯誤，希望有人能夠指出） 

閉環連續性是什麼

每次我們在上一步的資料庫查詢操作裡找到了候選關鍵幀之後，基本上找到的候選關鍵幀就是我們所要找的閉環關鍵幀，但是為了防止錯誤進行閉環檢測，我們非常有必要再進行一次連續性檢測，連續性檢測的意思就是，是否我們在三個當前的關鍵幀內都同時發現了某一個閉環候選幀的話，那麼就表明當前的SLAM系統已經閉環。

詳細解釋

比方說，在上圖中，通過資料庫查詢，我們可以在A點得到閉環候選關鍵幀有兩個(1,2)。在下一次進入DetectLoop函式的時候，我們當前拿到的關鍵幀是B，那麼在B點我們可以得到的閉環候選關鍵幀是(1,2,3)，以此類推，在再下一次進入DetectLoop函式的時候，也就是在C點的時候，我們這時對比到的閉環候選關鍵幀是(2,3)，所以2這個閉環候選關鍵幀被檢測到了三次。在LoopClosing中，mnCovisibilityConsistencyTh = 3 一致性共視閾值被設為3，並且如果一旦有一個閉環候選關鍵幀被檢測到3次，系統就認為檢測到閉環。

在每一次進行完閉環候選連續性檢測之後，該執行緒都會儲存在這一關鍵幀下的計數情況，儲存的變數也就是mvConsistentGroups，以供給下一輪迴圈使用。

計算Sim3 ComputeSim3

既然上一步我們已經檢測到了閉環，那麼我們現在就需要開始進行後端優化了。該函式主要工作就是在當前關鍵幀和閉環幀之間找到更多的對應點，並通過這些對應點計算當前關鍵幀和閉環幀之間的Sim3變換，求解出Rt和s。在這一過程中，共進行了三次對應點的查詢工作。

1. 對每一個閉環幀，通過BoW的matcher方法進行第一次匹配，匹配閉環幀和當前關鍵幀之間的匹配關係，如果對應關係少於20個，則丟棄，否則構造一個Sim3求解器並儲存起來。這一步主要是對效果較好的閉環幀構建Sim3求解器 
   

2. 對上一步得到的每一個滿足條件的閉環幀，通過RANSAC迭代，求解Sim3。 
   對於這裡的Sim3的Ransac迭代，我在網上也沒有找到很多的解釋，我們都知道Ransac是根據一組包含異常資料的樣本資料集，計算出資料的數學模型引數的方法，在這裡使用Ransac的方法，可以大大提高對雜點干擾的魯棒性。 
   

3. 通過返回的Sim3進行第二次匹配。 
   剛才得到了Sim3，所以現在要利用Sim3再去進行匹配點的查詢，本次查詢的匹配點數量，會在原來的基礎上有所增加。 
   

4. 使用非線性最小二乘法優化Sim3. 
   在拿到了第二次匹配的結果以後，要通過這些匹配點，再去優化Sim3的值，從而再精細化Rt和s。 
   const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);

5. 恢復閉環關鍵幀和其鄰居關鍵幀的MapPoint地圖點 
   最後一步求解匹配點的時候，將所指的閉環幀和與其連結的關鍵幀所看到的所有的MapPoint都恢復出來。通過這個方法，可以儘可能得到我們當前關鍵幀所能看到的所有的地圖點，為下一步做投影匹配，得到更多的匹配點做準備。 
   

6. 使用投影得到更多的匹配點，如果匹配點數量充足，則接受該閉環。 
   最後我們通過投影匹配得到了儘可能多的匹配點，通過匹配點的數量判斷是否接受閉環。 
   

糾正閉環後端優化 CorrectLoop

在上一步求得了Sim3和對應點之後，就糾正了當前幀的位姿，但是我們的誤差不僅僅在當前幀，此前的每一幀都有累計誤差需要消除，所以這個函式CorrectLoop就是用來消除這個累計誤差，進行整體的調節。

1. 如果有全域性BA運算在執行的話，終止之前的BA運算。 
   

2. 使用傳播法計算每一個關鍵幀正確的Sim3變換值

   • 得到當前關鍵幀的附近關鍵幀的Sim3位姿並用糾正的Sim3位姿與其相乘，儲存結果到CorrectedSim3變數中。 
     

   • 使用反向投影的方法，將當前關鍵幀和鄰居觀察到的地圖點得到三維場景下的位姿，並更新關鍵幀的位姿 
     

   • 將當前關鍵幀的地圖點進行融合，其實融合就是判斷如果是同一個點的話，那麼將當前的地圖點強制換成原本的地圖點。 
     

   • 使用已糾正的位姿，將在迴圈關鍵幀附近觀察到的地圖點投影到當前的關鍵幀和鄰居，融合重複點。 
     

   • 更新連結，檢測新連線 
     

3. 優化圖 
   Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale); 
   使用非線性最小二乘圖優化的方法來優化EssentialGraph。

4. 全域性BA優化 
   

轉自http://blog.csdn.net/u010128736/

轉自https://blog.csdn.net/chishuideyu/article/details/76165461