基于點(diǎn)線特征的改進(jìn)視覺慣性SLAM 方法

嚴(yán)紀(jì)強(qiáng)，吳一琳，胡劍凌

（蘇州大學(xué) 電子信息學(xué)院，江蘇 蘇州 215000）

0 引言

同步定位與建圖（Simultaneous Localization And Mapping，SLAM）是一種基于設(shè)備搭載的傳感器對(duì)周邊環(huán)境信息進(jìn)行感知以實(shí)現(xiàn)設(shè)備自身定位并建立周邊環(huán)境地圖的方法。在過去十幾年里，因價(jià)格較低、感知信息量多、適應(yīng)能力強(qiáng)等優(yōu)點(diǎn)，以相機(jī)作為傳感器的視覺SLAM 一直是移動(dòng)機(jī)器人、自動(dòng)駕駛以及增強(qiáng)現(xiàn)實(shí)等領(lǐng)域的研究熱點(diǎn)之一。

視覺SLAM 根據(jù)相機(jī)位姿估計(jì)的策略不同主要可以分為直接法和特征點(diǎn)法兩種。直接法基于灰度不變性假設(shè)，利用圖像的灰度信息進(jìn)行匹配，最小化光度誤差優(yōu)化求解相機(jī)位姿，計(jì)算量小，運(yùn)行速度快，但易受光照條件的影響，并僅能應(yīng)用于較小的位移，其代表性算法包括LSD-SLAM[1]和DSO[2]等。特征法基于特征的提取和匹配建立約束，最小化重投影誤差求解相機(jī)位姿，能夠適應(yīng)更大的場(chǎng)景變化，具有更強(qiáng)的魯棒性，其代表性算法包括PTAM[3]、ORB-SLAM[4]等。當(dāng)前特征法主要利用的特征為點(diǎn)特征，然而基于點(diǎn)特征的視覺SLAM 存在許多不足，一是單目相機(jī)無法建立尺度約束而雙目相機(jī)運(yùn)算量較大，資源占用多；二是基于點(diǎn)特征的視覺SLAM 在紋理缺失、運(yùn)動(dòng)模糊、弱光照等情況下難以建立良好的數(shù)據(jù)關(guān)聯(lián)，從而導(dǎo)致運(yùn)動(dòng)估計(jì)失敗。

為了解決這些問題，研究人員將視覺和慣性傳感單元（Inertial Measurement Units，IMU）相結(jié)合，提出了許多緊耦合的視覺慣性SLAM 方法，聯(lián)合優(yōu)化視覺和IMU 信息，如MSCKF[5]、VINS-Mono[6]等。另外，考慮到紋理缺失往往發(fā)生在室內(nèi)等人造環(huán)境中，而這些場(chǎng)景往往有豐富的線段特征，因此許多研究人員將線特征加入到圖像之間的數(shù)據(jù)關(guān)聯(lián)中。LU Y[7]等人基于RGB-D 相機(jī)實(shí)現(xiàn)了點(diǎn)線特征融合的視覺里程計(jì)，并證明點(diǎn)線特征比單一特征具有更小的運(yùn)動(dòng)不確定性。ZUO X[8]等人首次在SLAM 中采用正交表示最小參數(shù)化直線，并分析推導(dǎo)出相對(duì)于直線參數(shù)的重投影誤差的雅可比矩陣，大大改善了SLAM 的解決方案。GOMEZ O R 等人提出了基于雙目相機(jī)的PL-SLAM[9]方法，在所有過程中都使用了點(diǎn)線特征，包括視覺里程計(jì)、關(guān)鍵幀選擇、光束法平差，并提出一種新的詞袋模型，在閉環(huán)過程中綜合考慮點(diǎn)線特征的信息。YIJIA H等人基于VINS-Mono 方法提出了PL-VIO[10]方法，將點(diǎn)線特征與IMU 信息緊耦合優(yōu)化，優(yōu)于僅基于點(diǎn)特征的VIO 系統(tǒng)。LI X[11]等人又在PL-VIO 的基礎(chǔ)上增加了面特征和共面約束，基于點(diǎn)特征生成的三維網(wǎng)格進(jìn)一步建模環(huán)境拓?fù)洹Ｈ欢捎贚SD[12]算法提取線特征時(shí)間長(zhǎng)，以上點(diǎn)線特征結(jié)合的算法都難以實(shí)時(shí)運(yùn)行，趙良玉等人[13]在VINS-Fusion[14]方法基礎(chǔ)上利用改進(jìn)的FLD[15]算法實(shí)現(xiàn)了雙目視覺慣性SLAM 系統(tǒng)的實(shí)時(shí)運(yùn)行。QIANG F[16]等人對(duì)LSD 算法的隱參數(shù)進(jìn)行調(diào)整，在盡量不影響準(zhǔn)確性的情況下實(shí)現(xiàn)了LSD 算法的實(shí)時(shí)運(yùn)行。LEE J[17]等人在PL-VINS 基礎(chǔ)上增加了平行線約束，基于滅點(diǎn)對(duì)平行線聚類，將平行線殘差加入到優(yōu)化函數(shù)中，進(jìn)一步提升了實(shí)時(shí)定位精度。

