黃 平，曹 鎮(zhèn)，黃俊杰

（哈爾濱工程大學(xué)智能科學(xué)與工程學(xué)院，哈爾濱 150001）

同步定位與地圖構(gòu)建（Simultaneous Localization and Mapping, SLAM）技術(shù)的目的是使機(jī)器人在未知的環(huán)境中，并且不明確自身位置的情況下可以同時(shí)進(jìn)行自身定位與構(gòu)建周圍環(huán)境的地圖，SLAM 是智能機(jī)器人實(shí)現(xiàn)“智能化”的關(guān)鍵技術(shù)[1]。SLAM 作為智能機(jī)器人領(lǐng)域的底層技術(shù)，伴隨著計(jì)算機(jī)技術(shù)[2]和人工智能技術(shù)[3]的發(fā)展，也不斷拓寬智能機(jī)器人的應(yīng)用領(lǐng)域[4]。而視覺(jué)SLAM 的一個(gè)核心部分是使用相機(jī)作為傳感器的視覺(jué)里程計(jì)。

早期的視覺(jué)里程計(jì)使用單目和雙目相機(jī)，但此類相機(jī)并不能直接獲取深度信息，需要根據(jù)圖像中的特征及其匹配關(guān)系，利用計(jì)算機(jī)視覺(jué)中的多視圖幾何關(guān)系構(gòu)建出場(chǎng)景的深度值，并且單目相機(jī)具有尺度不確定性，在使用前需要進(jìn)行初始化確定尺度[5]。而RGB-D相機(jī)利用TOF（Time of Flight）原理直接獲取場(chǎng)景中的深度信息，在視覺(jué)SLAM 中取得了廣泛應(yīng)用。

視覺(jué)里程計(jì)的實(shí)現(xiàn)往往需要從圖像中提取特征，進(jìn)而將特征匹配得到對(duì)應(yīng)關(guān)系，而后才可以由此對(duì)應(yīng)關(guān)系根據(jù)一系列解算方法得到相機(jī)的位姿等信息。因此，使用相機(jī)時(shí)的所處環(huán)境對(duì)算法的性能有著極大的影響。如Rual 等人于2015 年提出的ORB-SLAM[6]是現(xiàn)代SLAM 系統(tǒng)中非常常用的一種，整個(gè)系統(tǒng)圍繞ORB 特征點(diǎn)[7]進(jìn)行計(jì)算，此系統(tǒng)在紋理明顯、點(diǎn)特征豐富的環(huán)境中有著極其優(yōu)秀的表現(xiàn)。但若在點(diǎn)特征極少的環(huán)境中，該算法很可能無(wú)法提取出足夠的特征點(diǎn)，進(jìn)而無(wú)法解算相機(jī)位姿使系統(tǒng)停止工作。如圖1 這樣點(diǎn)特征較少的環(huán)境我們稱之為欠點(diǎn)特征環(huán)境，而現(xiàn)實(shí)生活中，這樣的場(chǎng)景并不少見(jiàn)，完善視覺(jué)里程計(jì)在此類環(huán)境下的應(yīng)對(duì)方法，可以擴(kuò)展視覺(jué)SLAM 系統(tǒng)的使用場(chǎng)景，提高系統(tǒng)的魯棒性，因此也有著較高的實(shí)用價(jià)值。

圖1 點(diǎn)特征較少但線特征豐富的場(chǎng)景Fig.1 Scenes with few point features but rich line features

線特征對(duì)欠特征的環(huán)境下的視覺(jué)里程計(jì)運(yùn)行的穩(wěn)定性有明顯的幫助。引入線特征的SLAM 系統(tǒng)通常使用LSD（Line Segment Detector）提取線特征，LBD（Line Band Descriptor）描述線特征，便于幀之間的線特征匹配，再將線反投影至三維空間中用以位姿估計(jì)。

