ORB-SLAM2源碼學習（六）：相機跟蹤（局部地圖跟蹤和關鍵幀創建）

1.局部地圖跟蹤

1.1?更新局部關鍵幀UpdateLocalKeyFrames

1.2 更新局部地圖點（來自局部關鍵幀）UpdateLocalPoints()

1.3 投影匹配

2. 對比四種跟蹤方式以及使用的投影匹配

3.關鍵幀創建

3.1 判斷是否需要創建新關鍵幀: NeedNewKeyFrame()

3.2 關鍵幀的創建CreateNewKeyFrame()

1.局部地圖跟蹤

當初始位姿估計成功（參考關鍵幀來跟蹤、恒速模型跟蹤、重定位跟蹤跟蹤成功）后需要進行局部地圖跟蹤TrackLocalMap()。

初始位姿估計只是跟蹤一幀得到初始位姿，局部地圖跟蹤搜索局部關鍵幀、局部地圖點，和當前幀進行投影匹配，得到更多匹配的MapPoints后進行位姿優化。

bOK = TrackLocalMap();

具體流程：

更新局部地圖，包括局部關鍵幀和地圖點;UpdateLocalKeyFrames()?UpdateLocalPoints()
對局部MapPoints進行投影匹配?SearchLocalPoints()
BA優化位姿（和初始位姿估計調用優化函數相同）

1.1?更新局部關鍵幀UpdateLocalKeyFrames

一級共視關鍵幀：KF1、KF2是F的一級共視關鍵幀
二級共視關鍵幀：KF1、KF2的共視關鍵幀(虛線框內)是F的二級共視關鍵幀
當前幀：mCurrentFrame
參考關鍵幀: 與當前幀共視程度最高的關鍵幀作為參考關鍵幀，mCurrentFrame.mpReferenceKF 在KeyFrame::UpdateConnections() 里確定關鍵幀的父子關系
父關鍵幀：和當前關鍵幀共視程度最高的關鍵幀
子關鍵幀：是上述父關鍵幀的子關鍵幀

局部關鍵幀的組成：

當前地圖點的所有共視關鍵幀（鄰居）。
1中所有關鍵幀共視關系前10大的共視關鍵幀（鄰居的鄰居）。
1中所有關鍵幀的父子關鍵幀（鄰居的父母和孩子）。

最后將與當前幀共視關系最強的關鍵幀設為參考關鍵幀mpReferenceKF

1.2 更新局部地圖點（來自局部關鍵幀）UpdateLocalPoints()

局部關鍵幀能夠觀測到的所有地圖點，稱為局部地圖點。

1.3 投影匹配

將局部地圖點進行投影匹配，得到更多的匹配關系(目的)。注意，局部地圖點中已經是當前幀地圖點的不需要再投影，只需要將此外的并且在視野范圍內的點和當前幀進行投影匹配。

具體流程：

遍歷當前幀的地圖點，標記這些地圖點不參與之后的投影搜索匹配；
判斷所有局部地圖點中除當前幀地圖點外的點，是否在當前幀視野范圍內；（是否在視野范圍內有個單獨函數 isInFrustum()）
如果需要進行投影匹配的點的數目大于0，就進行投影匹配，增加更多的匹配關系。SearchByProjection()

2. 對比四種跟蹤方式以及使用的投影匹配

①參考關鍵幀跟蹤

通過詞袋模型計算參考關鍵幀和當前幀的匹配特征點（沒有用投影匹配），上一幀的位姿作為當前幀的初始位姿，然后進行BA求兩幀之間的位姿變換。

應用場景：沒有速度信息的時候、剛完成重定位、或者恒速模型跟蹤失敗后使用，大部分時間不用。只利用到了參考幀的信息（本質上是嘗試和最近一個關鍵幀去做匹配）。

②恒速模型跟蹤

首先根據上上幀到上一幀的位姿，把上一幀的特征點像素坐標映射到當前幀，然后在映射后結果坐標周圍進行搜索，找到與之匹配的特征點；根據速度得到當前幀的初始位姿，然后進行BA求兩幀之間的位姿變換。

使用了投影匹配，由于恒速模型可以計算當前幀相對于上一幀的平移向量，根據相機的前進/后退來約束搜索尺度范圍（在投影點多大的范圍內進行匹配）。此外它是對上一幀的有效地圖點（過濾外點）進行投影的。

投影匹配流程：1. 計算幀間平移 → 2. 投影上一幀地圖點 → 3. 運動方向約束搜索尺度 → 4. 匹配并記錄角度差 → 5. 旋轉直方圖剔除

尺度范圍：

在進行投影匹配的時候會給定特征點的搜索范圍,考慮到處于不同尺度(也就是距離相機遠近,位于圖像金字塔中不同圖層)的特征點受到相機旋轉的影響不同，因此會希望距離相機近的點的搜索范圍更大一點,距離相機更遠的點的搜索范圍更小一點,所以要在這里,根據點到關鍵幀/幀的距離來估計它在當前的關鍵幀/幀中，會大概處于哪個尺度。

應用場景