目前的點(diǎn)線SLAM 方法大多使用LSD 算法提取線特征并計(jì)算LBD[18]描述子進(jìn)行線段匹配，然而原始的LSD 算法在策略選擇時(shí)容易將一條長(zhǎng)線段分割為多條短線段，這些短線段不夠穩(wěn)定且容易造成誤匹配。另外，當(dāng)前點(diǎn)線誤差的融合策略為直接融合，未考慮到點(diǎn)線特征之間的差異性以及點(diǎn)線特征融合對(duì)系統(tǒng)帶來的影響。同時(shí)，目前的點(diǎn)線特征在視覺慣性SLAM中利用不夠充分，在閉環(huán)檢測(cè)中僅使用了點(diǎn)特征來計(jì)算圖像間的相似度。

針對(duì)上述存在的問題，本文提出一種基于點(diǎn)線特征的改進(jìn)視覺慣性SLAM 方法。首先，提出一種新的短線段合并算法，合并原屬于同一條直線的短線段，增加圖像之間的數(shù)據(jù)關(guān)聯(lián)；其次，基于LU Y[7]等人的結(jié)論提出一種新的誤差融合策略，調(diào)整了點(diǎn)線特征及IMU 之間優(yōu)化的權(quán)重；最后，將線特征加入到視覺慣性SLAM系統(tǒng)的閉環(huán)檢測(cè)中。基于以上改進(jìn)，所提的方法提升了視覺慣性SLAM 系統(tǒng)的魯棒性和定位準(zhǔn)確性。

1 系統(tǒng)框架

本文提出的方法主要基于VINS-Mono 系統(tǒng)進(jìn)行改進(jìn)，點(diǎn)線特征及IMU 的融合優(yōu)化策略使用了PLVIO 的方法。所設(shè)計(jì)的系統(tǒng)框圖如圖1 所示，主要分為特征提取與跟蹤、局部滑窗優(yōu)化以及閉環(huán)檢測(cè)三部分。初始化過程與VINS-Mono 采取相同的策略，基于視覺信息和IMU 信息的松耦合策略，首先利用純視覺SFM（Structure from Motion）估計(jì)滑動(dòng)窗口內(nèi)所有幀的位姿及3D 點(diǎn)的逆深度，最后與IMU 預(yù)積分的結(jié)果進(jìn)行對(duì)齊，并求解初始化參數(shù)。

圖1 基于點(diǎn)線特征的視覺慣性SLAM 改進(jìn)方法

特征提取與跟蹤首先對(duì)輸入的圖像提取Shi-Tomasi 特征點(diǎn)和LSD 特征線，LSD 算法使用了QIANG F 等人調(diào)整后的隱參數(shù)，從而實(shí)現(xiàn)系統(tǒng)的實(shí)時(shí)運(yùn)行，然后使用所提出的線段合并算法對(duì)線段進(jìn)行合并。對(duì)于Shi-Tomasi 點(diǎn)特征，基于光流法進(jìn)行特征點(diǎn)匹配與跟蹤，對(duì)于LSD 線特征，計(jì)算LBD 描述子，并基于描述子距離進(jìn)行匹配，并將結(jié)果通過機(jī)器人操作系統(tǒng)（The Robot Operating System，ROS）話題發(fā)布/訂閱機(jī)制發(fā)送到局部滑窗優(yōu)化部分，局部滑窗優(yōu)化對(duì)IMU 數(shù)據(jù)進(jìn)行預(yù)積分，并將預(yù)積分的結(jié)果與點(diǎn)線特征重投影誤差基于本文提出的加權(quán)方式進(jìn)行聯(lián)合優(yōu)化，求解得到相機(jī)位姿。同時(shí)，在這一過程中，根據(jù)特征在前后幀圖像間的視差量決定是否插入關(guān)鍵幀。若插入關(guān)鍵幀，則將當(dāng)前關(guān)鍵幀的點(diǎn)線特征發(fā)送到閉環(huán)線程，在閉環(huán)線程采用DBoW[19]詞袋模型分別計(jì)算圖像間的點(diǎn)特征相似度和線特征相似度，并根據(jù)點(diǎn)線特征的數(shù)量和分布情況對(duì)圖像相似度進(jìn)行加權(quán)，確定是否發(fā)生閉環(huán)，之后根據(jù)描述子匹配以及幾何驗(yàn)證確定閉環(huán)的準(zhǔn)確性，若發(fā)生閉環(huán)則進(jìn)行4 自由度全局位姿優(yōu)化。

