基于點線特征魯棒性評估的雙目視覺里程計

榮燊

（合肥工業大學電子科學與應用物理學院 安徽省合肥市 230009）

同步定位與地圖構建（Simultaneous Localization and Mapping）即SLAM 技術，在自動駕駛、無人機控制以及太空機器人等自動化領域應用廣泛，主流的視覺SLAM 大致分為三類：基于濾波器[1]、基于關鍵幀優化[2-4]、基于直接跟蹤的方法[5-7]。而作為視覺SLAM的前端視覺里程計，為后端估計出一個較為準確的位姿是其主要的工作。

基于點線特征的視覺里程計[8-10]位姿估計的精度依賴點線特征匹配的精度，傳統綜合特征的視覺里程計對提取出的特征匹配，采用最小化3D-3D 誤差[11]或3D-2D 誤差[9]對方法對位姿進行估計。這些方法把提取出的所有特征匹配的精度視為一致的，對這些匹配的特征建立單一誤差模型，求解相機的位姿?？紤]到在雙目立體匹配中，由于匹配的特征點的視差越大，三角測量的不確定性越小，估計出的特征點深度精度越高，但是視差越大的匹配點的深度越低，在特征跟蹤時容易丟失，且匹配的可靠性較差，這就形成了三角測量精度與特征點匹配精度相矛盾的地方。

本文綜合考慮匹配精度和三角測量精度，提出一種特征魯棒性估計方法，通過篩選魯棒性高的特征點和特征線段，綜合計算效率，提出了一種結合了ICP和PnP方法估計位姿的點線雙目視覺里程計，并在KITTI-ODOMETRY 公開數據集上通過與PL-StVO[9]對比，驗證了本文算法的有效性。

1 算法框架

本文視覺里程計算法框架如圖1 所示，分為位姿估計、關鍵幀選取、局部建圖三個部分。其中，位姿估計是本文的算法的核心工作。

特征提取過程中，本文使用了ORB[12]進行特征點提取和描述，LSD[13]進行特征線段提取并采用LBD[14]描述子進行描述。通過對提取的特征線段長度進行約束，過濾場景中大多數非結構化直線，減少線特征描述子的計算成本，對特征線段采用端點模型表示，簡化特征線段的處理方式。并通過對特征點線進行極線約束，進一步提高特征匹配的精度。通過對KITTI 數據集[15-16]中雙目圖像對特征匹配對的視差統計，結合雙目視差對三角測量不確定性的影響，對特征點進行魯棒性評估，對三角測量精度高的特征點認為魯棒性較高，計算特征點深度，作為3D 特征內點；對三角測量精度較低的特征點，認為其三角測量誤差較大，作為2D 特征內點，將特征點分為3D 和2D 特征。在幀間跟蹤時，針對3D-3D、3D-2D、2D-2D 不同的特征匹配，提出了結合ICP 和PnP 方法的位姿估計方法。最后進行選取信息冗余度低，位姿估計精度高的關鍵幀進行局部地圖優化。

圖1：算法框架

圖2：在KITTI_00 序列上使用LSD 提取直線的結果

2 雙目視覺里程計

2.1 特征提取和匹配

由于SLAM 系統的實時性要求，本文方法采用ORB 方法對特征點提取和描述，采用LSD 方法對特征線段進行提取，采用LBD方法對特征線段描述。

LSD 特征提取是一種基于區域生長的線段提取算法，在提取時會檢測到許多細小的線段，如圖2 所示，這些線段數量龐大，而且大多數非圖像中的結構化線段，帶有較大的不確定性，且對數量龐大的線特征計算描述子會耗費更多的計算成本，導致算法效率下降。本文首先通過對檢測出的線特征幾何長度進行約束，去除這些非結構性線段。

算法流程如下：

（1）對圖像首先采用梯度密度濾波器[17]進行預處理，剔除如圖2.1 中所標識出來的密集區域，然后采用LSD 算法進行線段提取；

表1：本文方法和LBD 描述子描述和匹配平均每幀耗時對比（ms/幀）

（2）計算所提取線段的LBD 描述子，進行特征匹配，并采用RANSAC 方法對特征匹配中的誤匹配進行預剔除；

（3）對成功匹配的特征線段使用端點模型表示，提取端點和線段中點進行極線約束，只要有一個點滿足了極線約束，則認為該線段特征匹配成立。

本算法第一步是為了對線特征進行粗篩選，減少計算細小線段所帶來的計算量，這些細小的線段往往非剛體結構，易受環境變化而變化，會給匹配帶來更大的誤差和更多的計算量，所以在粗篩選中直接剔除；第三步極線約束采用端點三選一策略是為了防止檢測到的存在局部遮擋的線特征被極線約束過濾。

本算法使用的測試平臺如下：CPU 為Intel（R） XEON（R） CPU E3-1231 v3 @ 3.4GHz，內存為8G DDR3 1600MHz，測試系統為Ubuntu16.04。對KITTI-ODOMETRY 的00-10 這11 個雙目序列使用單一線程對本算法進行時間統計，與直接使用LBD 計算描述子所用的時間對比結果如表1 所示。