本文針對(duì)深度相機(jī)存在的深度缺失問(wèn)題，增加深度圖推斷的圖像預(yù)處理步驟，推斷線特征對(duì)應(yīng)的深度圖像位置的深度信息，避免深度缺失；此外在位姿優(yōu)化部分進(jìn)行改進(jìn)，利用擬合直線過(guò)程中的最佳過(guò)點(diǎn)，以及重投影的線段與觀測(cè)線段的角度誤差信息，推導(dǎo)誤差關(guān)于位姿擾動(dòng)的雅克比矩陣，在圖優(yōu)化時(shí)利用重投影誤差優(yōu)化相機(jī)位姿，拓展傳統(tǒng)的優(yōu)化方法。

1 算法框架

本節(jié)介紹基于圖像線特征的視覺(jué)里程計(jì)算法框架。圖2 為視覺(jué)里程計(jì)整體算法框架圖。首先利用已標(biāo)定好的深度相機(jī)獲取彩色圖像及深度圖像。使用LSD 線段檢測(cè)算法[8]提取彩色圖像中的2D 線特征，而后確定包含線特征的各矩形區(qū)域，接著將各矩形區(qū)域映射到深度圖中，針對(duì)深度缺失問(wèn)題，對(duì)各矩形區(qū)域使用像素濾波方法進(jìn)行深度推斷。之后均勻采樣2D線特征中的點(diǎn)，結(jié)合深度推斷后的深度圖反投影為3D點(diǎn)，利用RANSAC 方法[9]找到過(guò)直線的最佳兩點(diǎn)擬合3D 直線，根據(jù)3D 直線的匹配關(guān)系估計(jì)相機(jī)位姿。最后，構(gòu)建包含距離誤差及角度誤差的雅克比矩陣，利用高斯-牛頓法優(yōu)化位姿。

圖2 算法框架圖Fig.2 Algorithm frame diagram

2 線特征的檢測(cè)與匹配

本文使用LSD 線段檢測(cè)算法提取圖像中的線特征，線特征為灰度值變化明顯的連續(xù)區(qū)域，該算法的優(yōu)勢(shì)是可以在線性時(shí)間內(nèi)得到亞像素級(jí)的線段信息。它通過(guò)計(jì)算像素梯度以及梯度法相，找到圖像亮度變化較大的一些點(diǎn)，當(dāng)這些點(diǎn)位置相鄰且梯度方向相近時(shí)，則構(gòu)成了直線段。如圖3 為實(shí)際場(chǎng)景中采取圖片提取出的線特征信息。

圖3 LSD 算法提取線特征Fig.3 LSD Algorithm to Extract Line Features

為了能將線特征進(jìn)行匹配，本文使用二進(jìn)制描述子LBD[10]進(jìn)行線特征的描述。通過(guò)計(jì)算描述子間的漢明距離采用窮舉匹配法即可找到匹配關(guān)系。

如圖4 即為兩幅圖像間線特征的匹配結(jié)果，而在本文視覺(jué)里程計(jì)實(shí)現(xiàn)中，我們將當(dāng)前幀中二維線特征反投影成的三維直線，同樣利用LBD 描述子，與局部地圖中的三維直線相匹配。

圖4 特征匹配示例Fig.4 Examples of feature matching

3 三維直線的擬合估計(jì)

3.1 深度圖推斷

本文利用線特征反投影擬合出的三維直線的匹配關(guān)系估計(jì)相機(jī)的位姿，因此，場(chǎng)景中采集到的二維線段變換到三維直線的估計(jì)過(guò)程非常重要，極大地影響著位姿估計(jì)的準(zhǔn)確性。擬合過(guò)程中需要將二維信息根據(jù)RGB-D 相機(jī)采集到的深度值反投影為三維信息，而一般的RGB-D 相機(jī)會(huì)有深度缺失問(wèn)題，表現(xiàn)為深度圖中灰度值為0 的純黑色區(qū)域，此缺陷會(huì)導(dǎo)致二維特征無(wú)法反投影為三維特征，因此，擬合三維直線前，我們首先進(jìn)行深度圖推斷。

