一種基于光度信息和ORB特征的建圖SLAM

李晨玥， 張雪芹， 曹 濤

（1. 華東理工大學信息科學與工程學院，上海 200237；2. 上海空間智能控制技術重點實驗室，上海 201109；3. 上海航天控制技術研究所，上海 201109）

同步定位與建圖（Simultaneous Localization and Mapping，SLAM）通常是指在機器人或者其他載體上通過傳感器數(shù)據(jù)進行采集和計算，得到對其自身位置姿態(tài)定位和場景地圖信息的系統(tǒng)。一個經典的基于視覺傳感器的Visual SLAM一般包括前端視覺里程計、后端優(yōu)化、回環(huán)檢測、建圖4個模塊。前端視覺里程計模塊用于估計相鄰圖像之間相機的位姿變換并得到局部地圖；后端優(yōu)化模塊對相機位姿及局部地圖信息進行優(yōu)化得到全局一致的軌跡和地圖；回環(huán)檢測模塊用于檢測傳感器是否又回到之前的位置，優(yōu)秀的回環(huán)檢測可以有效減少累計漂移誤差；建圖模塊則根據(jù)之前得到的跟蹤全局地圖建立所需要的地圖。

經典的視覺里程計采用的方法主要有基于特征的方法和直接法。基于特征的方法通過提取特征以及特征匹配得到圖像幀間的幾何對應關系，優(yōu)化重投影誤差來估計傳感器位姿。早期，基于特征的方法主要側重于特征點的提取與匹配。其中，由Mur-Artal等[1]提出的ORB-SLAM2是一個同時支持單目、雙目及深度相機的基于特征點的SLAM系統(tǒng)，它使用三線程結構取得了很好的跟蹤和建圖效果，一定程度上保證了軌跡與地圖的全局一致性。由于基于特征點的方法不足以提供有效的約束且在低紋理場景中容易失敗，不少學者開始嘗試結合線、平面、邊緣等其他特征。Pumarola等[2]提出了一個同時結合了點和線特征的SLAM系統(tǒng)，該系統(tǒng)在低紋理環(huán)境下表現(xiàn)穩(wěn)定。Hsiao等[3]提出的KDP-SLAM(Keyframebased Dense Planar SLAM)是一種基于關鍵幀密集平面匹配的SLAM系統(tǒng)，顯著地降低了漂移誤差。Li等[4]提出了利用點、線、面的結構特性包括線與線之間的平行性和相交性、點線之間的共面性來跟蹤相機運動，并在低紋理環(huán)境下得到了更高的系統(tǒng)穩(wěn)定性和較為精確的跟蹤結果。

直接法通過最小化圖像中像素之間的光度誤差來估計傳感器的運動信息，一方面能夠彌補基于特征的方法在某些特征缺失場景下失敗的問題，另一方面能夠直接得到稠密的地圖顯示。LSD-SLAM(Large-Scale Direct Monocular SLAM)[5]是Engel等提出的第一個不需要計算特征點且能構建半稠密地圖的單目SLAM系統(tǒng)，但該系統(tǒng)對相機內參及曝光非常敏感，在快速運動時容易丟失。他們后期又提供了一個更為穩(wěn)健、準確度更高的包含光度校準的直接法視覺里程計[6]，但這個系統(tǒng)不包括回環(huán)檢測，無法降低累積誤差。Gao等[7]在這個直接視覺里程計的基礎上加入基于詞袋的回環(huán)檢測功能，減少了累積誤差。

基于特征的方法在特征豐富的區(qū)域準確率較高且計算代價較小，但在特征點缺失的無紋理場景下容易失敗。而直接法不需要特征點，可以在特征缺失的場合下使用，但依賴于灰度不變性，易受光照影響。因此，有不少學者嘗試將這兩種方法結合以得到對環(huán)境更為魯棒的系統(tǒng)。Younes等[8]提出了FDMO(Feature Assisted Direct Monocular Odometry)方法，它在直接法出現(xiàn)故障的場景,如大基線時使用ORB特征進行位姿估計，提升了直接法系統(tǒng)的魯棒性。Lee等[9]提出了一種半直接法，以直接法跟蹤局部每幀的相機運動，以基于ORB特征的圖優(yōu)化方法優(yōu)化跟蹤關鍵幀的相機運動，得到了一個準確性和魯棒性且速度上都有提高的SLAM系統(tǒng)。這兩種SLAM系統(tǒng)都是單目的混合SLAM系統(tǒng)，無法得到稠密的三維地圖。Nicola等[10]提出以libviso2特征法作為雙目LSD-SLAM的運動估計先驗，得到了一個精度較高且較為穩(wěn)定的雙目SLAM系統(tǒng)，但它使用特征法作為SLAM系統(tǒng)的前端位姿估計，在特征缺失的環(huán)境中可能會跟蹤失敗。

