葉培楚，李 東，章 云

（廣東工業(yè)大學 自動化學院，廣東 廣州 510006）

自1980年至今，SLAM(即時定位與地圖構(gòu)建)[1-3]一直都是計算機視覺和機器人領(lǐng)域的熱門研究方向，廣泛應(yīng)用于需要實時定位的設(shè)備比如移動機器人、無人機[4]、自動駕駛汽車[5-6]以及增強現(xiàn)實/虛擬現(xiàn)實[7]等?，F(xiàn)有的SLAM算法中涵蓋了多種不同類型的傳感器，比如相機、激光雷達、GPS或者慣性測量單元(Inertial Measurement Unit, IMU)等。由于相機具有成本低、普及率高并且可被動地獲取豐富的環(huán)境信息等優(yōu)點，因此基于相機的視覺SLAM研究成為熱點。

視覺SLAM主要由前端和后端兩部分組成：前端實現(xiàn)對相機的跟蹤，根據(jù)當前地圖全局定位相機[7-10]，也可以根據(jù)視覺里程計本地定位相機[11-12]。后端創(chuàng)建和維護關(guān)鍵幀地圖并通過閉環(huán)檢測和地圖優(yōu)化減少全局誤差。

現(xiàn)有的視覺SLAM算法可以分為間接法和直接法，其中間接法通過對圖像提取特征點，實現(xiàn)對相機的定位和環(huán)境地圖的構(gòu)建，但只用少量的特征點代替一整幅圖像導(dǎo)致重建的環(huán)境地圖非常稀疏，無法用于機器人等設(shè)備導(dǎo)航[8-9]。盡管ORB-SLAM3[10]采用IMU和相機緊耦合的方式提高了定位精度[8-9]，并且利用新的回環(huán)方式減少系統(tǒng)整體的累積誤差，但是其純雙目版本的性能并無改善。此外，特征點的SLAM方法會耗費大量時間進行特征提取，并且對于弱紋理場景不具備很好的魯棒性。鑒于特征點這些缺點，文獻[12]提出了一種直接法的SLAM算法，直接基于圖像所有像素點和邊緣進行處理，實現(xiàn)對相機的定位和環(huán)境地圖構(gòu)建，并且構(gòu)建出非常稠密的環(huán)境地圖。然而，重建非常稠密的環(huán)境地圖需要消耗大量的計算資源，不適用于一般的移動設(shè)備，比如增強現(xiàn)實、虛擬現(xiàn)實、無人機等。因此，文獻[13]提出一種基于直接法的稀疏里程計方法(Direct Sparse Odometry, DSO)，通過提取圖像中梯度較大的點進行相機定位，在后端采用滑動窗口和邊緣化技術(shù)對相機位姿和環(huán)境點云進行優(yōu)化，實現(xiàn)相對稠密的環(huán)境重建以及定位，同時達到較高的幀率。然而，采用直接法、滑動窗口和邊緣化技術(shù)的直接稀疏里程計目前仍然存在許多挑戰(zhàn)，比如在室內(nèi)環(huán)境中運行時無法產(chǎn)生具有絕對尺度的定位結(jié)果和環(huán)境重建，在室外場景中運行時會產(chǎn)生嚴重的尺度漂移問題。雙目DSO通過引入雙目約束，解決了單目DSO中的尺度不確定性和大尺度環(huán)境下漂移的問題，但其深度初始化效率低，且并未在運動估計初值時引入雙目約束，無法得到較好的絕對尺度信息[14]。

針對上述問題，本文提出一種基于雙目強約束的直接稀疏視覺里程計。主要的工作如下：

(1) 提出一種新的雙目初始化方案，通過對梯度點提取快速描述子，采用絕對誤差和算法(Sum of Absolute Differences, SAD)在極線方向搜索最優(yōu)匹配點，利用歸一化積相關(guān)算法(Normalized Cross Correlation, NCC)進行驗證，實現(xiàn)了快速的雙目初始化。

(2) 基于雙目相機固定基線長度設(shè)置閾值，根據(jù)梯度點的初始深度值將所有點劃分為近點和遠點兩種類型。對遠點和近點分別賦予不同的深度范圍，深度濾波器在幀間濾波時可以快速收斂到其真實值。

(3) 在估計移動機器人運動初值時，引入雙目約束，增強運動估計階段的絕對尺度信息，使得移動機器人在定位精度上得到顯著提高。

(4) 與現(xiàn)有方法相比，本文所提出的方法無需提取特征點，對于弱紋理場景具有較好的魯棒性。優(yōu)化目標函數(shù)包含所有遠點和近點，極大提高了移動機器人定位的旋轉(zhuǎn)準確率。實驗結(jié)果表明，本文提出的方案在直接法SLAM中，如Stereo DSO與LSD-SLAM2，定位精度上達到了先進水平，而相比特征點法的SLAM方案，本文方案達到了與ORB-SLAM3相近的水平。