深度圖推斷的本質(zhì)是圖像的濾波，圖像濾波的方式有很多：如雙邊濾波、均值濾波等，但雙邊濾波對(duì)深度的推斷效果不明顯，而均值濾波又無(wú)法很好地保證原圖像的邊緣特性，因此本文采用像素濾波的方式首先推斷出深度圖中缺失的像素灰度值，而后采用滑動(dòng)窗方式的中值濾波降低噪聲的同時(shí)保留好邊緣信息。首先是像素濾波的處理，像素濾波的核心思想是使用待推斷像素點(diǎn)周圍的像素灰度值估計(jì)出待推斷像素點(diǎn)灰度值。如圖5 所示，像素濾波器往往使用兩個(gè)環(huán)帶，本文中使用的內(nèi)環(huán)帶為8 個(gè)像素點(diǎn)，外環(huán)帶為16 個(gè)像素點(diǎn)。

圖5 像素濾波器Fig.5 Pixel filter

將矩形待推斷區(qū)域內(nèi)灰度值為0 的像素定義為候選像素，以候選像素為中心使用如圖5 所示的像素濾波器，該算法可表示為：

式(1)中，Dcenter為中心候選像素點(diǎn)推斷后的灰度值，Dbefore為該點(diǎn)推斷前已有灰度值， c1、c2分別為像素濾波器內(nèi)層和外層中灰度值非零的個(gè)數(shù)，1T 、2T分別為內(nèi)層和外層中灰度值非零的個(gè)數(shù)的閾值，M 為內(nèi)外兩層中非零灰度值的眾數(shù)。因此，只有當(dāng)內(nèi)外兩層非零灰度像素個(gè)數(shù)都超過(guò)設(shè)定閾值且候選像素原灰度值為零時(shí)才進(jìn)行深度推斷，并將候選像素的灰度值設(shè)定為內(nèi)外兩層灰度中的眾數(shù)。

以上便完成了深度推斷的部分。在深度推斷后往往還需要進(jìn)行進(jìn)一步的濾波處理來(lái)降低圖像中的噪聲，而不能破壞原有特征，需要保留好邊緣的特征，最后采用中值濾波的方式對(duì)深度圖進(jìn)一步處理保留邊緣信息。

圖6 為經(jīng)過(guò)像素濾波與中值濾波后的深度圖變化情況，為便于比較，將灰度值為0 的像素用紅色表示，可以看出深度空洞的區(qū)域明顯減少。

圖6 深度推斷前后對(duì)比圖Fig.6 Contrast before and after depth inference

3.2 二維線段的采樣與反投影

在位姿估計(jì)的過(guò)程中需要使用三維直線信息，因此二維線段的反投影過(guò)程及其重要，接下來(lái)重點(diǎn)闡述將二維線段反投影為三維直線的過(guò)程。

為方便討論，我們以一條直線的三維估計(jì)為例進(jìn)行討論。彩色圖像記為G，G 中的任意一2D 點(diǎn)坐標(biāo)記為 gj∈G，gj=[ujvj]T。根據(jù)針孔相機(jī)模型，得到gj在相機(jī)坐標(biāo)系下的坐標(biāo)為：

式(2)即為坐標(biāo)反投影公式，其中，cu,cv為相機(jī)光心坐標(biāo)， fu,fv為焦距，s為尺度因子， dj為 gj所對(duì)應(yīng)的深度值。