在環(huán)境勘探工程中，機器人常在較空曠環(huán)境及非光滑表面行進，通過雙目或深度相機對環(huán)境進行勘探，并需要通過采集到的圖片及視頻對三維地形進行重建。針對SLAM系統(tǒng)因為相機抖動、非勻速的運動、快速的旋轉及場景結構單一、紋理簡單等問題而導致跟蹤失敗的情況，本文提出了一個魯棒的具有建圖功能的R-ORB SLAM系統(tǒng)。該系統(tǒng)的創(chuàng)新點主要體現(xiàn)在兩方面：（1）該系統(tǒng)基于ORB-SLAM2系統(tǒng)，同時采用一種輕量級的位姿估計作為特征視覺里程計的先驗，在跟蹤失敗的情況下，將先驗位姿的結果參與位姿估計；（2）通過相機模型計算得到關鍵幀點云，通過對每個關鍵幀點云進行拼接得到全局點云地圖，使用VoxelGrid濾波器[11]進行下采樣，得到稠密三維點云地圖；使用Poisson算法[12]實現(xiàn)表面重建，得到三維Mesh地圖。該系統(tǒng)充分利用了圖片的光度和深度信息，提高了系統(tǒng)的準確率和魯棒性。

1 ORB-SLAM2系統(tǒng)

1.1 系統(tǒng)流程

ORB-SLAM2系統(tǒng)是基于ORB特征的特征法SLAM。前端通過高斯金字塔模型提取均勻的ORB特征點，保證ORB特征的旋轉不變性和尺度不變性。

在ORB-SLAM2系統(tǒng)的雙目模式和RGB-D模式中，特征關鍵點由3個坐標xs=(uL,vL,uR) 表示，其中 (uL,vL) 為該特征關鍵點在左圖（RGB-D相機中的RGB圖像）中的像素坐標，uR為右邊圖像中的水平像素坐標，在RGB-D模式下，uR是一個虛擬的坐標，可由式(1)得到。

其中：fx為相機水平焦距；b為相機基線，d為對應像素深度信息。根據(jù)傳統(tǒng)相機模型可以將特征點映射到三維空間中。

其中： ρ 為魯棒Huber損失函數(shù)； Σ 為與關鍵點尺度相關的協(xié)方差矩陣； χ 為所有匹配點；投影函數(shù) π(·)可以由式(3)表示：新加入的每一個關鍵幀都要與之前的關鍵幀做回環(huán)檢測。ORB-SLAM2的回環(huán)檢測基于詞袋模型。

1.2 運動估計中的李代數(shù)

2 R-ORB SLAM系統(tǒng)

ORB-SLAM2系統(tǒng)在相機發(fā)生抖動（圖1(a)）、以及特征點數(shù)量較少、場景空曠簡單（圖1(b)）的情況下，極易發(fā)生跟蹤失敗的問題，而其系統(tǒng)自帶的重定位功能在發(fā)生跟蹤失敗時，只能回到之前經過的某個位置重新啟動跟蹤。這樣，當相機在新的場景中持續(xù)運動時則會徹底跟蹤失敗。針對該問題，本文提出了一種將光度信息和幾何特征相結合的R-ORB SLAM系統(tǒng)。

2.1 系統(tǒng)流程

圖1 ORB-SLAM，系統(tǒng)易失敗場景Fig. 1 Failure scenarios for ORB-SLAM2 system

R-ORB SLAM系統(tǒng)框圖如圖2所示。首先對輸入的圖片序列進行圖片矯正、金字塔分層模型及特征提取等預處理操作，然后根據(jù)重定位、運動模型或者本文提出的基于光度信息的輕量級位姿估計方法（Roughly Pose Estimation，RPE）得到初始位姿對前一幀進行跟蹤，最后根據(jù)優(yōu)化得到的位姿結果生成地圖。在建圖模塊中，保留ORB-SLAM2系統(tǒng)的建圖線程用于跟蹤，另外使用點云拼接線程拼接每個關鍵幀點云得到三維點云地圖，對得到的點云地圖使用VoxelGrid濾波器進行下采樣濾波，對濾波后的點云使用Poisson表面重建算法得到三維地圖模型。R-ORB SLAM算法主要適用于RGB-D深度傳感器。

2.2 輕量級位姿粗估計

