一種基于線特征的RGB-D 視覺里程計算法

黃 平，曹 鎮，黃俊杰

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

同步定位與地圖構建（Simultaneous Localization and Mapping, SLAM）技術的目的是使機器人在未知的環境中，并且不明確自身位置的情況下可以同時進行自身定位與構建周圍環境的地圖，SLAM 是智能機器人實現“智能化”的關鍵技術[1]。SLAM 作為智能機器人領域的底層技術，伴隨著計算機技術[2]和人工智能技術[3]的發展，也不斷拓寬智能機器人的應用領域[4]。而視覺SLAM 的一個核心部分是使用相機作為傳感器的視覺里程計。

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

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

圖1 點特征較少但線特征豐富的場景Fig.1 Scenes with few point features but rich line features

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

本文針對深度相機存在的深度缺失問題，增加深度圖推斷的圖像預處理步驟，推斷線特征對應的深度圖像位置的深度信息，避免深度缺失；此外在位姿優化部分進行改進，利用擬合直線過程中的最佳過點，以及重投影的線段與觀測線段的角度誤差信息，推導誤差關于位姿擾動的雅克比矩陣，在圖優化時利用重投影誤差優化相機位姿，拓展傳統的優化方法。

1 算法框架

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

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

2 線特征的檢測與匹配

本文使用LSD 線段檢測算法提取圖像中的線特征，線特征為灰度值變化明顯的連續區域，該算法的優勢是可以在線性時間內得到亞像素級的線段信息。它通過計算像素梯度以及梯度法相，找到圖像亮度變化較大的一些點，當這些點位置相鄰且梯度方向相近時，則構成了直線段。如圖3 為實際場景中采取圖片提取出的線特征信息。

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

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

如圖4 即為兩幅圖像間線特征的匹配結果，而在本文視覺里程計實現中，我們將當前幀中二維線特征反投影成的三維直線，同樣利用LBD 描述子，與局部地圖中的三維直線相匹配。

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

3 三維直線的擬合估計

3.1 深度圖推斷

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

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

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

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

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

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

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

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

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

在位姿估計的過程中需要使用三維直線信息，因此二維線段的反投影過程及其重要，接下來重點闡述將二維線段反投影為三維直線的過程。

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

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

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

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

3.3 三維直線的RANSAC 估計

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

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

記三維直線為L，u是L 的單位方向向量，d 的模為相機原點到空間直線的距離，d 的方向平行于由相機原點與空間直線所構成的平面的法向量n。假設m 是直線上的任意一點，則d 與u的關系如下：

其中^運算為叉乘。

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

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

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

表1 RANSAC 選擇最佳過直線兩點算法Tab.1 RANSAC method selects the best two points to cross the straight line

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

4 位姿估計

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

其中R 是3 × 3旋轉矩陣，t 為平移向量。首先計算旋轉，由 uc=Rur計算如下目標函數：

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

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

接下來，計算平移，由 dc=Rdc+ uc^t 計算如下目標函數：

式(10)對t 求偏導，并令偏導為0，得到：

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

最后，利用RANSAC 選取最合適的位姿。由三組匹配直線計算出初始位姿，而后計算出其他匹配線在該位姿下的投影誤差，設定閾值，統計內外點個數；循環以上過程，最終選取內點數最多的一組為此兩相鄰幀間的位姿變換結果。

5 位姿優化

在完成初步的位姿估計后，需要對估計出的位姿進一步優化，建立觀測特征與位姿的約束關系，利用圖優化方法提高位姿估計的精度，本節主要論述利用線特征的圖優化位姿方法，以及加入角度信息后進一步提高精度的優化方法。

5.1 利用線特征的位姿優化

本節利用匹配好的線特征信息進行位姿優化，類似于使用點特征的優化方法，使用重投影誤差作為誤差量，將局部地圖中世界坐標系下的三維直線重投影到當前幀，得到重投影二維線段，減小該線段與當前幀觀測二維線段的誤差，從而達到優化位姿的目的，具體實施方式如圖10 所示。