一種直觀的獲取三維直線的方法是使用二維線段的端點(diǎn)，將二維線段的兩個(gè)端點(diǎn)反投影成為3D 端點(diǎn)，過(guò)兩個(gè)點(diǎn)自然可以得到三維直線。但是這種方法有著很大的缺陷，通過(guò)式(2)可知，深度值的準(zhǔn)確性決定著三維點(diǎn)估計(jì)的準(zhǔn)確性，因此，若2D 線段端點(diǎn)的深度值不準(zhǔn)確，那么估計(jì)出的3D 直線將具有很大偏差，在后續(xù)的位姿估計(jì)計(jì)算中也將帶有很大的誤差。因此我們不再僅僅只用二維線段的兩個(gè)端點(diǎn)，而采用在二維線段上均勻采樣多點(diǎn)的方式進(jìn)行三維直線的估計(jì)。如圖7 所示，為了減少計(jì)算量，我們僅對(duì)那些能匹配成功的二維線段進(jìn)行2D 點(diǎn)采樣，對(duì)于一條二維線段l ，采樣后得到點(diǎn)集 P2D={ p1, p2... pn}∈l ，對(duì)于P2D中深度值為0 的2D 點(diǎn)直接剔除，其中n 是2D 點(diǎn)的個(gè)數(shù)，對(duì)于 P2D中的所有2D 點(diǎn)利用式(2)即可得到l對(duì)應(yīng)的3D 點(diǎn)集合，記為 P3D={ P1, P2... Pn}。

圖7 二維線段的采樣Fig.7 Sampling of 2-D Line Segments

3.3 三維直線的RANSAC 估計(jì)

空間直線的表示方法有很多，如圖8 所示，這里我們通過(guò)兩個(gè)向量( u,d )表示[11]。

圖8 三維直線表示Fig.8 3D straight line representation

記三維直線為L(zhǎng)，u是L 的單位方向向量，d 的模為相機(jī)原點(diǎn)到空間直線的距離，d 的方向平行于由相機(jī)原點(diǎn)與空間直線所構(gòu)成的平面的法向量n。假設(shè)m 是直線上的任意一點(diǎn)，則d 與u的關(guān)系如下：

其中^運(yùn)算為叉乘。

綜上，為了確定一條三維直線，我們需要找出的就是 P3D={ P1,P2,...

Pn}中最佳過(guò)直線兩點(diǎn)，該過(guò)程如圖9 所示，本文使用如表1 所示的RANSAC 方法擬合出三維直線。

圖9 三維直線擬合過(guò)程Fig.9 3D straight line fitting process

表1 RANSAC 選擇最佳過(guò)直線兩點(diǎn)算法Tab.1 RANSAC method selects the best two points to cross the straight line

已知最佳過(guò)直線的兩點(diǎn)自然可以按上述直線表示法得到三維直線，綜上，完成了2D 線段反投影為3D 直線。

4 位姿估計(jì)

針對(duì)當(dāng)前幀和參考幀（也可以是局部地圖）中匹配成功的三維直線，位姿變換前( ur, dr)與變換后( uc, dc)的約束關(guān)系為：

其中R 是3 × 3旋轉(zhuǎn)矩陣，t 為平移向量。首先計(jì)算旋轉(zhuǎn)，由 uc=Rur計(jì)算如下目標(biāo)函數(shù)：

為便于計(jì)算，將旋轉(zhuǎn)矩陣R 轉(zhuǎn)化為單位四元數(shù)q表示。則式(5)可以轉(zhuǎn)化為：

其中， u^為u 的反對(duì)稱矩陣，結(jié)合式(7)進(jìn)一步得到：

接下來(lái)，計(jì)算平移，由 dc=Rdc+ uc^t 計(jì)算如下目標(biāo)函數(shù)：

式(10)對(duì)t 求偏導(dǎo)，并令偏導(dǎo)為0，得到：

若式(12)滿秩，則平移向量為：

最后，利用RANSAC 選取最合適的位姿。由三組匹配直線計(jì)算出初始位姿，而后計(jì)算出其他匹配線在該位姿下的投影誤差，設(shè)定閾值，統(tǒng)計(jì)內(nèi)外點(diǎn)個(gè)數(shù)；循環(huán)以上過(guò)程，最終選取內(nèi)點(diǎn)數(shù)最多的一組為此兩相鄰幀間的位姿變換結(jié)果。

5 位姿優(yōu)化