RPE模型提取前一幀圖像梯度大于閾值的像素，并假設灰度不變，即假設一個空間點在各個視角下成像的灰度不變。將前一幀的任意一點通過與特征法相似的投影方法投影至當前幀平面，此時優(yōu)化的不再是重投影誤差而是光度誤差，公式如下：

其中：K為相機內參矩陣；Xi∈R3為與前一幀匹配成功的地圖點；xi為后一幀圖像平面的對應點；I(·) 表示該 點 的光度；Z2為Xi經 過 e xp(ξ∧) 變換 后 第二幀相機坐標系下的Z軸坐標。

圖2 R-ORB SLAM系統(tǒng)流程圖Fig. 2 Flow chart of R-ORB SLAM system

使用該式計算優(yōu)化問題的雅可比矩陣，然后使用高斯牛頓法或者Levenberg-Marquardt方法計算增量迭代求解。

2.3 權重模型

使用RPE模型作為位姿估計模型，與運動模型一起為ORB-SLAM2系統(tǒng)提供先驗。

ORB-SLAM2系統(tǒng)中的初始位姿是由運動模型和重定位得到的，系統(tǒng)根據(jù)該初始位姿將地圖點重投影至圖像平面，在投影得到的位置附近尋找匹配點，并在該初始位姿的基礎上進行優(yōu)化。當傳感器違背了運動模型做不規(guī)律的運動時，使用運動模型得到的初始位姿不能較好地為后續(xù)優(yōu)化提供先驗，導致跟蹤失敗問題。通常這樣的情況并不會發(fā)生在相機的整個運動路線中，而只是局部發(fā)生。針對這種情況，結合2.2節(jié)的輕量級位姿粗估計，設計了一個權重模型，將ORB-SLAM2系統(tǒng)得到的不夠準確的位姿和RPR模型得到的位姿相結合，得到一個估計值，作為跟蹤失敗時當前幀的位姿，并在此插入關鍵幀以防止后續(xù)的跟蹤失敗。

2.4 點云拼接建稠密地圖

ORB-SLAM2系統(tǒng)只保留一些在跟蹤過程中被反復檢測并優(yōu)化的特征三維點，簡單增加跟蹤過程中的地圖點，不僅會降低跟蹤的速度，還會增加一些可能錯誤的約束導致優(yōu)化失敗。本文將建圖和跟蹤分離開來，由建圖線程單獨進行建圖。

使用點云拼接的方式建立稠密的點云地圖。由跟蹤線程得到每個關鍵幀的位姿和與前一個關鍵幀的變換矩陣，使用該變換矩陣進行關鍵幀點云的拼接。每個關鍵幀的點云都可由深度圖和RGB圖以及常規(guī)針孔相機模型得到，由RGB圖中任意一個二維點x(u,v) 得到當前幀點云中點P(X,Y,Z,R,G,B) 的計算 公式，如式(15)所示：

其中：s是尺度因子，即深度圖里給的數(shù)據(jù)與實際距離的比例；R、G、B是該點的顏色信息。

由于ORB-SLAM2系統(tǒng)采用多關鍵幀的策略，如果采用每個關鍵幀的點云進行拼接，會導致大量點云的冗余問題。因此，本文使用VoxelGrid濾波器對每個關鍵幀的點云進行下采樣，在保持點云幾何特征的同時，減少點的數(shù)量。該濾波器通過輸入的點云數(shù)據(jù)創(chuàng)建一個三維體素柵格，在每個體素內，用體素中所有點的重心來表示其他點。

當全局優(yōu)化線程結束后，可得到一個全局的、經過濾波的三維點云，使用Poisson表面重建算法可以得到一個mesh類型的三維模型，后續(xù)顏色的渲染可通過MeshLab軟件[14]進行。

3 實驗結果與分析

在兩個流行的數(shù)據(jù)集TUM RGB-D[15]和ICLNUIM[16]中開展實驗，評估R-ORB SLAM系統(tǒng)的性能，并將R-ORB SLAM系統(tǒng)與其他SLAM系統(tǒng)進行比較。實驗在內存為8 GB，CPU為Intel? Core? i7-8 750H，攜帶了GTX 1 060的筆記本電腦上進行。由于多線程會造成實驗結果的非確定性，實驗在每個視頻序列上運行5次，得到平均軌跡精度。使用絕對軌跡均方根誤差 (Absolute Trajectory Root-Mean-Square Error，ATE RMSE)、失敗幀率 (Failure Ratio, FR)、表面重建誤差 (Surface Reconstruction Error, SRE)評估SLAM系統(tǒng)的跟蹤精度、魯棒性和重建精度。ATE RMSE通過計算估計位姿與真實位姿之間的歐式距離，得到絕對軌跡誤差的均方根誤差；FR表示跟蹤失敗的圖片幀在整個圖片序列中所占的比例；SRE由計算重建表面的每一點到最近真實3D模型的表面的平均距離得到。實驗中，R-ORB SLAM系統(tǒng)中的Nthre取ORB-SLAM2系統(tǒng)中的默認閾值。