2 改進(jìn)的LSD 算法

LSD 算法是一種能在線性時(shí)間內(nèi)得到亞像素級(jí)準(zhǔn)確度的線段檢測(cè)算法，其在任何數(shù)字圖像上都不需要額外調(diào)整參數(shù)。但由于在基于梯度進(jìn)行區(qū)域增長(zhǎng)的過程中每個(gè)像素點(diǎn)僅允許被一條直線使用，因此對(duì)于遮擋、相交、邊緣模糊等情況，LSD 算法容易將一條長(zhǎng)線段分割為多條短線段。

對(duì)此，謝曉佳[20]提出了一種利用點(diǎn)線距離、端點(diǎn)距離以及描述子距離進(jìn)行匹配的方法，林利蒙等人[21]詳細(xì)地闡述了基于角度、端點(diǎn)距離的篩選合并策略。但這些方法往往每次只能處理合并兩條短線段，對(duì)于多條線段的合并往往需要多次迭代，會(huì)帶來較高的時(shí)間復(fù)雜度。另外，如圖2 所示，對(duì)于線段a來說，相對(duì)于線段c，往往線段b更加應(yīng)該被合并，盡管在歐氏距離上線段c可能更近一些。在合并時(shí)，上述方法難以確定線段的相對(duì)位置，需要再進(jìn)行多個(gè)端點(diǎn)間的歐式距離計(jì)算，如圖2 中的a、b、d線段難以確定合并后直線的起點(diǎn)和終點(diǎn)，這會(huì)導(dǎo)致計(jì)算資源的浪費(fèi)。

圖2 線段的相對(duì)位置示意圖

本文提出的線段合并算法首先基于角度差值以及端點(diǎn)到直線的距離對(duì)線段進(jìn)行分組，其次根據(jù)線段的角度實(shí)行不同的排序方案，實(shí)現(xiàn)線段基于相對(duì)位置的有序排列，最后根據(jù)線段端點(diǎn)之間的距離和線段的長(zhǎng)度之和進(jìn)行比較，確定是否屬于同一條直線，并基于LBD 描述子對(duì)合并后的直線進(jìn)行篩選和剔除。

2.1 線段分組

實(shí)驗(yàn)發(fā)現(xiàn)，LSD 線段提取得到的相鄰平行線往往方向相反，因此，無需特意進(jìn)行平行線的剔除或合并。但也會(huì)有一些短線段和長(zhǎng)線段平行且方向相同的情況發(fā)生，針對(duì)此類情況，首先使用設(shè)定的長(zhǎng)度閾值Lth將短線段剔除。

考慮到長(zhǎng)線段特征更加穩(wěn)定，首先對(duì)LSD 提取的線段根據(jù)長(zhǎng)度進(jìn)行降序排序，得到L={L1,L2,L3,…,Ln}。從最長(zhǎng)的線段開始進(jìn)行分組，直到線段長(zhǎng)度小于設(shè)定的閾值Lmin。針對(duì)每一條線段Li，首先基于角度進(jìn)行篩選得到候選線段組Lθ，通過這一步可以快速篩選掉大量不匹配的線段，降低后續(xù)的計(jì)算量：

式中：Lj表示線段組L中長(zhǎng)度比Li短的線段，θi和θj分別為L(zhǎng)i及Lj與x軸的夾角，θth表示Li及Lj之間的角度差閾值。