我們在3.3 節利用RANSAC方法選擇出了最佳的過直線兩點，過此兩點構成了一條三維直線，即如圖10 所示中的三維直線PQ，該直線在局部地圖中的世界坐標系中。

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

首先，將世界坐標系中的直線投影到相機坐標系中，將最佳過直線的兩個3D 點投影至相機系便可以得到相機系下的直線，因此該問題轉化為了3D 點的重投影過程，以P 點為例，設該空間點坐標為P=[ X , Y , Z]T， 它對應的投影相機系坐標為P'=[ X ', Y ', Z ']T，像素系坐標為p'=[ x,y]T，首先將世界系中的坐標轉換為相機系下坐標，并取其前三維：

其中ξ 是李代數位姿，^是反對稱矩陣運算。接下來，轉換到像素坐標系：

其中，s 是縮放系數，K 是相機內參矩陣，對式(15)展開有：

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

我們所建立的誤差量是投影到像素系的點p' 和q'到當前幀觀測線段lpixel_2D的距離。當位姿估計完全準確時，理論上由點p' 和q'構成的重投影線段 l3D→2D應與觀測線段lpixel_2D完全重合，那么點p' 和q' 到lpixel_2D的距離應為0，因此我們選擇此距離作為誤差量，通過迭代優化不斷減小此誤差從而達到優化位姿的目的。為方便敘述，以其中一個點p' 到lpixel_2D的距離為例，點q'到lpixel_2D的距離同理。

設已知觀測線lpixel_2D上的兩個端點p=[ x1,y1]T，q=[ x2, y2]T。則p' 到lpixel_2D的距離：

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

誤差 e1求取對位姿擾動的雅克比矩陣，這里相機位姿使用李代數ξ 表示，根據鏈式求導法則，有：

對于等式第一項，有：

對于第二項，有：

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

以上完成了利用線特征信息的重投影誤差對位姿雅克比矩陣的推導，在實踐中，調用G2O 圖優化庫[12]，以相機位姿為圖優化頂點，以重投影誤差為邊構建圖優化模型，利用該雅克比矩陣便可結合高斯牛頓或列文伯格-馬夸爾特方法進行迭代優化。

5.2 加入角度信息的位姿優化

為進一步提高優化效果，可以在重投影的線段與觀測線段中構建一個角度誤差信息，如圖10 所示，定義誤差：

則此誤差對位姿擾動的雅克比矩陣為：

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

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

6 實驗結果及分析

本文采用一臺筆記本PC 機運行和調試算法，配置為Intel 酷睿i5 四核2.80GHz，內存為8G，操作系統為Ubuntu16.04LTS 版本。本文使用TUM 公開的RGB-D 數據集，對所述視覺里程計算法采用如圖11所示的RGB-D 數據集進行實驗，具體序列名稱為freiburg3_large_cabinet，序列內容為相機圍繞一個大辦公柜轉了一圈，櫥柜表面可選取的特征點很少，櫥柜結構簡單，大多是平面和直角。評測使用evo 評測工具對運行結果評測。

圖11 數據集示例Fig.11 Data set example

當相機運動過快或場景中點特征稀少時，會丟失很多點特征的匹配對，導致以點特征為核心的視覺里程計無法再繼續跟蹤[13]，但使用線特征可以對此環境有一定抵抗性，本節對比僅使用點特征的里程計和使用本文所述線特征里程計運行的結果。

如圖12 是僅使用ORB 點特征的里程計運行數據結果，其中圖12(a)為相機運行的俯視圖軌跡坐標，圖12(b)為世界坐標系下xyz 三個方向的坐標，虛線部分皆為實際軌跡坐標投影，實線部分為計算出的相機運動坐標。該數據集的點特征較少，因此僅使用點特征的里程計沒有很好地持續跟蹤，由圖12 可見，在17 s左右時無法繼續解算出位姿結果，表示實際解算結果的藍色線中斷。在可跟蹤部分，計算出的RMSE 為0.620 m。