3.1 TUM RGB-D數(shù)據(jù)集

TUM RGB-D數(shù)據(jù)集包含來自RGB-D傳感器的室內序列，這些序列分組在幾個類別中，包括手持式采集類及機器人采集類等，以評估不同紋理，不同運動速度、照明和結構條件下的SLAM方法。該公開數(shù)據(jù)集基準還提供了許多有用的工具，可用于預處理數(shù)據(jù)集和評估SLAM系統(tǒng)跟蹤結果。

本文從TUM RGB-D數(shù)據(jù)集中選擇了幾個常用的序列，并與ORB-SLAM2、DVO-SLAM[17]、RGB-D SLAM[18]、Kintinuous[19]、Elastic fusion[20]進行了比較，結果如表1所示。實驗結果顯示，在幾乎每個序列上，R-ORB SLAM系統(tǒng)比ORB-SLAM2系統(tǒng)的跟蹤精度都略有提高，與其他系統(tǒng)相比，僅在1個序列上略低于Elastic fusion。

在TUM RGB-D數(shù)據(jù)集中，fr2/360、fr2/slam、fr2/slam3 圖片序列通過安裝在Pioneer機器人頂部的Kinect相機拍攝采集得到。由于機器人的運動非勻速且較快，整個環(huán)境尺度比辦公室場景大，劇烈的抖動以及尺度較大的場景對SLAM系統(tǒng)的穩(wěn)定性是一個挑戰(zhàn)。大多數(shù)SLAM系統(tǒng)在這幾個序列中都會出現(xiàn)跟蹤失敗，包括ORB-SLAM2。TUM RGB-D數(shù)據(jù)集中還提供了用于檢測環(huán)境結構和紋理對SLAM系統(tǒng)影響的序列。其中，fr3/nostructure_ notexture_far記錄了攝像頭沿木制平面移動時拍攝的圖片序列，幾乎無場景結構和紋理特征；fr3/structure_notexture_far記錄了一個由木板制成的鋸齒形結構而幾乎無紋理特征的場景，大部分SLAM系統(tǒng)在這兩個場景中都極易跟蹤失敗。

R-ORB SLAM、ORB-SLAM2和Elastic fusion系統(tǒng)在這5個序列上的失敗幀率如表2所示，表中fr3/ns_nt_far、fr3/s_nt_far分別是指fr3/nostr-ucture_notexture_far、fr3/structure_notexture_far序列。從表2可以看出，Elastic fusion和ORB-SLAM2系統(tǒng)重定位可以恢復部分跟蹤，但失敗幀率仍然較高。本文方法在這5個序列中全部跟蹤成功，魯棒性較ORBSLAM2系統(tǒng)有了顯著的提高。

圖3示出了R-ORB SLAM系統(tǒng)與ORB-SLAM2系統(tǒng)在這5個序列中的跟蹤結果。可以看到，在fr2/360序列中，ORB-SLAM2系統(tǒng)在序列開始不久就由于輕微抖動而跟蹤失敗，當序列進行至中后半段才重定位成功，而R-ORB SLAM系統(tǒng)跟蹤較穩(wěn)定，且準確度較高。在fr2/slam序列中，由于較高的相機運動速度和高抖動，ORB-SLAM2系統(tǒng)在部分路徑中跟蹤完全失敗，R-ORB SLAM系統(tǒng)則全部跟蹤成功，但可以看出，由于相機抖動導致了一定的跟蹤誤差。

表1 TUM RGB-D數(shù)據(jù)集上的絕對軌跡均方根誤差比較Table 1 Comparison of ATE RMSE on TUM RGB-D dataset

表2 TUM RGB-D數(shù)據(jù)集的失敗幀率比較Table 2 Comparison of failure ratio on TUM RGB-D dataset

圖3 跟蹤結果比較Fig. 3 Comparison of tracking results

在fr2/slam3序列中，ORB-SLAM2系統(tǒng)在路線的后半段，因為相機的突然旋轉加速和抖動，從而跟蹤失敗，而R-ORB SLAM系統(tǒng)全程跟蹤成功，但仍有一定程度的漂移。在fr3/nostructure_notexture_far和fr3/structure_notexture_far序列上，ORB-SLAM2系統(tǒng)運行時，在序列開始就由于無足夠的特征地圖點而導致初始化失敗，而R-ORB SLAM系統(tǒng)則會使用RPE得到的粗略位姿作為結果，啟動并維持跟蹤。