然后，計(jì)算Lj兩端點(diǎn)到Li所在直線的距離，設(shè)線段Lj起點(diǎn)坐標(biāo)為ps，終點(diǎn)坐標(biāo)為pe，線段Li所在直線表示為l=[l1,l2,l3]T，即直線方程為l1x+l2y+l3=0，則兩端點(diǎn)到直線的距離d為：

式中：ds表示Lj起點(diǎn)到Li所在直線的距離，de表示Lj終點(diǎn)到Li所在直線的距離。

基于公式（2）計(jì)算得到距離d，篩選得到最終的候選線段組Lg：

式中：dth表示端點(diǎn)到直線的距離閾值。

通過以上條件篩選，可以將圖片中屬于同一條直線的線段篩選出來。這些線段可能本來屬于同一條直線，也可能是空間中不同直線上的線段投影到一條直線上的。因此，針對(duì)分組后的線段，通過線段合并策略，在盡量將屬于同一條直線的線段合并的同時(shí)剔除誤分組的線段，將之存放到LSD 提取的原始線段組L中，用作后續(xù)處理。

2.2 線段合并

經(jīng)過線段分組后得到一組線段Lg，由于在線段分組過程中未對(duì)線段端點(diǎn)距離進(jìn)行限制，分組中會(huì)存在一些在圖片中處于同一條直線，但在實(shí)際空間相差甚遠(yuǎn)的線段，在線段合并時(shí)要對(duì)這些錯(cuò)誤分組的線段進(jìn)行剔除。LSD 算法提取的直線是有方向的直線，對(duì)于一張二值圖顏色取反，則LSD 提取的直線不變，但方向完全相反，而在計(jì)算LBD 描述子的時(shí)候會(huì)對(duì)線段進(jìn)行方向的區(qū)分，因此在線段合并的時(shí)候需要判斷線段的相對(duì)位置。起始點(diǎn)和終點(diǎn)位置必須具有一致性，以便后續(xù)線段匹配準(zhǔn)確。

首先，為了防止排序的歧義性，將直角坐標(biāo)系根據(jù)角度分為4個(gè)部分，如圖3所示.其中，S表示線段起點(diǎn)，E表示線段終點(diǎn)。以區(qū)間1 為例，以該區(qū)間的線段為主線段時(shí)，經(jīng)過上述線段分組后獲得的線段組，均為S在下、E在上的形式。因此僅需要根據(jù)起點(diǎn)S的縱坐標(biāo)進(jìn)行排序，即可得到在幾何位置上有序的線段序列。

圖3 基于線段方向的分區(qū)示意圖

將之前分組時(shí)參考的長(zhǎng)線段稱為主線段lm，根據(jù)的角度θm執(zhí)行不同的排序策略。這樣做的目的是將線段組按照主線段的方向進(jìn)行對(duì)齊，便于后續(xù)端點(diǎn)距離的計(jì)算。設(shè)兩條線段li和lj，起點(diǎn)和終點(diǎn)分別為(si,ei)，(sj,ej)，每個(gè)點(diǎn)的坐標(biāo)為(x,y)，則根據(jù)式（4）所示的策略進(jìn)行排序。

式中：sort(*)表示基于括號(hào)內(nèi)的變量大小進(jìn)行排序。

通過該方法排序后的線段根據(jù)端點(diǎn)位置按照{(diào)si,ei,sj,ej}順序排列，之后先將排序后的線段根據(jù)主線段的位置分為兩組，如圖4 所示，并對(duì)每一組都進(jìn)行前綴和計(jì)算。前綴和計(jì)算時(shí)，每一組線段都要包含主線段。以排序后的線段組2 為例，設(shè)前綴和計(jì)算結(jié)果為spre=[lsum0,lsum4,lsum5,lsum6]。

圖4 基于相對(duì)位置分組后的線段示意圖

從距離主線段最遠(yuǎn)的線段開始，即圖4 中的l6，計(jì)算主線段lm起始端點(diǎn)sm和線段l6起始端點(diǎn)sj的距離d，并與之前計(jì)算得到的前綴和中對(duì)應(yīng)的線段總長(zhǎng)度lsum6進(jìn)行比較，如式（5）所示：

式中：rth為比例閾值。