圖12 僅使用ORB 點特征VO 運行軌跡Fig.12 VO trajectory using ORB point features only

接下來使用本文所述線特征里程計運行該數據集，結果如圖13 所示。

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

圖13 本文的VO 運行軌跡（未加入優化）Fig.13 VO trajectory in article（Optimization not added）

圖14 本文的VO 運行軌跡（加入優化）Fig.14 VO trajectory in article（Optimization added）

由于本文使用線特征，線特征固有的結構性信息可以帶來一定的精度提升，且本文在優化過程中使用擬合直線過程中最佳過點到觀測直線的距離，以及引入一個角度誤差，進一步優化了位姿結果。跟蹤完成后計算出的RMSE 為0.058 m。

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

圖15 默認參數ORB-SLAM2 運行軌跡Fig.15 Default parameter ORB-SLAM2 trajectory

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

圖16 降角點閾值ORB-SLAM2 運行軌跡Fig.16 Reduced corner points threshold ORB-SLAM2 trajectory

為了方便比較算法效果，將上述實驗結果整合列表展示，如表2 所示，此序列在各視覺里程計的運行情況如下。可以看到加入優化的本文算法相比降角點閾值ORB-SLAM2 的軌跡估計精度提高63%，并能完整地跟蹤軌跡。

表2 數據在各視覺里程計算法中的運行情況Tab.2 Operation of data in each visual odometry

圖17 是兩種里程計運行時的世界坐標系下xyz坐標的對比，藍色線為本文視覺里程計方法計算出的軌跡坐標，綠色線為僅使用ORB 特征點的視覺里程計方法計算出的軌跡坐標，使用本文的視覺里程計方法，會使跟蹤結果更加貼近真實軌跡，且跟蹤更加長遠。

圖17 兩種視覺里程計的位置估計對比Fig.17 Comparison of Position Estimation of Two VO

在當前實驗環境下，本文視覺里程計各部分運行時間如表3 所示。可以看到算法運行的時間主要消耗在特征提取及描述和地圖維護部分，平均一幀運行時間約214 ms，即為5 Hz 的運行頻率，滿足一般使用環境中的實時性要求。

表3 線特征VO 部分運行時間統計Tab.3 Statistics of Partial Run Time of Line Feature VO

在目前所流行的SLAM 系統中，ORB-SLAM2 是最優秀的以點特征為核心的SLAM 系統之一，無論在精度還是在實時性方面都有著極高的表現，但由于采用ORB 點特征，在欠點特征下不如線特征里程計魯棒。本文僅使用線特征，在優化方面采用不同的方法，使用擬合直線過程中的過點，引入一個角度誤差信息，拓展了線特征里程計優化方法。本節實驗中，本文提出的一種基于線特征的RGB-D 視覺里程計算法表現優于ORB-SLAM2，并且滿足一般環境的實時性需求。

7 結 論

本文構建了以線特征為核心的線特征視覺里程計框架。針對深度相機存在的深度缺失問題，增加深度圖推斷的圖像預處理步驟，使三維直線的估計更加準確。使用LSD 進行線特征提取并且使用LBD 描述子進行描述以便進行特征匹配。利用RANSAC 方法，推導了二維線段反投影擬合為三維直線的過程。本文在位姿優化部分進行改進，充分使用擬合三維直線過程中的最佳過直線兩點而非傳統的線段端點增加可靠性，并且加入了一個角度誤差信息，推導了誤差關于位姿擾動的雅克比矩陣，實現圖優化過程，該優化方法使得視覺里程計位姿估計結果更加準確。

使用線特征的視覺里程計相比于點特征里程計的計算量更大，特別是利用RANSAC 進行三維線段擬合時，算法復雜度較高，當線特征數量增加時，計算量增長較大，實時性并不如點特征，需要控制線特征數量保持在合理的范圍，在未來有待進一步優化。