圖4為ORB-SLAM2系統(tǒng)和R-ORB SLAM系統(tǒng)在fr2/360序列上的效果示例圖。圖4(a)中ORBSLAM2系統(tǒng)由于相機的輕微抖動，已跟蹤失敗，而R-ORB SLAM系統(tǒng)仍有部分特征點跟蹤成功，如圖4(b)所示。

3.2 ICL-NUIM數(shù)據(jù)集

ICL-NUIM數(shù)據(jù)集[16]通常用于對RGB-D、視覺測距和SLAM算法進行基準測試，是一個對評估表面重建質量很重要的數(shù)據(jù)集。它包括由手持深度相機拍攝的起居室和辦公室兩個場景，每個場景都提供真實測量位姿，但起居室場景還提供了一個真實的3D模型以供評估表面重建的質量。

本文從ICL-NUIM數(shù)據(jù)集中選擇了4個序列，并將R-ORB SLAM系統(tǒng)與其他SLAM系統(tǒng)的跟蹤精度進行比較，結果如表3所示。

圖4 fr2/360序列上的效果示例圖Fig. 4 Effect diagrams on fr2/360 sequence

從表3可以看出，雖然R-ORB SLAM系統(tǒng)的ATE RMSE誤差在4個序列中并不總是最低，但其平均誤差最小，仍然具有較高的精度和穩(wěn)定性。

為了進一步考察表面重建的精度，在ICL-NUIM的起居室數(shù)據(jù)集上，將本文系統(tǒng)和其他系統(tǒng)進行了比較，結果如表4所示。

實驗中，將VoxelGrid濾波器網格設置為0.05×0.05×0.05。表4中Ours+TSDF表示將R-ORB SLAM系統(tǒng)得到的定位結果，使用TSDF（Truncated Signed Distance Function）[21]算法進行重建。在實驗中TSDF中的體素也劃分為0.05×0.05×0.05。

表3 ICL-NUIM數(shù)據(jù)集上的ATE RMSE比較Table 3 Comparison of ATE RMSE on the ICL-NUIM dataset

表4 ICL-NUIM數(shù)據(jù)集的表面重建誤差（SRE）比較Table 4 Comparison of surface reconstruction error on the ICLNUIM dataset

從表4可以看出，R-ORB SLAM系統(tǒng)的重建模塊在kt1、kt2序列中并不是效果最好的，但在kt0和kt3序列中得到了很好的重建精度，平均表面重建精度最小。而簡單地應用TSDF算法并沒有得到很好的重建結果。圖5示出了本文方法在kt0序列上重建的結果。圖5(a)為kt0序列真實3D模型中的一個表面；圖5(b)為未使用Poisson表面重建算法時RORB SLAM系統(tǒng)得到的稠密點云；圖5(c)為使用Poisson表面重建算法的重建結果；圖5(d)為使用Ours+TSDF的重建結果。由于視角和設定的坐標問題，重建的表面看起來與真實模型是鏡像的。從圖5可以看到，使用TSDF算法可以得到更光滑且平整的表面，但由于使用了體素劃分，在本應圓滑的表面，如枕頭、臺燈表面，卻無法顯示原本的弧度，丟失了部分細節(jié)，如右下角的盆栽，而本文提出的直接融合點云的方法則能夠得到貼近物體原型的重建模型。

4 結束語

本文針對RGB-D 傳感器提出了一個使用光度信息和ORB特征相結合的R-ORB SLAM系統(tǒng)，可以有效減少系統(tǒng)因為相機抖動、非勻速運動、快速旋轉以及場景結構單一、紋理簡單等問題而導致的跟蹤失敗的情況。同時，通過在系統(tǒng)中加入拼接、濾波等點云處理，并采用Poisson表面重建算法實現(xiàn)了地圖三維重建。在公開數(shù)據(jù)集上的實驗結果顯示，RORB SLAM系統(tǒng)在跟蹤和重建上都有較好的精度和穩(wěn)定性。在本文的重建部分，由ORB-SLAM2得到的稀疏地圖僅用于跟蹤，而實際上該地圖上的點擁有更準確的位置信息。未來，考慮將這些地圖點的信息利用起來得到更精確的重建效果。

圖5 kt0序列的某一角度重建結果Fig. 5 Reconstruction results of kt0 sequence from one perspective