若線段總長(zhǎng)度占距離的比例大于閾值rth，則認(rèn)為從lm到l6之間的所有線段屬于同一條長(zhǎng)線段，連接lm的起點(diǎn)和l6的終點(diǎn)作為合并后的線段，否則將線段l6釋放回線段組L中，用于后續(xù)的處理，這是通過對(duì)線段進(jìn)行是否使用的標(biāo)記實(shí)現(xiàn)的，同時(shí)，繼續(xù)判斷次遠(yuǎn)的線段l5。由于長(zhǎng)線段往往具備足夠的信息和穩(wěn)定性，且本不屬于同一直線的長(zhǎng)線段的錯(cuò)誤合并往往會(huì)對(duì)系統(tǒng)造成更大的誤差，在進(jìn)行計(jì)算的過程中，對(duì)比例系數(shù)rth進(jìn)行了線性計(jì)算，限制長(zhǎng)線段的合并。

式中：α和β為參數(shù)因子。

最后，為了減少錯(cuò)誤合并，基于LBD 描述子進(jìn)行篩選，計(jì)算合并后的線段和主線段的描述子并計(jì)算描述子距離。若主線段及與其合并的線段屬于同一條直線，則合并前后的描述子的漢明距離相差應(yīng)該不會(huì)很大，因此當(dāng)描述子距離大于某個(gè)閾值Dth時(shí)，放棄此次合并，僅保留主線段作為當(dāng)前合并的結(jié)果。

長(zhǎng)線段特征往往來自于紋理比較明顯、梯度較大的區(qū)域，因此長(zhǎng)線段要比短線段更穩(wěn)定。在計(jì)算描述子之前，對(duì)線段進(jìn)行篩選，選取策略如式（7）所示：

式中：WI，HI表示圖像的寬和高，η代表比例系數(shù)。

3 視覺慣性聯(lián)合優(yōu)化

后端視覺慣性聯(lián)合優(yōu)化基于滑動(dòng)窗口模型。窗口長(zhǎng)度設(shè)置為11 幀，第n+N時(shí)刻滑動(dòng)窗口中的優(yōu)化變量定義如下：

式中：χ表示整個(gè)滑動(dòng)窗口中所有的狀態(tài)量，xi表示IMU 在第i幀的狀態(tài)，b表示body 坐標(biāo)系，即IMU 坐標(biāo)系；w表示世界坐標(biāo)系，分別代表在世界坐標(biāo)系下的IMU 位置、姿態(tài)速度以及在body 坐標(biāo)系下IMU 加速度計(jì)（accelerometers）偏置和陀螺儀（gyroscopes）偏置。λ表示空間點(diǎn)特征的逆深度，O表示空間直線的正交表示。

滑窗優(yōu)化基于ceres 庫，最小化目標(biāo)函數(shù)（9）優(yōu)化求解所有狀態(tài)變量。目標(biāo)函數(shù)包括點(diǎn)重投影誤差、線特征重投影誤差、IMU 預(yù)積分誤差以及邊緣化帶來的先驗(yàn)誤差，其中線特征重投影誤差也是基于端點(diǎn)到直線的距離：

式中：rp?JpX 表示在滑動(dòng)窗口中邊緣化一幀后的先驗(yàn)信息，表示兩次IMU 信息之間的測(cè)量殘差，其中B表示滑動(dòng)窗口中的所有預(yù)積分的IMU測(cè)量值；分別表示點(diǎn)投影殘差和線投影殘差，F(xiàn)和L分別表示滑動(dòng)窗口中相機(jī)觀測(cè)到的點(diǎn)特征和線特征，右下角角標(biāo)∑p，表示各殘差項(xiàng)的協(xié)方差矩陣；ρ表示魯棒核函數(shù)，一般采用Cauchy 核函數(shù)或Huber 核函數(shù)。

VINS-Mono 方法認(rèn)為視覺約束項(xiàng)的噪聲協(xié)方差與標(biāo)定相機(jī)內(nèi)參時(shí)的重投影誤差，即偏離像素的個(gè)數(shù)有關(guān)，原作者代碼中使用1.5 個(gè)像素作為協(xié)方差系數(shù)。對(duì)應(yīng)到歸一化相機(jī)平面上的協(xié)方差矩陣需除以焦距f，則信息矩陣等于協(xié)方差矩陣的逆，為：

PL-VIO 以及PL-VINS 方法均沿用了這一策略，對(duì)點(diǎn)和線段的重投影誤差均采用1.5 個(gè)像素的偏差。然而，根據(jù)Y LU 等人在文獻(xiàn)[7]的結(jié)論，點(diǎn)線特征結(jié)合能比單一特征帶來更小的不確定度，而不確定度大小主要體現(xiàn)在協(xié)方差矩陣的大小。另外，由于本文使用的LSD 算法在提取線特征時(shí)對(duì)圖像進(jìn)行了0.5 倍縮放，其不確定度設(shè)為點(diǎn)特征的兩倍，取點(diǎn)特征協(xié)方差系數(shù)為0.707，線特征協(xié)方差系數(shù)為1.414，即加權(quán)后的信息矩陣為：

