基于點(diǎn)線特征魯棒性評估的雙目視覺里程計(jì)

榮燊

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

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

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

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

1 算法框架

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

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

圖1：算法框架

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

2 雙目視覺里程計(jì)

2.1 特征提取和匹配

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

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

算法流程如下：

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

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

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

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

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

本算法使用的測試平臺(tái)如下：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 個(gè)雙目序列使用單一線程對本算法進(jìn)行時(shí)間統(tǒng)計(jì)，與直接使用LBD 計(jì)算描述子所用的時(shí)間對比結(jié)果如表1 所示。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

對3D-3D 匹配的特征點(diǎn)建模它們的三維世界坐標(biāo)誤差：

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

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

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

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

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

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

（1）給定初值ξ0

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

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

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

2.4 關(guān)鍵幀選擇

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

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

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

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

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

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

（2）當(dāng)前幀至少篩選出50 對特征點(diǎn)和10 對特征線段；

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

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

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

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

4 實(shí)驗(yàn)結(jié)果分析

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

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

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

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

5 結(jié)論

本文的主要貢獻(xiàn)包括：

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

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

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

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