榮燊

（合肥工業(yè)大學(xué)電子科學(xué)與應(yīng)用物理學(xué)院 安徽省合肥市 230009）

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

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

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

1 算法框架

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

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

圖1：算法框架

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

2 雙目視覺里程計

2.1 特征提取和匹配

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

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

算法流程如下：

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

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

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

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

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

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

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

2.2 特征內(nèi)點篩選

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

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

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

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

圖4：極線約束視差統(tǒng)計直方圖

2.3 結(jié)合ICP和PnP的位姿估計方法

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

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

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

（1）3D 特征內(nèi)點與3D 特征內(nèi)點（3D-3D）的匹配；

（2）3D 特征內(nèi)點與2D 特征內(nèi)點（3D-2D）的匹配；

（3）2D 特征內(nèi)點與2D 特征內(nèi)點（2D-2D）的匹配；

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

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

李代數(shù)上的擾動為：

表2：KITTI 數(shù)據(jù)集不同序列的平均軌跡誤差（%）和旋轉(zhuǎn)誤差（deg/m）

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

李代數(shù)上的擾動為：

待優(yōu)化的目標函數(shù)為：

建立非線性優(yōu)化框架：

（1）給定初值ξ0

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

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

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

2.4 關(guān)鍵幀選擇

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

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

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

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

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

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

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

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

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

3 局部地圖構(gòu)建

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

4 實驗結(jié)果分析

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

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

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

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

5 結(jié)論

本文的主要貢獻包括：

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

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

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

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