在完成初步的位姿估計(jì)后，需要對(duì)估計(jì)出的位姿進(jìn)一步優(yōu)化，建立觀測(cè)特征與位姿的約束關(guān)系，利用圖優(yōu)化方法提高位姿估計(jì)的精度，本節(jié)主要論述利用線特征的圖優(yōu)化位姿方法，以及加入角度信息后進(jìn)一步提高精度的優(yōu)化方法。

5.1 利用線特征的位姿優(yōu)化

本節(jié)利用匹配好的線特征信息進(jìn)行位姿優(yōu)化，類似于使用點(diǎn)特征的優(yōu)化方法，使用重投影誤差作為誤差量，將局部地圖中世界坐標(biāo)系下的三維直線重投影到當(dāng)前幀，得到重投影二維線段，減小該線段與當(dāng)前幀觀測(cè)二維線段的誤差，從而達(dá)到優(yōu)化位姿的目的，具體實(shí)施方式如圖10 所示。

我們?cè)?.3 節(jié)利用RANSAC方法選擇出了最佳的過(guò)直線兩點(diǎn)，過(guò)此兩點(diǎn)構(gòu)成了一條三維直線，即如圖10 所示中的三維直線PQ，該直線在局部地圖中的世界坐標(biāo)系中。

圖10 線段的重投影誤差Fig.10 Re-projection Error of Line Segment

首先，將世界坐標(biāo)系中的直線投影到相機(jī)坐標(biāo)系中，將最佳過(guò)直線的兩個(gè)3D 點(diǎn)投影至相機(jī)系便可以得到相機(jī)系下的直線，因此該問(wèn)題轉(zhuǎn)化為了3D 點(diǎn)的重投影過(guò)程，以P 點(diǎn)為例，設(shè)該空間點(diǎn)坐標(biāo)為P=[ X , Y , Z]T， 它對(duì)應(yīng)的投影相機(jī)系坐標(biāo)為P'=[ X ', Y ', Z ']T，像素系坐標(biāo)為p'=[ x,y]T，首先將世界系中的坐標(biāo)轉(zhuǎn)換為相機(jī)系下坐標(biāo)，并取其前三維：

其中ξ 是李代數(shù)位姿，^是反對(duì)稱矩陣運(yùn)算。接下來(lái)，轉(zhuǎn)換到像素坐標(biāo)系：

其中，s 是縮放系數(shù)，K 是相機(jī)內(nèi)參矩陣，對(duì)式(15)展開(kāi)有：

其中， cx,cy為相機(jī)光心坐標(biāo)， fx,fy為焦距，消去s便可得到像素系下的坐標(biāo)：

我們所建立的誤差量是投影到像素系的點(diǎn)p' 和q'到當(dāng)前幀觀測(cè)線段lpixel_2D的距離。當(dāng)位姿估計(jì)完全準(zhǔn)確時(shí)，理論上由點(diǎn)p' 和q'構(gòu)成的重投影線段 l3D→2D應(yīng)與觀測(cè)線段lpixel_2D完全重合，那么點(diǎn)p' 和q' 到lpixel_2D的距離應(yīng)為0，因此我們選擇此距離作為誤差量，通過(guò)迭代優(yōu)化不斷減小此誤差從而達(dá)到優(yōu)化位姿的目的。為方便敘述，以其中一個(gè)點(diǎn)p' 到lpixel_2D的距離為例，點(diǎn)q'到lpixel_2D的距離同理。

設(shè)已知觀測(cè)線lpixel_2D上的兩個(gè)端點(diǎn)p=[ x1,y1]T，q=[ x2, y2]T。則p' 到lpixel_2D的距離：

式(18)中由于分母是常數(shù)，則誤差：

誤差 e1求取對(duì)位姿擾動(dòng)的雅克比矩陣，這里相機(jī)位姿使用李代數(shù)ξ 表示，根據(jù)鏈?zhǔn)角髮?dǎo)法則，有：

對(duì)于等式第一項(xiàng)，有：

對(duì)于第二項(xiàng)，有：

則得到1× 6的雅克比矩陣：

