基于采樣點(diǎn)的直線匹配視覺(jué)慣性SLAM算法

林 林，黃宜慶*，李 宏

(1.安徽工程大學(xué) 高端裝備感知與智能控制教育部重點(diǎn)實(shí)驗(yàn)室，安徽 蕪湖 2410002.安徽工程大學(xué) 安徽省電氣傳動(dòng)與控制重點(diǎn)實(shí)驗(yàn)室，安徽 蕪湖 241000)

在自動(dòng)駕駛和智能機(jī)器人領(lǐng)域中，同時(shí)定位與建圖得到了廣泛的應(yīng)用。視覺(jué)慣性系統(tǒng)亦稱VIO(Visual-inertial Odometry)系統(tǒng)，是融合相機(jī)和IMU(Inertial Measurement Unit)數(shù)據(jù)實(shí)現(xiàn)SLAM(Simultaneous Localization and Mapping)的算法，也是目前SLAM研究領(lǐng)域的研究熱點(diǎn)。相機(jī)在快速運(yùn)動(dòng)時(shí)會(huì)出現(xiàn)圖片模糊的問(wèn)題，而IMU正好可以解決該類問(wèn)題，且其價(jià)格便宜，IMU的缺點(diǎn)是它經(jīng)過(guò)積分所得的加速度和角速度存在漂移問(wèn)題，而相機(jī)沒(méi)有這個(gè)缺點(diǎn)，所以兩者可以很好地互補(bǔ)。

Tong等[1]提出并開(kāi)源了VINS-Mono，這是一個(gè)具有魯棒性的初始化過(guò)程，采用了一種基于緊耦合、非線性優(yōu)化的方法，其效果在當(dāng)時(shí)的VIO系統(tǒng)中是非常優(yōu)秀的。但是基于點(diǎn)特征的VIO系統(tǒng)，其視覺(jué)包含的信息總是有限的，在光照強(qiáng)度較弱和快速運(yùn)動(dòng)的場(chǎng)景中，點(diǎn)特征的提取就變得尤為困難，使得SLAM系統(tǒng)難以保持較強(qiáng)的魯棒性和較高的精度。線特征廣泛存在于人造環(huán)境中，它可以提供額外的視覺(jué)約束，并為自動(dòng)導(dǎo)航建立一個(gè)信息更豐富的地圖，因此將線特征信息加入到視覺(jué)前端很有意義。文獻(xiàn)[2]在VINS-Mono的框架基礎(chǔ)上將線特征集成到VIO系統(tǒng)中。文獻(xiàn)[3]使用梯度密度濾波器剔除圖像中線特征的密集區(qū)域，使得線特征的誤匹配大大降低，但加入濾波器后,密集區(qū)域的內(nèi)部將不進(jìn)行線段檢測(cè),而僅在其周圍輪廓區(qū)域提取部分線段，這會(huì)導(dǎo)致很多的線段沒(méi)有被檢測(cè)到，從而失去很多線段信息。文獻(xiàn)[4]則提出引入線段分組的方法，將可能是同一條線上的線段合并，生成匹配和提取效率高的長(zhǎng)線段。但生成的長(zhǎng)線段存在一些誤匹配，影響了系統(tǒng)的精度。

文獻(xiàn)[5]在ORB-SLAM2[6]的系統(tǒng)上進(jìn)行創(chuàng)新，采用幾何約束進(jìn)行線特征匹配，利用線特征的深度信息構(gòu)造虛擬線段，提出不一樣的重投影誤差計(jì)算方法，但其重投影誤差的計(jì)算僅采用圖像的像素觀測(cè)，卻忽略了深度觀測(cè)。文獻(xiàn)[7]將直線分為結(jié)構(gòu)化直線和非結(jié)構(gòu)化直線，并對(duì)這兩類直線建立不同的數(shù)學(xué)模型，構(gòu)建殘差，加入到目標(biāo)函數(shù)中進(jìn)行優(yōu)化。