1 本文方法

1.1 系統(tǒng)框架

本文提出的基于雙目強約束的直接稀疏視覺里程計系統(tǒng)流程如圖1所示。

圖1 本文系統(tǒng)流程圖Fig.1 The proposed system flow

系統(tǒng)的輸入是雙目圖像對，通過對左幀圖像網(wǎng)格化，并在每個網(wǎng)格中計算梯度信息，得到期望的關(guān)鍵點。結(jié)合SAD以及NCC雙步驟驗證的方式進行雙目圖像初始化，得到每個關(guān)鍵點的初始深度值。

基于圖像雙目固定基線長度設(shè)置參考閾值，根據(jù)初始深度值將所有關(guān)鍵點劃分成近點和遠點。由于雙目相機的固定基線長度較短，對于近距離的點深度估計較為準確，因此近距離的點初始深度范圍被限制得比較小。遠距離的點深度直接由雙目相機估計會產(chǎn)生較大的誤差，需要融合較多相鄰幀的信息做深度優(yōu)化，因此遠距離的點初始化深度范圍設(shè)置得相對比較大。

在估計相對運動初值時，引入雙目相機約束，從而更準確地估計絕對尺度。

在系統(tǒng)后端，所有模型參數(shù)包括相機固有參數(shù)、所有關(guān)鍵幀的相機位姿以及所有關(guān)鍵點的深度值，都在一個滑動窗口中聯(lián)合優(yōu)化。為了在滑動窗口中保持固定數(shù)量的關(guān)鍵幀和激活點，系統(tǒng)采用了邊緣化的操作，以保持數(shù)據(jù)關(guān)聯(lián)的一致性。

1.2 Two-stages雙目初始化

為了獲得跟蹤階段需要的初始深度圖像，文獻[14]采用NCC的雙目匹配方式[15]實現(xiàn)雙目初始化。對于左幀圖像IL中檢測到的每個關(guān)鍵點p，沿著極線方向在右?guī)瑘D像IR計算NCC匹配分數(shù)，搜索最優(yōu)匹配點q。對于每一個3 ×5的圖像塊M，NCC度量相似性的計算方法如式(1)所示。

其中，Mp和Mq分 別表示左幀圖像中的點p和右?guī)瑘D像中的點q對 應(yīng)的3 ×5的 匹配塊，IL[pi]和IR[qj]分別表示左幀圖像塊Mp中的點pi與右?guī)瑘D像塊Mq中的點qj對 應(yīng)的圖像光度值，ILavg[p]和IRavg[q]分別表示對應(yīng)圖像塊光度平均值。

由于每一次匹配，NCC都必須計算公式(1)，顯然是非常耗時的。為了提高雙目初始化的效率，并保持與Stereo DSO[14]深度濾波器誤差方程的一致性，本文提出Two-stages的雙目初始化方式。采用圖2所示的描述子[16]對SAD進行搜索匹配[17]，如式(2)所示。

圖2 雙目初始化采用的描述子Fig.2 The descriptor used for stereo initialization

其中，Qp和Qq分別表示左幀圖像中的點p和右?guī)瑘D像中的點q對應(yīng)的描述子。

顯然，本文采用的描述子的大小為1 6維，與NCC匹配塊大小接近，但度量相似性的計算步驟更簡單，可以極大提高匹配效率。為了保證匹配結(jié)果可靠性，NCC被用于驗證SAD的匹配結(jié)果。通過這種雙步驟驗證的方式，降低雙目初始化的計算量，在不損失定位精度的情況下提高了匹配速度。

1.3 深度范圍初始化

由于雙目相機固定基線的長度固定，距離相機中心比較遠的關(guān)鍵點深度值無法準確測量，因此得到的初始深度圖像并不完全可靠。

如圖3所示，假設(shè)世界坐標系下有兩個點P和Q，它們在左、右?guī)瑘D像中的投影分別用p1、p2和q1、q2表示，根據(jù)多視圖幾何約束，可以通過三角化得到其對應(yīng)空間深度值。值得注意的是，由于測量存在誤差，對于距離相機中心較近的點，當測量存在一個像素誤差時，估計的深度值誤差為eP，而距離相機中心較遠的點，一個像素誤差導(dǎo)致的深度值誤差為eQ，顯然eP＜eQ。距離相機中心越遠的點，其深度值不確定性會越大，即測量誤差越大。

圖3 三角化測量不確定性Fig.3 Uncertainty in triangulation

