• 概述
• FAST特徵提取
• 地圖初始化
• 跟蹤定位
• 關鍵幀選取
• 重定位
• Bundle Adjustment
• 極線搜尋
• 分析

概述

PTAM，全稱Parallel Tracking And Mapping，是最早提出將Track和Map分開作為兩個執行緒的一種SLAM演算法，是一種基於關鍵幀的單目視覺SLAM演算法。PTAM主要分為這幾部分：

1) Track執行緒

• FAST特徵提取
• 地圖初始化
• 跟蹤定位
• 選取新增關鍵幀到快取佇列
• 重定位

2) Map執行緒

• 區域性BundleAdjustment
• 全域性BundleAdjustment
• 從快取佇列取出關鍵幀到地圖
• 極線搜尋加點到地圖

另一方面，按照一般的視覺SLAM框架，PTAM也可分為

• 感測器資料獲取（攝像頭輸入影象資料）
• 前端視覺里程計（跟蹤定位、重定位）
• 後端優化（Bundle Adjustment）
• 建圖（極線搜尋加點）
• 沒有迴環檢測

程式碼：GaoHongchen/ptam_cg，對原始PTAM程式碼進行了部分改動（將原來的makefile工程改成了cmake工程，改變了部分程式碼結構，新增帶有公式的Doxygen文件註釋，某些本文件提及的演算法公式可能會出現程式碼註釋中），核心演算法沒變。

FAST特徵提取

• 為了SLAM的實時性，選擇FAST作為特徵提取的方法；
• 對於從資料流中輸入的每一幀影象先進行金字塔分層(4層均值金字塔)，每一層都要進行FAST特徵提取；

地圖初始化

對於每幀影象提取的FAST特徵點，因經常出現“扎堆”現象，再進行非極大值抑制，選出較好的特徵點，然後對每個特徵點計算Shi-Tomas得分，選出得分較高的特徵點（不超過1000個，設定數量閾值），作為特徵匹配的候選特徵點。

先選擇一幀影象，再通過基於SSD的塊匹配選出第二幀影象，作為兩幀關鍵幀；根據兩幀影象間的匹配特徵點，計算出兩幀間的單應性矩陣，然後分解出對應的旋轉平移矩陣

因單目的尺度不確定問題，根據經驗設定一個尺度，作用於初始兩幀間的旋轉平移矩陣，並作為全域性的尺度。

根據初始兩幀間的旋轉平移矩陣和特徵點畫素座標，利用線性三角法深度估計演算法估算出第一幀座標系下的世界點三維座標，再通過BundleAdjustment方法對世界點和相機初始位姿進行優化；因先前計算出的世界點數量可能不夠，再通過極線搜尋新增世界點，再通過BundleAdjustment方法對世界點和相機初始位姿進行優化。

根據現在的世界點，通過RANSAC找出主平面，作為系統的世界座標系，同時計算出質心$C$；計算出內點和主平面質心的協方差矩陣，通過PCA主成分分析得出主平面的法向量$N$，然後通過Gram-Schmidt正交化計算出第一幀座標系和主平面座標系旋轉矩陣$R$，再根據質心$C$和公式$P_w=R(P_c-C)=RP_c-RC$計算出平移向量$t=-RC$。

根據主平面計算出的旋轉平移矩陣，將第一幀座標系下的世界點和兩幀的旋轉平移矩陣變換到主平面對應的世界座標系下，第二幀對應的旋轉平移矩陣作為當前相機的位姿。

跟蹤定位

（1）根據上一幀的相機位姿（旋轉平移矩陣），通過作用運動模型和基於ESM的視覺跟蹤演算法對當前幀的相機位姿進行預測。

（2）根據預測的相機位姿，將當前所有世界點根據小孔成像原理進行投影，投影后的畫素點記為$p_i$，並計算出對應的金字塔層級。

（3）根據金字塔高層優先原則，選取一定數量世界點（通常，粗搜尋選取30～60個，細搜尋選取1000個左右）。

（4）遍歷選取的世界點，對於每一個世界點對應源幀影象中已經進行Warp變換的8x8的模板塊，和以當前幀影象點$p_i$一定範圍內的每一個FAST特徵點為中心選取的8x8畫素塊，進行基於SSD的相似度計算，選擇具有最小SSD值的FAST特徵點，並記錄查詢到的特徵點數量，用於後期跟蹤質量評估；出於精確考慮，可通過反向合成影象對齊演算法求取該特徵點的亞畫素座標，記為$p_i'$，這樣選取的每個世界點都對應$p_i$和$p_i'$，重投影誤差即為$p_i' - p_i$。

（5）根據重投影誤差建立起誤差函式，以預測的相機位姿作為初始值，通過高斯-牛頓非線性優化方法計算出當前幀的相機位姿，其中每次迭代的位姿增量為李代數形式。

實際上，PTAM中位姿計算分粗跟蹤和細跟蹤兩個階段，每個階段均進行上述的（3）～（5）過程，主要差別在於選取世界點進行計算的點數。

關鍵幀選取

關鍵幀選取的指標主要有：

• 跟蹤質量（主要根據跟蹤過程中搜索到的點數和搜尋的點數比例）
• 距離最近關鍵幀的距離是否足夠遠（空間）
• 距離上一關鍵幀的幀數是否足夠多（時間）
• 關鍵幀快取佇列是否已滿

重定位

構建關鍵幀時，每一幀都會生成一個高斯模糊小圖（大小為頂層金字塔尺寸的一半，並進行了0.75的高斯卷積，以及去中心化）。

重定位時，基於SSD演算法 計算 當前幀的高斯模糊小圖 和 地圖中所有的關鍵幀的高斯模糊小圖 的 相似度，選擇相似度最高的關鍵幀的相機位姿，根據基於ESM的視覺跟蹤演算法計算出相機位姿作為當前幀的相機位姿。

Bundle Adjustment

Bundle Adjustment（以下簡稱BA），中文翻譯“光束法平差”，本質是一個優化模型，目的是最小化重投影誤差，用於最後一步優化，優化相機位姿和世界點。PTAM中BA主要在Map執行緒中，分為區域性BA和全域性BA，是其中比較耗時的操作。

區域性BA用於優化區域性的相機位姿，提高跟蹤的精確度；全域性BA用於全域性過程中的相機位姿，使相機經過長時間、長距離的移動之後，相機位姿還比較準確。

BA是一個圖優化模型，一般選擇LM(Levenberg-Marquardt)演算法並在此基礎上利用BA模型的稀疏性進行計算；可以直接計算，也可以使用g2o或者Ceres等優化庫進行計算。

極線搜尋

選擇關鍵幀容器中最後一幀作為源幀，然後在所有關鍵幀中找到距離其最近的一幀作為目標幀。

通過源幀中畫素特徵點、場景平均深度和場景深度方差，根據對極幾何原理，找出源幀中平均深度附近一定範圍光束，並將其投影到目標幀成像平面，為一段極線。

遍歷該段極線附近所有的候選特徵點，通過基於SSD的塊匹配方法查找出與源幀影象匹配的特徵點，再通過反向合成演算法求取其亞畫素座標，然後三角法計算世界點。

分析

優化演算法：基本是基於最小二乘的非線性優化演算法，跟蹤部分使用G-N求解基於權重的最小二乘，BA使用L-M；

矩陣求逆：Cholesky分解；

線性方程組求解：SVD分解 ；

初始化相機位姿求解，用的是基於2D-2D的對極幾何計算單應性矩陣，適用於共面特徵點；後面跟蹤部分相機位姿求解，用的是基於2D-3D的PnP演算法；

塊匹配：基於SSD的相似度計算；