基于上述研究，在視覺(jué)前端加入線特征，提出一種新的直線匹配方法，將幀到幀和幀到平面的兩種直線匹配方法結(jié)合，并在線特征提取階段，設(shè)置動(dòng)態(tài)的長(zhǎng)度閾值過(guò)濾掉短線特征，用以提高系統(tǒng)在弱紋理和快速運(yùn)動(dòng)的場(chǎng)景中的魯棒性和精度；在視覺(jué)后端，將構(gòu)建好的線特征重投影誤差模型加入到基于滑動(dòng)窗口內(nèi)的目標(biāo)函數(shù)進(jìn)行求解，最后在數(shù)據(jù)集上驗(yàn)證所提算法。

1 算法框架

本文算法以VINS-Mono為系統(tǒng)框架，采用優(yōu)化方法將點(diǎn)線特征的視覺(jué)觀測(cè)數(shù)據(jù)與IMU觀測(cè)數(shù)據(jù)緊密耦合。算法的系統(tǒng)框架如圖1所示。由圖1可知，系統(tǒng)框架主要包含3個(gè)線程：數(shù)據(jù)預(yù)處理、回環(huán)檢測(cè)與全局位姿圖優(yōu)化、滑動(dòng)窗口優(yōu)化。其中，算法改進(jìn)的地方已經(jīng)用底紋標(biāo)出。

數(shù)據(jù)預(yù)處理：分別對(duì)IMU和相機(jī)得到的點(diǎn)線特征進(jìn)行提取、匹配并對(duì)齊。在點(diǎn)特征方面，采用Harris角點(diǎn)[8]，LK(Lucas-Kanade)光流以及雙向跟蹤[9]相結(jié)合的方法提高跟蹤的精度。在正向光流跟蹤中，如果上一幀有特征點(diǎn)，就直接進(jìn)行LK跟蹤并進(jìn)行預(yù)測(cè)，然后統(tǒng)計(jì)和上一幀成功匹配的數(shù)目；如果小于10，就擴(kuò)大搜索，輸入的最大金字塔層數(shù)為3；如果沒(méi)有進(jìn)行預(yù)測(cè)的話，就直接輸入最大金字塔層數(shù)為3。接下來(lái)進(jìn)行反向跟蹤，如果正向反向都能找到特征點(diǎn)，則認(rèn)為這些特征點(diǎn)是符合要求的。

在線特征方面，為更好地解決位姿估計(jì)問(wèn)題，在LSD(Line Segment Detector)算法[10]上進(jìn)行改進(jìn)，設(shè)置提取線特征的長(zhǎng)度閾值[11]來(lái)去除掉短線特征，提高提取效率。并在直線匹配中加入基于點(diǎn)特征跟蹤的平面到平面(2D-2D)和平面到立體(2D-3D)的直線匹配算法，從而提高匹配模塊的速度和精度。

滑動(dòng)窗口優(yōu)化，回環(huán)檢測(cè)和全局位姿圖優(yōu)化與VINS-Mono的方法大致相同，不同之處將在下文介紹，這里不再贅述。

2 線特征提取及匹配

2.1 線特征提取

對(duì)于線特征的處理方法，現(xiàn)階段基本上都是采用LSD算法。LSD算法是將梯度方向相近的像素點(diǎn)合并，然而由于噪聲等一些因素，一條長(zhǎng)的直線可能被分割成很多條短的線段，這些短的線段所包含的圖像信息不足，用于后面的匹配則效率過(guò)低，為此設(shè)置長(zhǎng)度抑制策略。

長(zhǎng)度抑制策略：使用最小長(zhǎng)度閾值Lmin來(lái)去除短線特征：

Lmin=[γ×min(WL,HL)],

(1)

(2)

式中,min(WL,HL)為輸入圖像的寬度與高度二者之間較小的那個(gè)值;γ為長(zhǎng)度比例因子;i為提取到線特征的數(shù)量。在文獻(xiàn)[11]中，作者是根據(jù)經(jīng)驗(yàn)將γ設(shè)為一個(gè)固定的值，但這樣設(shè)置的γ不能使提取數(shù)量始終維持在一個(gè)合理的值，因此根據(jù)提取線特征的數(shù)量來(lái)動(dòng)態(tài)地改變其值。當(dāng)線段數(shù)量較少時(shí)，增大長(zhǎng)度閾值,反之減小。為減少計(jì)算資源，該算法依然會(huì)設(shè)置最小長(zhǎng)度的下限。通過(guò)該方法，線特征提取精度得到提升。