式中：Λl表示線特征的信息矩陣，表示線特征的協(xié)方差矩陣。

4 閉環(huán)檢測(cè)

系統(tǒng)在長(zhǎng)期運(yùn)行中往往會(huì)產(chǎn)生累積誤差。在沒有外部參照的情況下，累積誤差會(huì)越來越大，導(dǎo)致估計(jì)出來的軌跡偏離實(shí)際位置。閉環(huán)檢測(cè)可以檢測(cè)到相機(jī)是否回到之前經(jīng)過的位置，在當(dāng)前幀和回環(huán)幀之間建立約束，從而可以通過后續(xù)的全局優(yōu)化修正軌跡，減小累積誤差。當(dāng)前主要使用的閉環(huán)檢測(cè)策略為詞袋模型（Bag of Words，BoW）。

4.1 詞袋模型

BoW 是用于衡量?jī)煞鶊D像相似性的一種數(shù)據(jù)結(jié)構(gòu)。如圖5 所示，將視覺特征通過使用K叉樹，在根節(jié)點(diǎn)使用k-means++算法將所有樣本聚類為k類，然后下一層針對(duì)每一類再聚類為k類，聚類d次，最后得到葉節(jié)點(diǎn)，即單詞（Words）。由于不同的特征具有不同的區(qū)分度，常用的相似度計(jì)算使用的是TF-IDF（Term Frequency-Inverse Document Frequency）方 法。TF 的思想是，某一特征在圖像中出現(xiàn)頻率越高則越具有區(qū)分度，IDF 的思想則是某特征在詞典中出現(xiàn)頻率越高，則這一特征越常見，越?jīng)]有特異性。

圖5 K 叉樹詞袋示意圖

IDF 一般是在建立詞典時(shí)計(jì)算的，統(tǒng)計(jì)某個(gè)葉子節(jié)點(diǎn)wi中的特征數(shù)量相對(duì)于所有特征數(shù)量的比例：

式中：ni表示當(dāng)前節(jié)點(diǎn)在詞典中出現(xiàn)的次數(shù)，n表示所有單詞出現(xiàn)的總次數(shù)。顯然，ni越大，當(dāng)前單詞在所有單詞中出現(xiàn)的次數(shù)越多，該單詞的權(quán)重也就越小。

TF指的是該特征在單幅圖像中出現(xiàn)的概率：

在游戲中，設(shè)計(jì)了多種游戲場(chǎng)景來考核學(xué)生的學(xué)習(xí)效果，例如：有任務(wù)場(chǎng)景、闖關(guān)場(chǎng)景、限時(shí)場(chǎng)景等。玩家在游戲中，可以自由選擇進(jìn)入某一場(chǎng)景。通過在游戲中回答問題，獲得或失去一定分?jǐn)?shù)。在游戲的過程中，如果玩家的分?jǐn)?shù)低于指定的分值時(shí)，游戲即判斷玩家失敗。在游戲中為了提高學(xué)習(xí)效率，對(duì)于玩家回答錯(cuò)誤的問題，會(huì)繼續(xù)出現(xiàn)在游戲中，而對(duì)于玩家回答正確的問題，則會(huì)被游戲屏蔽，而不再出現(xiàn)。以下圖2是一次玩家回答問題的圖示：

考慮權(quán)重后，圖像I的特征點(diǎn)可對(duì)應(yīng)許多個(gè)單詞，這就組成了I的詞袋：

給定兩個(gè)圖像的詞袋vA，vB，其相似度評(píng)分可基于L1范數(shù)求解：

4.2 點(diǎn)線特征融合策略

VINS-Mono 方法中，前端對(duì)于點(diǎn)特征匹配使用的是基于Shi-Tomasi 角點(diǎn)特征的光流跟蹤，因此VINSMono 方法在閉環(huán)線程重新提取FAST 角點(diǎn)并計(jì)算BRIEF 描述子，而在前端對(duì)于線特征使用了LSD 和LBD 描述子進(jìn)行線特征匹配。線特征的描述子已經(jīng)被計(jì)算出來，因此可以直接通過ROS 系統(tǒng)將線特征描述子發(fā)布到閉環(huán)節(jié)點(diǎn)中，避免重復(fù)計(jì)算。同時(shí)，也要將線特征發(fā)布到閉環(huán)節(jié)點(diǎn)，后續(xù)用來計(jì)算線特征的分布情況。