在本文中，參考文獻[9]將區(qū)分近點和遠點的閾值設(shè)置為40倍雙目相機固定基線長度。初始深度值小于40倍基線的點被定義為近點，否則定義為遠點。Stereo DSO[14]采用深度濾波器[18]對關(guān)鍵點的深度進行估計，但其對所有的點都采用同樣的深度范圍，并未考慮三角測量不確定性。本文通過區(qū)分近點和遠點，對近點賦予較小的深度范圍，對遠點賦予較大的深度范圍。通過相鄰幀約束，使關(guān)鍵點的深度范圍快速收斂到真實值。

1.4 基于雙目約束的運動估計

在跟蹤線程中，滑動窗口中的所有激活點被投影到最新的關(guān)鍵幀構(gòu)建參考幀深度圖。通過將參考幀中的點反投影到當前幀并優(yōu)化能量函數(shù)(3)，可以獲得當前幀Ic相對于參考幀Ir的相對運動。

其中，Pr表示參考幀圖像中的點，Np采用與DSO[13]一樣的圖像塊，ωp表示點的權(quán)重，并且Np中所有的點均采用同樣的權(quán)重， ‖ ·‖γ為核函數(shù)，防止畸形點破壞優(yōu)化結(jié)構(gòu)。 (ar,br)和 (ac,bc)分別表示參考幀和當前幀的光度仿射參數(shù)，用于建模光度變化。對于投影點q～，由公式(4)計算得出。

其中，π (·)表示將相機坐標系的三維點投影到相機平面，同理，π-1(·)表示將相機平面的點反投影到相機坐標系中。Tcr表 示參考幀到當前幀的相對位姿，dp為點p的逆深度，Np中的所有點均采用同樣的深度值。

實際上，具有固定基線的雙目圖像可以為絕對尺度估計提供更多的約束，但Stereo DSO[14]并未引入這個約束。因此，在本文中，通過引入雙目約束，將能量函數(shù)(3)拓展為公式(5)。

通過最小化公式(5)，當前幀的參數(shù)包括相機位姿、左右?guī)瑘D像的光度仿射參數(shù)被有效初始化，隨后傳入后端滑動窗口[19]中進一步優(yōu)化。

2 實驗分析

2.1 數(shù)據(jù)集

本文使用KITTI數(shù)據(jù)集[20]對所提出的算法進行驗證與分析。KITTI數(shù)據(jù)集是車載雙目相機在街區(qū)和高速公路等地區(qū)同步采集，并經(jīng)過校正得到的高分辨率雙目圖像，是視覺SLAM測試算法在大場景環(huán)境下的定位性能常用的數(shù)據(jù)集。為了驗證本文算法的有效性，在該數(shù)據(jù)集上進行了定位與建圖實驗，同時與Stereo DSO[14]、LSD-SLAM2[21]以及ORBSLAM3[10]分別進行了對比，其中Stereo DSO、LSDSLAM2是目前公開性能最優(yōu)的直接法SLAM方案，ORB-SLAM3是目前公開性能最優(yōu)的特征點法SLAM方案。本文算法的實驗平臺為：Intel i5 4200H處理(2.8 GH)，12 GB RAM，64位Ubuntu16.04操作系統(tǒng)。

2.2 定位精度比較

由于本文所提出的方案是基于直接法的SLAM，因此為了驗證本文方案的性能，首先將本文方案與基于直接法的視覺SLAM方案(LSD-SLAM2以及Stereo DSO)在位移誤差、旋轉(zhuǎn)誤差上進行定位精度對比。由于Stereo DSO并未開源代碼，我們無法直接與其進行對比實驗。通過復(fù)現(xiàn)Stereo DSO提出的方法，與本文方案進行對比。從表1的實驗結(jié)果可以看出，本文提出的基于雙目強約束的直接稀疏里程計系統(tǒng)通過增強雙目約束條件，實現(xiàn)了比Stereo DSO與LSD-SLAM2更好的定位效果。特別是，本文的平均位移誤差和平均旋轉(zhuǎn)誤差分別為0.89%和0.23%，而LSD-SLAM2為1.14%和0.4%，Stereo DSO為0.95%和0.25%，均是本文提出的方案最優(yōu)。相比LSD-SLAM2和Stereo DSO，本文采用快速雙目初始化的策略，提高梯度點深度值初始化效率，同時根據(jù)初值深度值區(qū)分近遠點，為深度濾波器提供較為準確的深度初始范圍，使梯度點可以快速收斂到其真實值。在估計運動初始階段，引入雙目約束，可以有效克服尺度誤差，提高定位精度。

表1 本文算法與LSD-SLAM2以及Stereo DSO定位精度對比Table 1 Comparison of accuracy on KITTI between our proposed and LSD-SLAM2 as well as Stereo DSO