2.2 2D-2D的直線匹配

針對(duì)直線匹配的精度和穩(wěn)定性，采用2D-2D和2D-3D的直線匹配方法。一般情況下，使用2D-2D的跟蹤方法來(lái)匹配新的檢測(cè)到的線，如果匹配到的線數(shù)量太少，將啟用2D-3D的方法來(lái)提升匹配到的線的數(shù)量。

此方法是對(duì)前一幀中的所有線進(jìn)行采樣，得到采樣點(diǎn)的集合P1{Ω1}，使用極線搜索和塊匹配[12]的方法在新圖像中檢測(cè)到對(duì)應(yīng)的候選匹配點(diǎn)。使用零均值差平方和(Zero-mean Sum of Squared Difference，ZMSSD)[13]計(jì)算兩點(diǎn)的匹配分?jǐn)?shù)，選取匹配分?jǐn)?shù)最高的點(diǎn)作為跟蹤點(diǎn)，最后可以獲取跟蹤點(diǎn)的集合P2{Ω2}。如果被跟蹤的點(diǎn)到當(dāng)前幀的直線距離小于我們?cè)O(shè)定的距離閾值，則認(rèn)為該被跟蹤的點(diǎn)是有效的。如果這些有效的點(diǎn)的數(shù)量大于前一幀線上采樣點(diǎn)的0.8倍，則這條線被認(rèn)為是匹配最佳的線。

(3)

它可以被近似為

(4)

式中,N是修補(bǔ)的寬度乘以修補(bǔ)的高度。

(5)

計(jì)算兩張圖像之間的匹配點(diǎn)的ZMSSD值，從而判斷匹配點(diǎn)的相似程度。

2.3 2D-3D的直線匹配

在2D-3D的直線匹配中，采用零均值歸一化互相關(guān)(Zero-normailized Cross-correlation,ZNCC)[14]的匹配方法，相比于歸一化互相關(guān)(Normailized Cross-correlation，NCC)[15]，它在公式中去除了窗口內(nèi)的均值，對(duì)于光照的變化能表現(xiàn)出更強(qiáng)的魯棒性。兩張圖像的亮度可能由于照明和曝光條件而發(fā)生變化，第一張圖像可以首先被歸一化。這通常是通過(guò)減去平均值和除以標(biāo)準(zhǔn)差的方式在每一步完成。也就是說(shuō)，兩張圖像之間的相似程度可以為

(6)

式中,n是f1和f2的像素?cái)?shù)；μ是f的平均值；σ是f的標(biāo)準(zhǔn)差。

當(dāng)相機(jī)快速運(yùn)動(dòng)和有場(chǎng)景遮擋時(shí)，需要匹配的線段長(zhǎng)度會(huì)相差很大，從而影響匹配的精度。所以采用極線幾何約束來(lái)限制線的采樣范圍，配合ZNCC進(jìn)行匹配。即計(jì)算了基本矩陣和當(dāng)前幀之間的本質(zhì)矩陣之后，在基本矩陣中線的端點(diǎn)根據(jù)極線幾何投影到當(dāng)前幀，并且通過(guò)兩條極線與當(dāng)前幀的匹配線來(lái)確定對(duì)應(yīng)點(diǎn)的采樣范圍，這樣成功地提高了ZNCC匹配方法。

3 線重投影誤差模型及其優(yōu)化

3.1 線重投影誤差模型

線重投影殘差以點(diǎn)-線距離為模型。首先，定義線幾何變換，給定一個(gè)從世界坐標(biāo)系平面πw到相機(jī)坐標(biāo)系平面πc的變換矩陣Tcw=[Rcw,tcw]，其中Rcw∈SO(3)和tcw∈R3定義為旋轉(zhuǎn)和平移。利用這個(gè)矩陣，將Lw從πw變換到πc為

(7)

然后將Lc進(jìn)行投影得到投影直線：

I=[l1,l2,l3]T=KLnc,

(8)

式中，KL表示直線投影矩陣。

最后假設(shè)Lw為第i個(gè)相機(jī)幀觀測(cè)到的第j條空間線，線重投影誤差可以定義為

(9)

3.2 基于滑動(dòng)窗口的優(yōu)化