以上完成了利用線特征信息的重投影誤差對(duì)位姿雅克比矩陣的推導(dǎo)，在實(shí)踐中，調(diào)用G2O 圖優(yōu)化庫(kù)[12]，以相機(jī)位姿為圖優(yōu)化頂點(diǎn)，以重投影誤差為邊構(gòu)建圖優(yōu)化模型，利用該雅克比矩陣便可結(jié)合高斯牛頓或列文伯格-馬夸爾特方法進(jìn)行迭代優(yōu)化。

5.2 加入角度信息的位姿優(yōu)化

為進(jìn)一步提高優(yōu)化效果，可以在重投影的線段與觀測(cè)線段中構(gòu)建一個(gè)角度誤差信息，如圖10 所示，定義誤差：

則此誤差對(duì)位姿擾動(dòng)的雅克比矩陣為：

由于 eang的定義是cos＜qp ,qp' ＞，當(dāng)測(cè)量線段與觀測(cè)線段接近重合時(shí)，＜qp,qp'＞→0o， 有cos＜qp ,qp'＞→1， 因此我們可以設(shè)觀測(cè)量measurement 恒為1，則：

誤差越接近0 越好，同上一小節(jié)，通過(guò)高斯牛頓或列文伯格-馬夸爾特方法迭代使誤差下降，從而得到更優(yōu)的位姿結(jié)果。

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

本文采用一臺(tái)筆記本PC 機(jī)運(yùn)行和調(diào)試算法，配置為Intel 酷睿i5 四核2.80GHz，內(nèi)存為8G，操作系統(tǒng)為Ubuntu16.04LTS 版本。本文使用TUM 公開(kāi)的RGB-D 數(shù)據(jù)集，對(duì)所述視覺(jué)里程計(jì)算法采用如圖11所示的RGB-D 數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)，具體序列名稱為freiburg3_large_cabinet，序列內(nèi)容為相機(jī)圍繞一個(gè)大辦公柜轉(zhuǎn)了一圈，櫥柜表面可選取的特征點(diǎn)很少，櫥柜結(jié)構(gòu)簡(jiǎn)單，大多是平面和直角。評(píng)測(cè)使用evo 評(píng)測(cè)工具對(duì)運(yùn)行結(jié)果評(píng)測(cè)。

圖11 數(shù)據(jù)集示例Fig.11 Data set example

當(dāng)相機(jī)運(yùn)動(dòng)過(guò)快或場(chǎng)景中點(diǎn)特征稀少時(shí)，會(huì)丟失很多點(diǎn)特征的匹配對(duì)，導(dǎo)致以點(diǎn)特征為核心的視覺(jué)里程計(jì)無(wú)法再繼續(xù)跟蹤[13]，但使用線特征可以對(duì)此環(huán)境有一定抵抗性，本節(jié)對(duì)比僅使用點(diǎn)特征的里程計(jì)和使用本文所述線特征里程計(jì)運(yùn)行的結(jié)果。

如圖12 是僅使用ORB 點(diǎn)特征的里程計(jì)運(yùn)行數(shù)據(jù)結(jié)果，其中圖12(a)為相機(jī)運(yùn)行的俯視圖軌跡坐標(biāo)，圖12(b)為世界坐標(biāo)系下xyz 三個(gè)方向的坐標(biāo)，虛線部分皆為實(shí)際軌跡坐標(biāo)投影，實(shí)線部分為計(jì)算出的相機(jī)運(yùn)動(dòng)坐標(biāo)。該數(shù)據(jù)集的點(diǎn)特征較少，因此僅使用點(diǎn)特征的里程計(jì)沒(méi)有很好地持續(xù)跟蹤，由圖12 可見(jiàn)，在17 s左右時(shí)無(wú)法繼續(xù)解算出位姿結(jié)果，表示實(shí)際解算結(jié)果的藍(lán)色線中斷。在可跟蹤部分，計(jì)算出的RMSE 為0.620 m。

圖12 僅使用ORB 點(diǎn)特征VO 運(yùn)行軌跡Fig.12 VO trajectory using ORB point features only

