版權聲明：本文為博主原創文章，未經博主允許不得轉載。

　　本系列文章旨在總結主流視覺SLAM算法的框架，對比各個算法在子模塊的差異，最終提煉出融合各個算法優點的架構。

　　PTAM[1]是視覺SLAM領域裏程碑式的項目。在此之前，MonoSLAM[2]為代表的基於卡爾曼濾波的算法架構是主流，它用單個線程逐幀更新相機位置姿態和地圖。地圖更新的計算復雜度很高，為了做到實時處理（30Hz），MonoSLAM每幀圖片只能用濾波的方法處理約10~12個最穩定的特征點。PTAM最大的貢獻是提出了tracking、mapping雙線程的架構，tracking線程只需要逐幀更新相機位置姿態，可以很容易實現實時計算；而mapping線程並不需要逐幀更新，有更長的處理時間，原來只能用在離線SfM（Structure from Motion）的BA（Bundle Adjustment）也可以用起來。這種基於優化的算法比濾波法在單位計算時間可以得到更高的精度[3]。這種多線程的處理方式也更順應現代CPU的發展趨勢。之後的視覺SLAM算法幾乎全部沿用了這一思想。

　　在tracking線程，地圖（由地圖點和關鍵幀組成）是已知且固定的。對圖片構造金字塔的目的有兩個：1）加快匹配；2）提高地圖點相對於相機遠近變化時的魯棒性。FAST是常用的特征點，優點是快，缺點是不魯棒。通常會先提取出大量的（成百上千個）FAST特征，然後用各種約束剔除誤匹配。基於運動模型（PTAM選用減速模型，一般勻速模型更常見）假定當前幀的初始位置姿態，把地圖點投影到當前幀，建立起當前幀和關鍵幀（每個地圖點會關聯第一次觀察到它的關鍵幀）的聯系。隨後在匹配點附近找一塊小區域（這就是所謂的patch），通過比較當前幀和關鍵幀的patch的相似度（計算SSD），可以剔除誤匹配。為了去除觀測位置姿態不同的影響，會在匹配前對patch做仿射變換，直觀的解釋是：正方形從另一個角度看是平行四邊形。Patch匹配的另一個用處是提高匹配點的精度，可以用插值得到亞像素級別的坐標，這對於金字塔高層圖片上的匹配點幫助很大。找到一系列匹配點之後，就可以優化求解當前幀的位置姿態，優化通常是在李群求解。PTAM為了加速計算，設計了從粗到細兩輪求解過程，粗測階段只考慮金字塔最高層圖片上的少量匹配點（50個），優化出的位置姿態作為精測階段的初值，精測階段會納入更多匹配點（1000個）和金字塔所有層。從粗到細兩輪求解的缺點是會增加抖動（jitter），改進的辦法是在接近靜止的狀態下跳過粗測直接進入精測階段。PTAM用三個級別評判tracking質量：好、不好、丟失。只會在“好”的狀態下插入新關鍵幀和地圖點，如果“丟失”，會有簡單的重定位功能（在所有關鍵幀中找相似的）。

　　在mapping線程，優化對象是地圖點位置和關鍵幀位置姿態，不再考慮當前幀的位置姿態。先利用兩幀圖片初始化，具體方法是先五點法加RANSAC求出初值，然後所有匹配點（約1000個）BA優化。由於是單目，所以尺度是未知的，當做常數固定下來。首先從以下幾個角度判斷當前幀是否是關鍵幀：1）tracking效果好；2）距離上一個關鍵幀至少20幀圖片；3）距離最近的地圖點大於一個閾值，這是為了保證baseline足夠大。如果當前幀不是關鍵幀，則做BA優化。PTAM把BA優化分為局部和全局兩部分，這和tracking線程從粗到細兩輪求解是一個思路，都是為了降低計算復雜度，加速求解。在局部BA階段，只考慮滑動窗內的關鍵幀（5幀），以及它們能觀測到的所有地圖點。額外的約束是能觀測到這些地圖點的其他關鍵幀，這些關鍵幀固定不動，並不是優化對象。全局BA階段，優化對象納入所有的關鍵幀和地圖點。在閑暇時間mapping線程可以利用舊的關鍵幀改善地圖，要麽從舊的關鍵幀觀察新添加的地圖點，要麽重新測量之前被剔除的外點，如果被成功觀測並收斂，則作為新的地圖點插入地圖。如果當前幀是關鍵幀，則做兩件事情：1）把所有地圖點投影到這個新的關鍵幀（tracking線程處於計算量的考慮只投影了一部分地圖點），為之後的BA做準備；2）生成新的地圖點，會對新關鍵幀的特征點做非極大值抑制，並篩選出最顯著（Shi-Tomasi分數）的一批特征點，然後在最近的關鍵幀上沿極線搜索匹配點，只要能找到匹配點，就三角化出地圖點。

參考文獻：

[1] Klein G, Murray D. Parallel tracking and mapping for small AR workspaces[C]//Mixed and Augmented Reality, 2007. ISMAR 2007. 6th IEEE and ACM International Symposium on. IEEE, 2007: 225-234.

[2] Davison A J, Reid I D, Molton N D, et al. MonoSLAM: Real-time single camera SLAM[J]. IEEE transactions on pattern analysis and machine intelligence, 2007, 29(6): 1052-1067.

[3] Strasdat H, Montiel J M M, Davison A J. Real-time monocular SLAM: Why filter?[C]//Robotics and Automation (ICRA), 2010 IEEE International Conference on. IEEE, 2010: 2657-2664.

視覺SLAM算法框架解析(1) PTAM