在滑動(dòng)窗口中定義與IMU、點(diǎn)和線測(cè)量信息相關(guān)的所有狀態(tài)向量：

(10)

oT=(ΦT,φ),

(11)

式中,Φ為一個(gè)三維向量;φ定義為一個(gè)標(biāo)量。

通過(guò)最小化所有測(cè)量殘差項(xiàng)之和來(lái)優(yōu)化滑動(dòng)窗口內(nèi)的所有狀態(tài)變量：

(12)

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

本文采用公共數(shù)據(jù)集EuRoc對(duì)此算法進(jìn)行評(píng)估，并將評(píng)估的結(jié)果與經(jīng)典SLAM算法VINS-Mono的結(jié)果進(jìn)行比較。本實(shí)驗(yàn)在臺(tái)式電腦上展開(kāi)，電腦的基本配置為：Intel(R)Core(TM)i7-9700 CPU@3.00GHz 8G內(nèi)存，Ubuntu 16.04。

本文算法在MH_05_difficult序列上的可視化如圖2、3所示。由圖2、3可以看出，在光照較暗的場(chǎng)景中線特征的提取效果依然良好。在V2_03_difficult序列上的兩種算法軌跡與真實(shí)軌跡的對(duì)比如圖4所示。由圖4可以明顯看出，改進(jìn)后的算法軌跡更接近真實(shí)的軌跡。

為驗(yàn)證本文改進(jìn)系統(tǒng)相比于VINS-Mono具有更高的精確度，采用場(chǎng)景復(fù)雜程度較高的3個(gè)不同序列進(jìn)行實(shí)驗(yàn)，系統(tǒng)所得結(jié)果與實(shí)際結(jié)果的誤差如圖5～7所示。本文算法在EuRoc數(shù)據(jù)集運(yùn)行的絕對(duì)位姿誤差的結(jié)果如表1所示，絕對(duì)位姿誤差與VINS-Mono的對(duì)比結(jié)果如表2所示。

表2 絕對(duì)位姿誤差與VINS-Mono的對(duì)比 (單位：m)

根據(jù)表1、2可得，在公共數(shù)據(jù)集EuRoc的7個(gè)不同難度、不同場(chǎng)景的序列中，本文算法相比于VINS-Mono都有提升，在MH_01_easy和MH_02_easy兩個(gè)場(chǎng)景比較簡(jiǎn)單的序列中，提升效果不明顯，究其原因是在簡(jiǎn)單場(chǎng)景中，點(diǎn)特征所包含的視覺(jué)信息足以使系統(tǒng)達(dá)到了一定的精度，即使加入線特征并改進(jìn)了算法，也不能將精度提高很多，相反線特征的加入會(huì)增加計(jì)算量，降低系統(tǒng)的實(shí)時(shí)性。但在復(fù)雜的場(chǎng)景中，該算法的優(yōu)越性就得到了體現(xiàn)，其中在快速運(yùn)動(dòng)的場(chǎng)景中，精度提高得更明顯。這得益于在線特征提取階段使用長(zhǎng)度抑制策略，匹配階段使用了兩種直線匹配的方法。

表1 本文算法在EuRoc數(shù)據(jù)集運(yùn)行的絕對(duì)位姿誤差的結(jié)果(單位：m)

5 結(jié)束語(yǔ)

該算法在VINS-Mono的基礎(chǔ)上，通過(guò)將線特征融合到系統(tǒng)中，解決點(diǎn)特征包含視覺(jué)信息不足的問(wèn)題，并在原有的直線特征提取算法上通過(guò)長(zhǎng)度抑制過(guò)濾掉短線特征，用來(lái)改善提取效果。并且將2D-2D與2D-3D的直線匹配方法結(jié)合使用，用于提高匹配的精度。而后建立線重投影誤差模型，整合各部分殘差，構(gòu)建目標(biāo)優(yōu)化函數(shù)。通過(guò)在不同難度場(chǎng)景下的實(shí)驗(yàn)仿真，得到本系統(tǒng)的軌跡和誤差，并與VINS-Mono運(yùn)行結(jié)果進(jìn)行對(duì)比，通過(guò)分析可知該算法在快速運(yùn)動(dòng)和弱紋理的情況下表現(xiàn)出更強(qiáng)的魯棒性和更高的精度。