在計(jì)算獲得點(diǎn)特征描述子以及線特征描述子后，分別在對(duì)應(yīng)詞袋中進(jìn)行查詢獲得遞減的n個(gè)匹配關(guān)鍵幀的ID 以及相似度得分，得到點(diǎn)特征相似度得分sp和線特征相似度得分s1。由于點(diǎn)特征是在閉環(huán)線程重新提取的FAST 角點(diǎn)，數(shù)量一般在2 000 以上，而前端獲取的線特征數(shù)量往往為100 左右，若基于點(diǎn)線特征的數(shù)量比例對(duì)相似度得分進(jìn)行加權(quán)，則線特征的作用無法體現(xiàn)出來，因此采用固定值wp與w1進(jìn)行加權(quán)。另外，基于點(diǎn)線特征在圖像中分布的均勻情況對(duì)點(diǎn)線特征進(jìn)行加權(quán)，點(diǎn)特征的分布基于其坐標(biāo)求標(biāo)準(zhǔn)差σp，線特征的分布基于線段中點(diǎn)的位置進(jìn)行計(jì)算σ1：

最后，圖像總相似度評(píng)分s如式（18），然后根據(jù)這一得分判斷是否發(fā)生閉環(huán)：

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

5.1 實(shí)驗(yàn)環(huán)境和數(shù)據(jù)集介紹

實(shí)驗(yàn)所用的數(shù)據(jù)集為蘇黎世聯(lián)邦理工學(xué)院開源的雙目+IMU 數(shù)據(jù)集EuRoC[22]，主要采集自廠房和室內(nèi)兩個(gè)環(huán)境，包含從低速運(yùn)動(dòng)到在紋理重復(fù)、紋理缺失、光照變化等復(fù)雜場(chǎng)景下的快速移動(dòng)。實(shí)驗(yàn)基于HP Z460 工作站進(jìn)行，詳細(xì)參數(shù)配置如表1 所示。

表1 實(shí)驗(yàn)環(huán)境參數(shù)配置

為了評(píng)估不同算法的準(zhǔn)確性，通常采用EVO（Evaluation of Odometry and SLAM）與真實(shí)值進(jìn)行時(shí)間戳對(duì)齊以及絕對(duì)軌跡誤差（Absolute Trajectory Error，ATE）計(jì)算，常用的ATE 評(píng)價(jià)參數(shù)為均方根誤差（Root Mean Square Error，RMSE）：

式中：trans表示平移誤差，Test,i表示i時(shí)刻的估計(jì)位姿，Tgt,i表示第i時(shí)刻的真實(shí)位姿。

5.2 改進(jìn)線特征質(zhì)量測(cè)試

選取EuRoC 數(shù)據(jù)集中的MH_01_easy 序列進(jìn)行試驗(yàn)，統(tǒng)計(jì)平均每幀的線段平均長(zhǎng)度和前后兩幀之間成功匹配的線段數(shù)量，對(duì)LSD 算法和改進(jìn)后的LSD 算法進(jìn)行性能分析。結(jié)果顯示，相比于原始的LSD 算法，提取線段的平均長(zhǎng)度提升了19.03%，前后幀間線段匹配數(shù)量平均提升了43.8%。這表明，基于改進(jìn)后的LSD 算法，建立了更多前后幀之間的數(shù)據(jù)關(guān)聯(lián)，從而能夠提升系統(tǒng)的魯棒性。

表2 改進(jìn)前后LSD 算法性能比較

線段提取結(jié)果圖如圖6 所示。圖6（a）為在MH_01_easy 序列中選取的一張圖片，圖6（b）為基于LSD 提取的線段結(jié)果，圖6（c）為對(duì)LSD 提取的結(jié)果直接進(jìn)行長(zhǎng)度閾值篩選的結(jié)果，圖6（d）為對(duì)經(jīng)過線段合并后長(zhǎng)度閾值篩選的結(jié)果。圖6（b）和圖6（d）中的矩形框展示了合并前后的線段，可以看出，提出的線段合并算法將屬于同一條直線的短線段進(jìn)行了合并，因此與圖6（c）相比，圖6（d）增加了更多的長(zhǎng)線段。