實驗證明本文方法與直接使用LBD 描述子相比在KITTI 00-10的所有數據集上的速度均更快，平均耗時大約僅為直接使用LBD描述子的25%左右。

2.2 特征內點篩選

通過極線約束可以初步篩選出匹配精度較高的特征內點，這些特征內點以它們各自在對應圖像中的二維像素坐標的形式儲存，需要對這些匹配特征內點通過三角測量，求出它們所對應的空間3D點坐標。但是由于數字圖像的分辨率是由像素決定的，在相機基線一定的情況下，三角測量的不確定性將由匹配點的視差決定。

本文以KITTI-ODOMETRY 公開數據集中使用的相機的內參數矩陣為例，利用計算出視差沿逐個像素增長時深度估計的區間寬度。如圖3 所示，可以很明顯的看出隨著視差值的增加，三角測量的不確定性也在大幅下降，當視差大于20 個像素時三角測量估計出的深度誤差已經在1 米以下,當視差大于100 個像素時，三角測量的深度誤差已經降到1%以下。但是在雙目匹配中，視差值較大的特征點其深度值較小，在相機運動時容易丟失，且有可能是誤匹配的特征，保留所有視差較大的特征匹配會引入誤匹配的特征點，三角測量將沒有意義。

本文通過對KITTI 00-10 這11 個序列中每個序列均勻抽取100幀立體圖像對，進行特征匹配和極線約束，一共篩選出133273 對匹配的特征內點進行視差統計，繪制直方圖如圖4 所示。考慮到位姿估計時，多次迭代所需要的最小特征點數量，發現大約54%的特征點的視差的絕對值聚集在[0，200]這個區間內，平均每幀有66 個特征點。由于三角匹配和特征匹配的要求互相矛盾，首先考慮滿足特征匹配的要求，建立準確的特征匹配是估計相機位姿重要的一步。首先對極線約束的視差進行分割，選擇視差的絕對值范圍在[100,200]之間的圖像特征點，其三角測量誤差較小，約占特征內點總數的22%，平均每幀有27 對可靠的3D 特征內點匹配對；視差的絕對值范圍在[20,100)之間的圖像特征點，其三角測量得出的深度值誤差較大，認為三角測量不可靠，大約占特征內點總數的25%，平均每幀有30 對可靠的2D 特征內點匹配對；對視差范圍在200 以外的特征點，其深度值過小，在相機運動時容易丟失，且特征匹配的可靠性較低，將其從特征內點中移除。

圖3：雙目視差與三角測量不確定性

圖4：極線約束視差統計直方圖

2.3 結合ICP和PnP的位姿估計方法

圖5：00（a）、01（b）、08（c）、10（d）四個序列的相機軌跡對比

估計相機位姿是視覺里程計最重要的工作之一。視覺SLAM后端的優化效果在一定程度上取決于前端視覺里程計給定的初始值，前端如果能在保證算法實時性的基礎上估計出一個良好的初始值給后端，可以減小后端迭代優化算法的計算成本，提高后端優化的精度。

通過三角測量和特征魯棒性評估，篩選出了通過三角測量估計出特征點的空間位置，對這些特征匹配點進行分析，發現在幀間匹配時會出現以下幾種情況的匹配：

（1）3D 特征內點與3D 特征內點（3D-3D）的匹配；

（2）3D 特征內點與2D 特征內點（3D-2D）的匹配；

（3）2D 特征內點與2D 特征內點（2D-2D）的匹配；

考慮2D-2D 特征內點之間由于深度不確定性較大，通過2D-2D 特征內點估計位姿時沒有深度信息的約束，導致估計時相機的位移t 誤差較大。所以本文對2D-2D 匹配的特征內點不進行位姿估計，直到它們重新與3D 特征內點建立匹配。

對3D-3D 匹配的特征點建模它們的三維世界坐標誤差：

李代數上的擾動為：

表2：KITTI 數據集不同序列的平均軌跡誤差（%）和旋轉誤差（deg/m）

對3D-2D 匹配的特征點建模它們的重投影誤差：

李代數上的擾動為：

待優化的目標函數為：

建立非線性優化框架：

（1）給定初值ξ0

（2）對于第k 次迭代，求解

（3）若δξk足夠小，則停止，否則，返回第二步。

其中，本文采用勻速運動模型使用上一幀的相機位姿ln(exp(Δ ξ^) exp(ξ^))?對ξ0進行賦初值，采用Pseudo-Huber 魯棒核函數減少誤差過大的特征對位姿估計的影響，并對參與位姿估計的特征內點數量進行統計。

2.4 關鍵幀選擇

在相機運動過程中采集到的數據規模會隨著時間越來越大，而且不同時刻相機采集到的圖像質量也參差不齊，將每一幀圖像所估計出的相機位姿均作為后端優化的狀態會導致地圖規模迅速擴大，后端優化的成本也會急速增長，導致后端效率低下，因此需要從整個圖像序列中挑選一些有代表性的圖像作為關鍵幀。關鍵幀的選取極為重要，它包含了整個系統所維護地圖的大多數數據，對于系統精度和效率的影響極大。關鍵幀挑選的過于密集，會導致后端計算效率過低，地圖維護成本過高；關鍵幀挑選的過于稀疏，會導致關鍵幀之間的位姿估計誤差較大，幀間匹配困難，甚至導致跟蹤丟失。