接下來(lái)使用本文所述線特征里程計(jì)運(yùn)行該數(shù)據(jù)集，結(jié)果如圖13 所示。

從圖13 我們看到里程計(jì)并沒(méi)有完全進(jìn)行跟蹤，在18 s 左右表示實(shí)際解算結(jié)果的藍(lán)色線中斷，由于未采用任何優(yōu)化措施，導(dǎo)致后續(xù)連續(xù)幾個(gè)幀的位姿結(jié)果較差，因不會(huì)向局部地圖中添加任何線特征，導(dǎo)致后續(xù)幀沒(méi)有能夠匹配的特征，最終無(wú)法解算而導(dǎo)致跟蹤失敗，在可跟蹤部分的RMSE 為0.050 m。最后使用本文線特征里程計(jì)，且加入了誤差優(yōu)化方法后的結(jié)果如圖14 所示，此時(shí)里程計(jì)可以做到完整跟蹤。

圖13 本文的VO 運(yùn)行軌跡（未加入優(yōu)化）Fig.13 VO trajectory in article（Optimization not added）

圖14 本文的VO 運(yùn)行軌跡（加入優(yōu)化）Fig.14 VO trajectory in article（Optimization added）

由于本文使用線特征，線特征固有的結(jié)構(gòu)性信息可以帶來(lái)一定的精度提升，且本文在優(yōu)化過(guò)程中使用擬合直線過(guò)程中最佳過(guò)點(diǎn)到觀測(cè)直線的距離，以及引入一個(gè)角度誤差，進(jìn)一步優(yōu)化了位姿結(jié)果。跟蹤完成后計(jì)算出的RMSE 為0.058 m。

ORB-SLAM2 作為最優(yōu)秀的以點(diǎn)特征為核心的SLAM 系統(tǒng)之一，本文也對(duì)其在該序列上的表現(xiàn)進(jìn)行了測(cè)試。如圖15 所示，默認(rèn)參數(shù)下，ORB-SLAM2未能完整跟蹤數(shù)據(jù)，在成功跟蹤部分 RMSE 為0.050 m。本質(zhì)上算法在兩幀之間跟蹤成功需要足夠的特征點(diǎn)，增加圖像中提取的特征點(diǎn)數(shù)量就可以提高跟蹤的成功率。在ORB-SLAM2 代碼配置參數(shù)文件中，OR Bextractor.iniThFAST 對(duì)提取的特征點(diǎn)數(shù)量有明顯的影響，該參數(shù)是提取FAST 角點(diǎn)時(shí)的閾值，當(dāng)角點(diǎn)值大于閾值時(shí)才會(huì)被成功提取，降低該值意味著提取的角點(diǎn)質(zhì)量下降，數(shù)量增加。

圖15 默認(rèn)參數(shù)ORB-SLAM2 運(yùn)行軌跡Fig.15 Default parameter ORB-SLAM2 trajectory

將該參數(shù)值從默認(rèn)的20 降為15 時(shí)，ORB-SLAM2能夠成功完整跟蹤數(shù)據(jù)，如圖16 所示。跟蹤完成后計(jì)算出的RMSE 為0.159 m。

圖16 降角點(diǎn)閾值ORB-SLAM2 運(yùn)行軌跡Fig.16 Reduced corner points threshold ORB-SLAM2 trajectory

為了方便比較算法效果，將上述實(shí)驗(yàn)結(jié)果整合列表展示，如表2 所示，此序列在各視覺(jué)里程計(jì)的運(yùn)行情況如下?？梢钥吹郊尤雰?yōu)化的本文算法相比降角點(diǎn)閾值ORB-SLAM2 的軌跡估計(jì)精度提高63%，并能完整地跟蹤軌跡。

表2 數(shù)據(jù)在各視覺(jué)里程計(jì)算法中的運(yùn)行情況Tab.2 Operation of data in each visual odometry

