orb_slam2易忘知識點總結，改代碼關掉局部BA,全局BA

ORB寫的好的博客：

https://www.cnblogs.com/wall-e2/p/8057448.html

https://www.cnblogs.com/mafuqiang/p/6932679.html

orb_slam2有註釋的代碼：https://gitee.com/paopaoslam/ORB-SLAM2

TRACKING 線程

• 地圖初始化：適用於平面場景的單應矩陣H和適用於非平面場景的基礎矩陣F，然後根據一些具體的方法來選擇一個模型。

       從前一幀估計位姿：先利用運動學模型與上一幀匹配，如果數量少，則與關鍵幀匹配，利用詞袋模型加速。

• 局部地圖跟蹤：是在上步運行完，當前幀與局部地圖能夠尋找更多匹配點，匹配所有地圖點，然後被優化。

  FUCTION2.2.3.1.1:TrackLocalMap

  local map意思是當前幀、當前幀的MapPoints、當前關鍵幀與其它關鍵幀共視關係，1. 更新局部地圖，包括局部關鍵幀和關鍵點、2. 對局部MapPoints進行投影匹配、3. 根據匹配對估計當前幀的姿態、4. 根據姿態剔除誤匹配。

  motion-only BA：有時候意思是計算單幀的重投影誤差。

• 重定位：計算當前幀的詞袋向量與之前關鍵幀的詞典數據比較，利用PNP+RANSAC 求出位姿。

• 正式跟蹤的步驟：

• 1.先判斷mVelocity（之前求得的“t-2”,"t-1"兩幀之間的位姿變換）速度模型是否計算成功，如果成功就用速度模型，這時獲得當前幀的一個初始位姿了，然後當前幀與上一幀進行匹配意思是將上一幀特徵點對應3Dmappoints投影到當前幀，其中用到了project加速匹配，用的是3D點的描述子。

• 2.如果速度模型沒有成功，或者成功了但是匹配失敗了，則使用TrackReferenceKeyframe（）,位姿初值就用上一幀的，通過當前幀與參考關鍵幀進行匹配（使用了BOW加速），優化每個特徵點對應的3D重投影誤差得到當前幀的位姿。參考關鍵幀就選與當前幀共視程度最高的關鍵幀。

• 3.重定位：計算與當前幀具有公共單詞數的關鍵幀，計算當前幀與這些關鍵幀的相似度得到一個候選幀向量，然後對每一幀進行BOW加速匹配，如果匹配點對超過15，則調用SolvePnpRansac,一直迭代直到候選幀數爲0。

• 4.TrackLocalMap(),當前幀能看到的所有地圖點爲MapPoints，遍歷MapPoints,找到同樣能觀測到它的關鍵幀和相鄰關鍵幀（相鄰的意思是與有共識關係幀的那些最佳共視的關鍵幀）上述這些關鍵幀及其Mappoints作爲LocalMap。遍歷局部地圖中的所有地圖點（但要去除掉MapPoints，因爲已經有了），判斷1.其投影過去是否在當前幀的圖像範圍內，2.計算MapPoint到相機中心的距離是否在尺度範圍內，3.計算MapPoint視角與平局場景視角，如果角度小則可以。對滿足上述三條的點，通過投影加速（重投影加速中包括，金字塔層級的剔除外點，和方向直方圖的剔除外帶逆），與當前幀的特徵點進行匹配，做只改變當前幀位姿的優化。

• 5.收尾工作：NeedNewKeyFrame():如下面關鍵幀判別標準。

LOCAL MAPPING 線程

• 地圖點的刪除：爲了保存地圖點，必須在創建該點雲的前三幀測試通過約束，才能真正被保存，這樣才能保證可跟蹤且不容易在三角化時出現較大誤差。一個點要被加入Map，需要滿足下面條件： 

（1）這個點要在可預測到能夠觀察到該點的關鍵幀中，有超過25%的關鍵幀能夠跟蹤到這個點； 
（2）如果一個地圖點被構建，它必須被超過三個關鍵幀觀察到（在代碼中，可以發現如果是單攝像頭，這個閾值被設置爲2）。

• 新的地圖點的創建：

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

• 關鍵幀的判別標準：

1. 在上一個全局重定位後，又過了20幀； 
2. 局部建圖閒置，或在上一個關鍵幀插入後，又過了20幀； 
3. 當前幀跟蹤到大於50個點； 
4. 當前幀跟蹤到的比參考關鍵幀少90%
5. localMapper處於空閒狀態

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

• 關鍵幀剔除標準KeyFrameCulling：

1. 冗餘地圖點數目佔地圖點數目超過90%的局部關鍵幀
2. 冗餘地圖點：

1)被超過3個其他關鍵幀觀測到

遍歷該局部關鍵幀的MapPoints，判斷是否90%以上的MapPoints能被其它關鍵幀（至少3個）觀測到，該局部關鍵幀90%以上的MapPoints能被其它關鍵幀（至少3個）觀測到，則認爲是冗餘關鍵幀。