為了進一步驗證本文算法的可靠性，還與特征點法SLAM方案ORB-SLAM3進行比較。由于本文算法是純雙目視覺里程計，因此為了公平比較，關(guān)閉了ORB-SLAM3的回環(huán)模塊與全局優(yōu)化模塊等功能。實驗結(jié)果如表2所示，顯然本文的直接法SLAM與基于特征點法的ORB-SLAM3取得相近的性能，在平均位移誤差上，本文算法弱于ORB-SLAM3，主要原因是KITTI數(shù)據(jù)集幀間距離較遠，相鄰幀圖像同一位置的像素偏差較大，對直接法SLAM方案挑戰(zhàn)較大，此外，光照強度變化較大時也會對直接法的定位精度產(chǎn)生一定影響。特別是，在KITTI序列03和序列09，本文提出的方案在位移誤差上與ORB-SLAM3相差較大，原因在于這兩個序列的光照強度較大，強弱變化也非常頻繁，直接法很難對這樣的光照變化進行非常準確的建模。但在旋轉(zhuǎn)誤差上，本文算法幾乎完全優(yōu)于ORB-SLAM3。這是由于ORB-SLAM3在估計運動時只考慮近點，而本文算法同時考慮遠點和近點兩類點，其中近點可以提高位移估計的準確度，而遠點可以更好地估計旋轉(zhuǎn)變化，特別是表2中的序列01和序列10，旋轉(zhuǎn)誤差明顯遠小于ORB-SLAM3。從實驗結(jié)果也可以明顯看出，同時融合遠點和近點的策略可以極大提高旋轉(zhuǎn)估計的精度。在絕對軌跡誤差上，本文算法在大部分序列中都優(yōu)于ORB-SLAM3，本文算法的絕對軌跡誤差為3.21 m，而ORB-SLAM3的絕對軌跡誤差為3.44 m。從綜合結(jié)果來看，本文提出的算法與ORB-SLAM3性能相近，為無人駕駛汽車/移動機器人提供一個較優(yōu)的解決方案，特別是在弱紋理以及窄基線的場景環(huán)境中。

表2 本文算法與ORB-SLAM3定位精度對比Table 2 Comparison of accuracy on KITTI between our proposed and ORB-SLAM3

圖4是本文算法在KITTI 02序列估計的軌跡與真實軌跡的對比圖，可以看出本文提出的算法定位精度是非常高的。

圖4 本文算法在KITTI-02的估計軌跡與真實軌跡對比Fig.4 Comparison between the proposed and the ground-truth on KITTI 02

圖5是本文算法在KITTI 00序列重建的環(huán)境地圖，相比于基于特征點法的ORB-SLAM3，本文算法重建的環(huán)境地圖更加稠密，能更好地實現(xiàn)機器人/無人駕駛汽車的導(dǎo)航功能。

圖5 本文算法在KITTI-00的重建效果Fig.5 3D reconstruction of the proposed on KITTI 00

由上述實驗結(jié)果可知，本文通過增強雙目約束，在公開的直接法SLAM方案中定位精度達到了先進水平，并實現(xiàn)了與最優(yōu)的特征點法SLAM系統(tǒng)ORBSLAM3相近的定位精度。

3 結(jié)語

本文提出了基于雙目強約束的直接稀疏視覺里程計系統(tǒng)進行SLAM應(yīng)用研究，為直接法SLAM提供一種改進方案。通過引入SAD與NCC雙步驟的雙目匹配方案，實現(xiàn)了快速的雙目初始化?；陔p目相機固定基線長度設(shè)置閾值條件，將觀測點劃分成遠點和近點，由于三角化不確定性隨觀測點的觀測深度變化而變化，因此對兩類點分別初始化不同的深度范圍，后續(xù)深度濾波估計時可以快速收斂到其真實值。在估計相對運動初值時，添加雙目約束可以為估計絕對尺度信息提供更多約束信息。本文分別與目前性能最優(yōu)的直接法SLAM系統(tǒng)Stereo DSO和LSDSLAM2以及性能最優(yōu)的特征點法SLAM系統(tǒng)ORBSLAM3在KITTI數(shù)據(jù)集11個序列上進行了對比試驗，實驗結(jié)果驗證了雙目強約束的有效性并證明本文提出的方案在直接法SLAM中達到了先進水平，相比于目前最優(yōu)的基于特征點法的ORB-SLAM3，本文方案實現(xiàn)了與其相近的定位精度。

本文提出的基于雙目強約束的直接稀疏視覺里程計，僅是一個直接法視覺SLAM前端，隨著系統(tǒng)長時間長距離的運動，會產(chǎn)生累積誤差，導(dǎo)致定位精度下降。因此，為了使本文提出的方案在大場景環(huán)境下更加魯棒，未來需要進一步為系統(tǒng)添加回環(huán)檢測和全局優(yōu)化模塊。