圖17 是兩種里程計(jì)運(yùn)行時(shí)的世界坐標(biāo)系下xyz坐標(biāo)的對(duì)比，藍(lán)色線為本文視覺(jué)里程計(jì)方法計(jì)算出的軌跡坐標(biāo)，綠色線為僅使用ORB 特征點(diǎn)的視覺(jué)里程計(jì)方法計(jì)算出的軌跡坐標(biāo)，使用本文的視覺(jué)里程計(jì)方法，會(huì)使跟蹤結(jié)果更加貼近真實(shí)軌跡，且跟蹤更加長(zhǎng)遠(yuǎn)。

圖17 兩種視覺(jué)里程計(jì)的位置估計(jì)對(duì)比Fig.17 Comparison of Position Estimation of Two VO

在當(dāng)前實(shí)驗(yàn)環(huán)境下，本文視覺(jué)里程計(jì)各部分運(yùn)行時(shí)間如表3 所示?？梢钥吹剿惴ㄟ\(yùn)行的時(shí)間主要消耗在特征提取及描述和地圖維護(hù)部分，平均一幀運(yùn)行時(shí)間約214 ms，即為5 Hz 的運(yùn)行頻率，滿足一般使用環(huán)境中的實(shí)時(shí)性要求。

表3 線特征VO 部分運(yùn)行時(shí)間統(tǒng)計(jì)Tab.3 Statistics of Partial Run Time of Line Feature VO

在目前所流行的SLAM 系統(tǒng)中，ORB-SLAM2 是最優(yōu)秀的以點(diǎn)特征為核心的SLAM 系統(tǒng)之一，無(wú)論在精度還是在實(shí)時(shí)性方面都有著極高的表現(xiàn)，但由于采用ORB 點(diǎn)特征，在欠點(diǎn)特征下不如線特征里程計(jì)魯棒。本文僅使用線特征，在優(yōu)化方面采用不同的方法，使用擬合直線過(guò)程中的過(guò)點(diǎn)，引入一個(gè)角度誤差信息，拓展了線特征里程計(jì)優(yōu)化方法。本節(jié)實(shí)驗(yàn)中，本文提出的一種基于線特征的RGB-D 視覺(jué)里程計(jì)算法表現(xiàn)優(yōu)于ORB-SLAM2，并且滿足一般環(huán)境的實(shí)時(shí)性需求。

7 結(jié) 論

本文構(gòu)建了以線特征為核心的線特征視覺(jué)里程計(jì)框架。針對(duì)深度相機(jī)存在的深度缺失問(wèn)題，增加深度圖推斷的圖像預(yù)處理步驟，使三維直線的估計(jì)更加準(zhǔn)確。使用LSD 進(jìn)行線特征提取并且使用LBD 描述子進(jìn)行描述以便進(jìn)行特征匹配。利用RANSAC 方法，推導(dǎo)了二維線段反投影擬合為三維直線的過(guò)程。本文在位姿優(yōu)化部分進(jìn)行改進(jìn)，充分使用擬合三維直線過(guò)程中的最佳過(guò)直線兩點(diǎn)而非傳統(tǒng)的線段端點(diǎn)增加可靠性，并且加入了一個(gè)角度誤差信息，推導(dǎo)了誤差關(guān)于位姿擾動(dòng)的雅克比矩陣，實(shí)現(xiàn)圖優(yōu)化過(guò)程，該優(yōu)化方法使得視覺(jué)里程計(jì)位姿估計(jì)結(jié)果更加準(zhǔn)確。

使用線特征的視覺(jué)里程計(jì)相比于點(diǎn)特征里程計(jì)的計(jì)算量更大，特別是利用RANSAC 進(jìn)行三維線段擬合時(shí)，算法復(fù)雜度較高，當(dāng)線特征數(shù)量增加時(shí)，計(jì)算量增長(zhǎng)較大，實(shí)時(shí)性并不如點(diǎn)特征，需要控制線特征數(shù)量保持在合理的范圍，在未來(lái)有待進(jìn)一步優(yōu)化。