• 關鍵幀刪除

1. (1)剔除和地圖點之間的關聯關係
2. (2)更新連接圖、Essential Graph和生成樹

 

共視圖(covisibility graph)：將每個關鍵幀作爲一個節點，若兩個關鍵幀存在一定數量（至少15個）共視地圖點則這兩個這兩個關鍵幀之間存在一條邊，邊的權重爲共視地圖點的數量。

本質圖(essential graph)：使用位姿圖來校正閉環處的累積誤差，由於共視圖中邊的數量較多，我們創建共視圖的子圖-本質圖。本質圖包含生成樹、共視地圖點數量超過100的邊和迴環的邊

生成樹(spanning tree)：ORB-SLAM系統在第一個關鍵幀開始就維護一個生成樹，每個關鍵幀只與共視點最多的相鄰幀產生邊。本質圖包含生成樹、共視地圖點數量超過100的邊和迴環的邊。

• 局部集束調整

LOOP CLOSING 線程

• 步驟

1.  

• loop candidates detection

首先計算 KiKi 和 convisibility graph 相連的幀最小相似度 sminsmin，迴環檢測時在 recognition database 中去掉相似度小於 sminsmin 的幀，並且去掉 convisibility graph 中和當前幀相連的幀。在判斷某一幀爲迴環時，還需要判斷連續的 3 幀都爲迴環（To accept a loop candidate we must detect consecutively three loop candidates that are consistent (keyframes connected in the covisibility graph).）

單目的 SLAM 中，重建的地圖在旋轉，平移和尺度上都可以有漂移，所以在 close a loop 時，需要計算當前幀和迴環幀之間的相似變換。首先計算當前幀和迴環幀之間的匹配點，匹配點用 vocabulary tree 搜索，找到的匹配點是 3D-3D 的點，然後用 RANSAC 算法對這這些點計算一個相似變換，對於得到的相似變換如果能有足夠的 inliers，則接受迴環。

• Loop Fusion

迴環的地方，地圖會被重建多次，所以在 loop fusion 時，需要把迴環兩端的地圖融合在一起，並且迴環兩端的 convisibility graph 也需要添加新的邊。 
首先將計算的相似變換作用到當前幀，並且傳播到和當前幀連接的關鍵幀，通過這樣做迴環兩端的地圖可以對齊。 
然後迴環幀，和迴環幀相連的關鍵幀的所有地圖點，向當前幀，和當前幀相連接的關鍵幀投影尋找匹配點，所有的這些匹配點和計算相似變換時的匹配點進行融合，融合會使得迴環兩端的關鍵幀建立新的共視點，從而需要 convisibility graph 中更新關鍵幀之間的邊連接。

Essential Graph Optimization

Essential Graph /pose graph Optimization：To effectively close the loop, we perform a pose graph optimization over the Essential Graph，對於單目 SLAM 優化是針對相似變換做的，優化後地圖點根據能觀測到它的一幀關鍵幀進行位置的更新。

優化的參數：每個關鍵幀的 pose，優化參數 pose 是個相似變換，優化完後再作用到關鍵幀位姿，作用後，關鍵幀位姿仍然是個歐式變換，優化後也作用到地圖點。

優化約束 
這裏的約束都是位姿約束，位姿約束指幀和幀之間的位姿變換。 
約束有： 
當前迴環兩端新建立的邊（迴環兩端地圖點融合產生連接回環兩端新邊）形成的位姿約束。 
spanning tree 位姿約束。 
每個關鍵幀的迴環約束。 
convisibility graph 中權值大於一定閾值的邊約束。

Global BA

Global BA：優化地圖中所有關鍵幀的 pose 和所有地圖點的座標。優化的變量包括所有關鍵幀 pose，所有特徵點座標，優化的約束是所有特徵點所有觀測的投影誤差。

代碼部分:

如果在./build_ros.sh 中，export了後仍然不好使，則參考：http://86rev0.smartapps.cn/pages/blog/article-detail?userName=Robot_Starscream&articleId=90245477    就ok了

關掉局部BA需要註釋掉LocalMapping.cc的第80行和81行，即：

// Local BA
                if(mpMap->KeyFramesInMap()>2)
                    Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

關掉迴環需要註釋掉LoopClosing.cc中的第71-75行：

               if(ComputeSim3())
               {
                   // Perform loop fusion and pose graph optimization
                   CorrectLoop();
               }

關掉全局BA需要註釋掉LoopClosing.cc中的第579行：

mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);

   關掉生成的點雲，Tracking.cc 註釋掉

// insert Key Frame into point cloud viewer
    //mpPointCloudMapping->insertKeyFrame( pKF, this->mImRGB, this->mImDepth );

system.cc 裏 mpPointCloudMapping 關鍵字的句子註釋掉，搜pointcloud註釋掉