最近在讀ORB-SLAM的代碼，雖然代碼注釋算比較多了，但各種類和變量互相引用，看起來有點痛苦。索性總結了一下Tracking部分的代碼結構，希望能抓住主要思路，不掉坑里。
作者的程序分為兩種模式：SLAM模式和Localization模式。SLAM模式中，三個線程全部都在工作，即在定位也在建圖。而Localization模式中，只有Tracking線程在工作，即只定位，輸出追蹤結果（姿態），不會更新地圖和關鍵幀。Localization模式主要用於已經有場景地圖的情況下（在SLAM模式下完成建圖后可以無縫切換到Localization模式）。Localization模式下追蹤方法涉及到的關鍵函數是一樣的，只是策略有所不同。
以下介紹SLAM模式下的追蹤算法。
主要流程
初始追蹤（Init pose estimation）
作者在追蹤這部分主要用了幾種模型：運動模型（Tracking with motion model）、關鍵幀（Tracking with reference key frame）和重定位（Relocalization）。
下面一一介紹。
Tracking with motion model
假設物體處於勻速運動，那么可以用上一幀的位姿和速度來估計當前幀的位姿。上一幀的速度可以通過前面幾幀的位姿計算得到。這個模型適用於運動速度和方向比較一致，沒有大轉動的情形下，比如勻速運動的汽車、機器人、人等。而對於運動比較隨意的目標，當然就會失效了。此時就要用到下面兩個模型。
Tracking with reference key frame
假如motion model已經失效，那么首先可以嘗試和最近一個關鍵幀去做匹配。畢竟當前幀和上一個關鍵幀的距離還不是很遠。作者利用了bag of words（BoW）來加速匹配。首先，計算當前幀的BoW，並設定初始位姿為上一幀的位姿；其次，根據位姿和BoW詞典來尋找特征匹配（參見ORB－SLAM（六）回環檢測）；最后，利用匹配的特征優化位姿（參見ORB－SLAM（五）優化）。
Relocalization
假如當前幀與最近鄰關鍵幀的匹配也失敗了，意味着此時當前幀已經丟了，無法確定其真實位置。此時，只有去和所有關鍵幀匹配，看能否找到合適的位置。首先，計算當前幀的Bow向量。其次，利用BoW詞典選取若干關鍵幀作為備選（參見ORB－SLAM（六）回環檢測）；再次，尋找有足夠多的特征點匹配的關鍵幀；最后，利用特征點匹配迭代求解位姿（RANSAC框架下，因為相對位姿可能比較大，局外點會比較多）。如果有關鍵幀有足夠多的內點，那么選取該關鍵幀優化出的位姿。
姿態優化（Track local map）
姿態優化部分的主要思路是在當前幀和（局部）地圖之間尋找盡可能多的對應關系，來優化當前幀的位姿。實際程序中，作者選取了非常多的關鍵幀和地圖點。在跑Euroc數據集MH_01_easy時，幾乎有一半以上的關鍵幀和地圖點（后期>3000個）會在這一步被選中。然而，每一幀中只有200~300個地圖點可以在當前幀上找到特征匹配點。這一步保證了非關鍵幀姿態估計的精度和魯棒性。個人覺得這里有一定的優化空間。
更新局部地圖（Local mapping thread）
這里簡單提一下local mapping thread。Tracking成功以后，需要更新motion model，並判斷當前幀是否是新的關鍵幀。如果是，將其加入並更新局部地圖（local map），建立當前關鍵幀與其它關鍵幀的連接關系，更新當前關鍵幀與其它關鍵幀之間的特征點匹配關系，並利用三角法生成新的三維點，最后做一個局部優化（local BA，包括相鄰關鍵幀和它們對應的三維點，參見ORB－SLAM（五）優化）。
程序
作者的這部分程序邏輯判斷很多，如果讀者對作者的程序有興趣，那么可以參照下圖來閱讀追蹤這部分的程序。
Map is active應改為Localization is active。從Localization is active開始，NO代表SLAM模式，YES代表Localization模式。
該系列的其它文章：