結合ICP 和PnP 的位姿估計，可以為后端估計出相機的初始位姿，但是這些初始位姿的不確定性，需要對其定量分析。根據文獻[14]，本文采取了將相機的相對位姿估計的不確定性從協方差矩陣轉化為熵：

然后，對于上一個最近提取的關鍵幀i，需要檢查第i 幀的熵與當前幀i+u 的上的比值：

如果a 的值低于設立的閾值，那么就認為新幀帶來的不確定較多，需要將當前幀第i+u 幀加入關鍵幀。本實驗中，閾值將設為0.9。

于是本文所采用的關鍵幀選擇需滿足以下所有條件：

（1）距上一次關鍵幀插入已經間隔超過n 幀或局部建圖線程空閑，n 取決于序列總長度和相機運動速度；

（2）當前幀至少篩選出50 對特征點和10 對特征線段；

（3）當前幀熵與上一個關鍵幀的熵的比值大于0.9。

其中條件2 確保了當前幀作為關鍵幀時位姿估計的精度，條件3 保證了兩個關鍵幀之間由于新信息的引入而導致的不確定性的增長，代表了兩個關鍵幀之間必定要有一定的不相關。

3 局部地圖構建

本文通過將關鍵幀加入局部地圖，通過對局部地圖中關鍵幀之間描述子的匹配關系，建立關鍵幀之間的共視圖，即局部閉環，對共視圖中的關鍵幀及其對應的路標點進行光束平差法優化消除局部累積誤差，獲得局部精確的關鍵幀位姿。

4 實驗結果分析

為了保證本文提出系統精度、效率的可對比性，本文的系統在KITTI 公開數據集上進行測試。KITTI 數據集由德國卡爾斯魯厄理工學院和豐田美國技術研究院聯合創辦，是目前國際上最大的自動駕駛場景下的計算機視覺算法評測數據集[15-16]。本文選取文獻[9]中不含后端閉環的PL-StVO 與本文提出的視覺里程計位姿估計的結果進行對照，在給出了真實軌跡和位姿的KITTI 公開數據集的00-10 這11 個序列上進行對比。

本文對于每一次實驗，通過比較本文算法和PL-StVO 算法對相機軌跡和位姿的估計值和實際值之間的誤差來衡量估計軌跡的全局一致性，如圖5 所示，給出軌跡對比較為明顯的00、01、08、10四個序列的對比效果。在圖5 中可以較為明顯的看出與PL-StVO對比，本文算法的相機軌跡精度更高。這是由于在觀測值誤差噪聲較小的情況下，最小化幀間跟蹤的特征匹配對的3D-3D 誤差具有最優的封閉解，在存在唯一解和無窮多解的情況下，求出的局部極小值即為全局最優值，不會出現局部極小而非全局最小的情況。

為了量化算法精度，本文采用KITTI 公開數據集所提供的里程計開發工具分別對本文算法和PL-StVO 進行評測。評測結果如表2所示。

對比PL-StVO，可以看出在平均軌跡長度上本文算法估計出的平均相機軌跡誤差在00、01、02、03、04、05、07、08、09、10 序列上均比PL-StVO 要好,估計出的平均相機旋轉誤差在00、01、03、04、05、07、08、09、10 序列上要優于PL-StVO。在平均相機運動速度上本文算法估計出的平均相機軌跡誤差在00、02、07、08、09、10 上要優于PL-StVO，估計出的平均相機旋轉誤差在00、03、06、08、10 等序列上均優于PL-StVO，證明了對相機軌跡的估計精度和對相機位姿估計的精度都有一定提升。這是由于本文算法對特征匹配中的特征對進行魯棒性分析，在保證匹配精度的同時篩選出三角測量精度更高的特征匹配，通過對它們對應的空間點建模3D-3D 誤差，對魯棒性較差的特征匹配建模3D-2D 的重投影誤差，提高了精度更高的觀測點的對位姿估計的影響，所以提高了相機位姿估計的精度。

5 結論

本文的主要貢獻包括：

（1）在前端視覺里程計中針對線特征，利用了線段的幾何信息改進了LBD 描述子的匹配過程，考慮了線段易受不同程度的遮擋，采用端點模型三選一極線約束的方式完成匹配。

（2）綜合考慮點線特征的匹配過程中特征匹配的精度和三角測量不確定性之間的矛盾，提出特征魯棒性評估，根據三角測量精度劃分不同魯棒性的特征。

（3）改進了位姿估計的計算方式，通過對3D-3D 匹配的特征內點建?？臻g點位置誤差，對3D-2D 匹配的特征內點建模重投影誤差，重寫優化函數，并通過非線性優化的方式估計位姿。

（4）改進了新的關鍵幀選擇策略及地圖更新方式，提高了選擇的關鍵幀的精度，避免了關鍵幀的冗余，使其在后續的局部建圖和閉環檢測及校準過程更為高效。