大部分時間都用這個跟蹤，只利用到了上一幀的信息。

③重定位

當TrackWithMotionModel 和 TrackReferenceKeyFrame 都沒有跟蹤成功，位置丟失后，需要在之前的關鍵幀中匹配相近的關鍵幀，然后使用3D-2D匹配點的 EPnP算法求初始位姿，之后再使用BA來優化位姿。

具體而言：用詞袋找到與當前幀相似的候選關鍵幀（可能找到多個），再然后通過BoW加速計算每一個候選關鍵幀和當前幀的匹配特征點（這個過程和?參考關鍵幀跟蹤中當前幀和參考幀匹配特征點一樣），使用EPnP依次求初始位姿。每個位姿結果使用BA優化，選擇內點數量大于50的作為初始位姿。

如果內點數量不夠多呢？就通過投影匹配方式對之前未匹配的點進行匹配，再進行BA優化求解。由于進行重定位，上一幀沒有意義，基于地圖點距離預測尺度范圍。

投影匹配流程：1. 遍歷局部關鍵幀地圖點 → 2. 過濾已匹配點 → 3. 投影并預測尺度 → 4. 多層級搜索 → 5. 最佳匹配篩選 → 6. 旋轉直方圖剔除

④局部地圖跟蹤

搜索局部關鍵幀、局部地圖點，和當前幀進行投影匹配，得到更多匹配的地圖點后進行位姿優化，初始位姿根據前面三種方法計算的到。

具體而言：根據當前幀的地圖點來找能觀測到當前幀的一級共視關鍵幀，將這些一級共視關鍵幀的二級關鍵共視幀、子關鍵幀、父關鍵幀一起作為局部關鍵幀；局部關鍵幀中所有的地圖點作為局部地圖點；局部地圖點（去掉當前幀的地圖點和不在當前幀視野范圍內無效的地圖點）投影到當前幀進行特征點匹配，匹配結果進行BA優化。

投影匹配的時候通過視角余弦（RadiusByViewingCos）和預測尺度（nPredictedLevel）調整搜索尺度范圍。

總結：

四種跟蹤方式對比：

相同點：本質上都是計算匹配點對以及初始位姿，然后使用BA進行位姿優化。

不同點：求解匹配特征點方法和當前幀的初始位姿設置不同

參考關鍵幀跟蹤通過BoW計算當前幀和參考關鍵幀的匹配點對；初始位姿使用上一幀的位姿；
恒速運動模型通過投影匹配計算匹配點對；初始位姿通過速度（這里的速度是上一幀到當前幀的位姿Tcl）確定；
重定位通過BoW計算每一個相似關鍵幀和當前幀的匹配點對；初始位姿通過EPnP計算；
局部地圖跟蹤是將局部地圖點（來自局部關鍵幀）投影到當前幀計算匹配點對；初始位姿由前面三種方法確定。

3.關鍵幀創建

關鍵幀是取局部相近幀中最有代表性的一幀

選取的指標主要有：

距離上一關鍵幀的幀數是否足夠多（時間）。比如我每隔固定幀數選擇一個關鍵幀，這樣編程簡單但效果不好。比如運動很慢的時候，就會選擇大量相似的關鍵幀，冗余，運動快的時候又丟失了很多重要的幀。
距離最近關鍵幀的距離是否足夠遠（空間）/運動。比如相鄰幀根據pose計算運動的相對大小，可以是位移也可以是旋轉或者兩個都考慮，運動足夠大（超過一定閾值）就新建一個關鍵幀，這種方法比第一種好。但問題是如果對著同一個物體來回掃就會出現大量相似關鍵幀。
跟蹤局部地圖質量（共視特征點數目）。記錄當前視角下跟蹤的特征點數或者比例，當相機離開當前場景時（雙目或比例明顯降低）才會新建關鍵幀，避免了第2種方法的問題。缺點是數據結構和邏輯比較復雜。

if(NeedNewKeyFrame())CreateNewKeyFrame();

3.1 判斷是否需要創建新關鍵幀: NeedNewKeyFrame()

是否生成關鍵幀,需要考慮以下幾個方面:

最近是否進行過重定位，重定位后位姿不會太準，不適合做參考幀；
當前系統的工作狀態: 如果LocalMapping線程還有很多KeyFrame沒處理的話,不適合再給它增加負擔了；
距離上次創建關鍵幀經過的時間: 如果很長時間沒創建關鍵幀了的話,就要抓緊創建關鍵幀了；
當前幀的質量: 當前幀觀測到的地圖點要足夠多，同時與參考關鍵幀的重合程度不能太大。

3.2 關鍵幀的創建CreateNewKeyFrame()

注：雙目和RGBD創建新的地圖點，單目不會（單幀沒有尺度）

具體流程：

將當前幀構造成關鍵幀；
將當前關鍵幀設置為當前幀的參考關鍵幀；
對于雙目或rgbd攝像頭，為當前幀生成新的地圖點；單目無操作；
關鍵幀插入到鏈表 mlNewKeyFrames中，等待local mapping線程處理

思維導圖：