ORB-SLAM2代碼（三）定位

參考：orb-slam2源碼註釋版

---

上一講到了SLAM中的建圖（其本質是對MapPoint的管理），有了地圖，我們就有了環境的struct，這樣我們就可以通過特徵點匹配、優化等一系列步驟進行定位了，這次將對orb-slam中的定位部分進行解析，其本質是對frame的處理，包括關鍵幀和普通幀.

---

1、設置當前幀的參考關鍵幀

在設置當前幀的參考關鍵幀時（mCurrentFrame.mpReferenceKF），一般都會同時設置tracking線程的參考關鍵幀（mpReferenceKF）. 涉及到的地方有：
1）初始化完成時.
2）在跟蹤局部地圖之前，使用上一次普通幀跟蹤時生成的mpReferenceKF設置當前幀的參考關鍵幀.

mCurrentFrame.mpReferenceKF = mpReferenceKF;

3）TrackLocalMap() 函數，UpdateLocalKeyFrames() 函數選擇共視程度最高的關鍵幀作爲當前幀的參考關鍵幀. 注意，這裏選擇的並非時間上最近的，而是共視程度最高的.

if (pKFmax) {
	mpReferenceKF = pKFmax;
	mCurrentFrame.mpReferenceKF = mpReferenceKF;
}

4）CreateNewKeyFrame() 函數，使用新生成的關鍵幀設置

KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
mpReferenceKF = pKF;
mCurrentFrame.mpReferenceKF = pKF;

---

2、更新上一普通幀的位姿

在 TrackWithMotionModel() 函數， UpdateLastFrame() 函數， 使用上一普通幀的參考關鍵幀，更新上一幀的位姿，這裏主要是考慮到上一普通幀的參考關鍵幀會在local mapping中優化後改變，所以才這麼做的.

// Update pose according to reference keyframe
KeyFrame* pRef = mLastFrame.mpReferenceKF;
cv::Mat Tlr = mlRelativeFramePoses.back();
mLastFrame.SetPose(Tlr*pRef->GetPose());

---

3、生成關鍵幀

NeedNewKeyFrame() 函數用於判斷是否需要插入關鍵幀，下面對需要滿足的條件進行分析
條件1：
local Mapping 是否被 Loop Closure 凍結，關於線程調度，可以參考之前的博客：orb-slam2代碼總結（一）線程調度，代碼如下：

if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
        return false;

條件2：
條件2-1：
首先地圖中的關鍵幀的數量大於mMaxFrames（等於相機的幀率，比如，30Hz），即從初始化開始後插入到Map中關鍵幀數目必須大於30，該條件才能被使能，也就說，如果當前Map中的關鍵幀數目少於30幀，那麼這個條件永遠不會成立，即插入關鍵幀不受該條件2的影響
條件2-2：
當Map中的關鍵幀數目多於30個時，那麼整個條件2是否成立只與第一部分有關了，如果之前發生過重定位，那麼需要在一秒之內，是肯定不允許插入關鍵幀的！

if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)
        return false;

條件3:
條件3-1：
由下面三部分構成，下面三個條件是或的關係，所以只要一個成立，3-1就成立了
條件3-1-1：一秒鐘過去了，還沒有新的關鍵幀被插入，就需要考慮插入關鍵幀了

// 【Condition 1a】: More than "MaxFrames" have passed from last keyframe insertion
const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames; 

條件3-1-2：local mapping處於空閒狀態，即現在該線程正在進行休眠，這個時候插入關鍵幀不會對local mapping中的 CheckNewKeyFrames() 函數造成影響

// 【Condition 1b】: More than "MinFrames" have passed and Local Mapping is idle                                                                      
const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);  

條件3-1-3：這個條件是爲雙目打造的，mnMatchesInliers是track with local map中匹配到的MapPoint數目，nRefMatches是當前幀的參考關鍵幀（即，與當前幀共視最多的關鍵幀）中所有MapPoint中nObs超過3的MapPoints（即表示那些穩定的MapPoint），
mnMatchesInliers<nRefMatches*0.25條件成立，表示跟蹤到的MapPoint數目已經不及關鍵幀中穩定的MapPoint數目的四分之一
ratioMap<0.3f表示 $"matches to map"/"total matches"$，其中total matches= matches to map + visual odometry matches，即總數等於當前幀與地圖中的MapPoint匹配數加上雙目自己新生成的MapPoint.

// 【Condition 1c】: tracking is weak                                                                                                                                                          
const bool c1c =  mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || ratioMap<0.3f); 

條件3-2：
這個條件分成兩部分，
當前幀匹配到的Map Point的個數不能超過參考關鍵幀（與當前幀擁有最多map point的關鍵幀）對應Map Point的90%
當前的幀匹配到的Map Point數目大於15

// 【Condition 2】: Few tracked points compared to reference keyframe. Lots of visual odometry compared to m                                                                           
const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio || ratioMap<thMapRatio) && mnMatchesInliers>15);

---

4、更新關鍵幀之間共視圖

每一個關鍵幀都會維護一個共視圖，根據共視程度，共視圖包括下面三種：

1） Covisibility Graph （閾值15），主要用於：

• local mapping線程中的 LocalBundleAdjustment() 優化，根據當前關鍵幀選擇local map，即局部地圖
• SearchInNeighbors() 中用於fuse時，從當前關鍵幀的Covisibility Graph 選擇前20個（雙目是10個）作爲一級相鄰
• 剔除關鍵幀，選取當前關鍵幀所有Covisibility Graph，對共視的關鍵幀進行篩選

2） Essential Graph（閾值100），用於閉環檢測，建立pose graph，做位姿圖優化，將累積誤差分配到整個位姿圖中.
3）Spanning Tree（共視程度最高的作爲父節點），即，最大生成樹，數據結構裏的內容，這裏表示與新加入的關鍵幀共視程度最高的那個關鍵幀，將其作爲這個新加入的關鍵幀的父節點mpParent，這個在 SaveTrajectoryKITTI() 函數中會用，用於在存儲普通幀的位姿時，尋找這個所依靠的沒被剔除的關鍵幀

更新關鍵幀之間共視圖實現在UpdateConnections() 函數和 AddConnection() 兩個函數，調用的地方有：

• tracking線程，初始化完成時
• local mapping線程，ProcessNewKeyFrame() 函數和 SearchInNeighbors() 函數
• loop close線程，CorrectLoop() 函數

一般來說，在使用到pKF->GetVectorCovisibleKeyFrames()之前都要先進行UpdateConnections()，這樣返回的mvpOrderedConnectedKeyFrames纔是當前的情況，因爲增加和減少nObs屬性（參考上一篇博客）都會影響共視圖.

---

5、關鍵幀的剔除

操作對象是mpCurrentKeyFrame的共視關鍵幀，即Covisibility Graph（閾值大於15），如果這個局部關鍵幀看到MapPoints中能夠被其它至少三個關鍵幀也觀測到MapPoints所佔的比例多餘90%時，就剔除該關鍵幀. 中間有個細節要注意下，在搜尋這個MapPoint對應的observations時，要對尺度進行進行檢查，尺度儘量不要高於這個MapPoint在這個局部關鍵幀中的尺度，這樣做是因爲，特徵點的尺度越高，精度越低，誤匹配的概率越大.

---

<完>
@leatherwang

---