圖6 線段提取結(jié)果圖

圖7 為線段匹配結(jié)果圖。圖7（a）展示了基于原始LSD 算法的兩幀間線段匹配結(jié)果，圖7（b）展示了基于合并后的線段的兩幀間匹配結(jié)果。從匹配線的數(shù)量可以看出，基于改進(jìn)的LSD 算法能夠在前后幀之間建立更多的線特征匹配約束，增強(qiáng)了前后幀之間的數(shù)據(jù)關(guān)聯(lián)，從而提升了算法的魯棒性。

圖7 線段匹配結(jié)果圖

5.3 系統(tǒng)精度測(cè)試

表3 展示了不同算法在EuRoC 數(shù)據(jù)集下的表現(xiàn)。其中，PL-VIO 方法為視覺里程計(jì)，沒有閉環(huán)檢測(cè)部分，X 表示跟蹤丟失，加粗部分表示最佳結(jié)果。由表3 可知，在無閉環(huán)的情況下，除了MH_01_easy 和MH_02_easy 兩個(gè)序列，本文提出的算法在其他序列上的性能均優(yōu)于之前的算法；在無閉環(huán)情況下，RMSE較VINS-Mono 方法平均降低了14.90%，較PL-VIO方法平均降低了13.17%，較PL-VINS 方法平均降低了8.96%；在有閉環(huán)的情況下，本文提出的算法也在絕大多數(shù)數(shù)據(jù)集序列上取得了比之前算法更好的效果，RMSE較VINS-Mono 方法平均降低了24.74%，較PLVINS 方法平均降低了5.99%，相較于PL-VINS 方法，本文提出的算法的RMSE在V1_03_difficult 序列降低了19.34%，在V2_03_difficult 序列降低了23.9%。這些序列場(chǎng)景都相對(duì)復(fù)雜，V1_03_difficult 序列運(yùn)動(dòng)快速、場(chǎng)景模糊且有弱光場(chǎng)景，V2_03_difficult 則主要有場(chǎng)景模糊及紋理稀疏問題，這體現(xiàn)了本文提出的算法對(duì)各種場(chǎng)景更具有魯棒性。

表3 不同算法在EuRoC 數(shù)據(jù)集下的均方根誤差

圖8 展示了在序列V2_03_difficul 的算法絕對(duì)軌跡誤差的琴型直方圖對(duì)比圖，其縱軸長(zhǎng)度表示誤差的分布范圍，橫軸寬度表示誤差的分布密度。圖8（a）為無閉環(huán)的結(jié)果，從左到右分別為本文結(jié)果、PL-VINS結(jié)果、PL-VIO 結(jié)果以及VINS-Mono 結(jié)果；圖9（b）為有閉環(huán)的結(jié)果，從左到右分別為本文結(jié)果、PLVINS 結(jié)果以及VINS-Mono 結(jié)果。可以看出，本文提出的算法絕對(duì)軌跡誤差的分布范圍更小，且誤差主要集中在下部，具有更多的小誤差項(xiàng)，即估計(jì)軌跡與真實(shí)軌跡更接近。

圖8 絕對(duì)軌跡誤差對(duì)比圖

6 結(jié)語

本文提出了一種基于點(diǎn)線特征的改進(jìn)視覺慣性SLAM 方法，采用修改隱參數(shù)的LSD 進(jìn)行線特征提取，并對(duì)短線段進(jìn)行合并，增加了成功匹配線段的數(shù)量，增強(qiáng)了幀間數(shù)據(jù)關(guān)聯(lián)，使之更適用于SLAM 系統(tǒng)。另外，通過對(duì)已有文獻(xiàn)結(jié)論的分析和實(shí)驗(yàn)，改進(jìn)了點(diǎn)線和IMU融合誤差項(xiàng)的權(quán)重比，提升了系統(tǒng)整體的精確度。在閉環(huán)檢測(cè)階段，在視覺慣性系統(tǒng)中引入線特征，將前端計(jì)算得到的線段描述子通過ROS 直接發(fā)布到閉環(huán)節(jié)點(diǎn)，然后基于點(diǎn)線特征的數(shù)量和圖像紋理對(duì)圖像相似度進(jìn)行加權(quán)來確定閉環(huán)，增強(qiáng)了系統(tǒng)的魯棒性。