面向半稠密三維重建的改進單目ORB-SLAM

周 彥，曠鴻章，牟金震，王冬麗，劉宗明

1.湘潭大學 自動化與電子信息學院，湖南 湘潭411105

2.上海航天控制技術(shù)研究所，上海201109

目前同時定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）技術(shù)是機器人、自動駕駛、增強現(xiàn)實等領域的關鍵技術(shù)之一。定位與地圖構(gòu)建是自主移動平臺導航與控制領域的兩個基本問題，SLAM技術(shù)恰好是同時解決這兩個問題的最有效解決方案。人們在生產(chǎn)生活中對自主移動產(chǎn)品與日俱增的需求也使得SLAM技術(shù)成為自主移動平臺領域研究的熱點問題。

回溯SLAM技術(shù)的發(fā)展歷程，早期主要研究基于概率估計的SLAM，例如拓展卡爾曼濾波[1]、粒子濾波[2]和組合濾波[3]。文獻[4]中以擴展卡爾曼濾波構(gòu)圖與定位（Extended Kalman Filter-SLAM，EKF-SLAM）為基礎，對該拓展卡爾曼濾波SLAM 算法進行了改進以實現(xiàn)機器人的定位與地圖創(chuàng)建。之后基于關鍵幀的單目視覺SLAM逐漸發(fā)展起來，自從Klein和Murray在2007年提出了具有開創(chuàng)性的并行跟蹤與制圖（Parallel Tracking And Mapping，PTAM）方法[5]以來，現(xiàn)已經(jīng)有許多研究者提出其他實時視覺SLAM（visual SLAM，vSLAM）方法，包括基于特征點的ORB-SLAM[6]（Oriented FAST and Rotated BRIEF-SLAM）方法，以及基于直接法的大范圍單目定位與制圖[7]（Large-Scale Direct Monocular SLAM，LSD-SLAM）方法和使用RGB-D攝像頭來同時定位與制圖的RGBD-SLAM[8]方法。國內(nèi)研究者在文獻[9]中對vSLAM在無人平臺上的研究進展做了深刻地論述。

一個好的SLAM 算法應該具有在復雜環(huán)境下進行準確處理、算法計算復雜度要求盡可能低、實時性好、能產(chǎn)生精確的結(jié)果等優(yōu)點，而且要在運算條件和傳感器價格低的條件下盡可能產(chǎn)生稠密的、細節(jié)豐富的地圖。Mur-Artal 等于2015 年提出的ORB-SLAM 方法是目前最先進的算法之一。ORB-SLAM具有遠超其他方案的穩(wěn)健性，當然也存在一些不足之處，如構(gòu)建的稀疏特征點地圖只能滿足對定位的需求，而無法提供機器人導航、避障和交互等諸多功能。為了盡可能實現(xiàn)這些優(yōu)點，本文將詳細論述在多個算法基礎上重新設計的一個新算法，此算法擁有上述多種優(yōu)點，在多個數(shù)據(jù)集上取得了很不錯的實驗效果。

圖1 改進ORB-SLAM方法的框架

1 回顧單目ORB-SLAM方法

單目ORB-SLAM方法能從由單目攝像頭獲取到的連續(xù)視頻中估計關鍵幀的位姿以及由這些關鍵幀計算得到大量的地圖點。該基于特征的SLAM 方法選擇了ORB 特征進行精確跟蹤與構(gòu)建精簡的地圖，同時使用基于DBoW2[10]的詞袋向量模型去服務于全局重定位和閉環(huán)檢測。

ORB-SLAM方法框架由跟蹤線程（Tracking）、局部建圖線程（Local Mapping）和回環(huán)檢測線程（Loop Closing）組成。跟蹤線程負責對每幀圖像的相機位姿進行定位，并決定何時插入新的關鍵幀。局部建圖線程負責處理新的關鍵幀，并對新插入的關鍵幀周圍的關鍵幀對應的相機位姿進行局部捆集約束（BA）[11]，同時也負責剔除冗余關鍵幀?；丨h(huán)檢測線程判斷新插入的關鍵幀是否使得所有關鍵幀形成閉環(huán)，如果閉環(huán)被檢測到則進一步計算累積誤差并減小累積漂移量。

2 改進ORB-SLAM方法的框架

本文在ORB-SLAM 算法上進行改進，使其能對場景進行半稠密地圖重建。如圖1中各個階段所示，圖中紅色與綠色虛線框出的區(qū)域為原來的單目ORB-SLAM方法部分，圖中藍色與綠色虛線框出的區(qū)域為在ORBSLAM 方法的基礎上新增的三個部分。本文方法由如下四部分構(gòu)成。

（1）原來的單目ORB-SLAM方法：獲得關鍵幀信息與一系列地圖點。

（2）關鍵幀再剔除算法（KRC）：將上一階段得到的關鍵幀再剔除一遍。

（3）直線段提取算法：提取剔除冗余關鍵幀后的關鍵幀中的直線段。

（4）三維直線段重建算法：通過匹配各個關鍵幀中提取到的直線段來重建場景的三維模型。

3 基于ORB-SLAM方法三維重建

3.1 關鍵幀再剔除

首先由ORB-SLAM方法實時提供關鍵幀信息和計算得到的大量地圖點。通過分析與實驗，如果直接使用這些關鍵幀將導致計算復雜度大、不確定性高和信息冗余等問題。本文在文獻[12]中的關鍵幀選擇算法的基礎上設計了一種使用幀間平移向量和旋轉(zhuǎn)向量的范數(shù)和（運動度）作為參考值來再次裁剪關鍵幀的算法，將其稱為關鍵幀再剔除算法。通過計算幀間運動度來考慮是否將該關鍵幀用于后續(xù)三維重建線程。

相機運動是一個剛體運動，在三維空間坐標系下可用旋轉(zhuǎn)矩陣R和平移向量t描述相機的旋轉(zhuǎn)與平移。旋轉(zhuǎn)矩陣R可由歐式空間中的一個四元數(shù)q表示，式（1）中θ代表相機繞單位向量n旋轉(zhuǎn)的角度。

如圖2 所示，單位四元數(shù)q1將向量p0旋轉(zhuǎn)到p1，q2將向量p0旋轉(zhuǎn)到p2。p0對應的c0是相機的初始幀光心，p1和p2對應的c1和c2是兩個旋轉(zhuǎn)后的相機的光心。c0到c1的平移向量是t1,c0到c2的平移向量是t2,c1到c2的平移向量是Δt=t2-t1，Δt即為兩關鍵幀之間的平移向量。由公式推導單位四元數(shù)把p1旋轉(zhuǎn)到p2,q2q1-1定義如式（2），式中‖nx,ny,nz‖=1，角度θ=2×arccosω∈(0,2)，表示為兩個關鍵幀之間的旋轉(zhuǎn)角度。

圖2 單目相機運動關系示意圖

KRC 算法的思想是使用當前幀相對于參考幀的平移量Δt和旋轉(zhuǎn)角度θ的范數(shù)和（運動度）來度量攝像機的運動程度。如公式（3）定義，max_norm為相機最小運動閾值，max_norm為相機的最大運動度閾值，只有在當前幀相對參考幀的運動度處于上面兩閾值之間才把該幀視為關鍵幀，否則直接剔除該幀。在本文中根據(jù)實際經(jīng)驗，min_norm和max_norm分別取值0.4和5。

3.2 直線段提取

本文使用Topal等人提出的實時直線段檢測（EDLines）方法[13]來檢測KRC 算法剔除掉冗余幀后的所有關鍵幀的直線段。EDLines由邊緣描繪（ED）算法[14]、直線段提取以及直線段驗證三個步驟構(gòu)成。接下來簡單描述這幾個步驟。

整個LS 提取算法包括三個階段：首先輸入灰度圖像，使用邊緣檢測算法產(chǎn)生一組干凈的、連續(xù)的邊緣圖。第二步，利用直線段擬合方法從邊緣圖中提取直線段。最后，遵循赫姆霍茲原理來驗證直線段。

3.3 三維直線段場景重建

ORB-SLAM方法能持續(xù)地給后續(xù)進程輸入關鍵幀信息，記做St幀關鍵幀K。同時獲取了當前地圖中的地圖點Mt和關鍵幀的位姿信息Tt。在經(jīng)過3.1 節(jié)的KRC算法后，被剔除去冗余幀，保留下來的關鍵幀為Kt。

整個三維重建方法流程是一個大的循環(huán)過程，假設在t1、t2時刻獲得的關鍵幀Kt1與Kt2。由關鍵幀的性質(zhì)可知Kt1與Kt2有很大一部分是相同的，所以3.2 節(jié)的直線段提取算法只需對新增關鍵幀Knew={Kn1,Kn2,…,Kni} (Knew?Kt2但Knew?Kt1) 進 行 直 線 段 提取。將Knew中每一幀輸入到直線段提取算法進行直線段檢測。算法運行至此處時就擁有Kt2幀內(nèi)的所有直線段、地圖點Mt和位姿信息Tt。將以上信息輸入到由Hofer 等人提出的基于直線段提取有效三維場景（Line3D++）算法[15]，就能重建得到一系列精確的三維直線數(shù)據(jù)。下面簡單介紹該三維重建算法的步驟，具體原理請參閱文獻[15]中算法。

（1）建立不同圖像線段之間的潛在對應關系。

（2）基于相鄰視圖的支持度來評估這些對應關系。

（3）為每個二維直線段選擇最合理的對應結(jié)果作為其3D位置假設。

（4）基于三維空間鄰近關系對二維線段進行聚類。

（5）使用組合束調(diào)整（BA）優(yōu)化結(jié)果[可選]。

表1 KRC算法參與與否對改進ORB-SLAM算法性能影響對比

4 實驗結(jié)果與分析

為了評估新算法的性能，接下將使用TUM RGB-D數(shù)據(jù)集基準[16]將本文新提出的改進ORB-SLAM方法與當前最先進的幾個SLAM方法進行比較，包括PTAM算法[5]、LSD-SLAM[7]和RGBD-SLAM[8]。該數(shù)據(jù)集與基準可支持對算法的科學評價和客觀比較。通過實驗即可記錄在統(tǒng)一基準下本文方法與其他方法關鍵幀定位軌跡的絕對誤差（ATE）。在實驗中，本文提出的方法不使用TUM RGB-D數(shù)據(jù)集的深度通道的信息。

測試條件為一臺普通筆記本電腦，配備Intel?CoreTMi7-3520 CPU @ 2.9 GHz，8 GB內(nèi)存與Ubuntu 14.04操作系統(tǒng)，本文提出的改進算法由C++編寫實現(xiàn)，同時盡可能多地使用了計算機的并行計算能力。如在三維直線段重構(gòu)階段，就多次使用到OpenMP這一種共享內(nèi)存并行系統(tǒng)的多處理器程序設計方案。而且原來的ORBSLAM 部分與本文新增部分也都采用了線程并行計算方式運行。

為了證明本文提出方法性能的良好一致性，且能重建效果出色的半稠密三維場景，實驗選取三個主流的測試視頻序列：TUM RGB-D 數(shù)據(jù)集中的fr2_desk、fr3_nstr_tex_near 和fr3_str_tex_near,并與LSD-SLAM運行結(jié)果進行比較，幾個有代表性序列的測試結(jié)果展示在圖3中，本文方法和LSD-SLAM方法都能構(gòu)建場景的半稠密地圖，本文的方法半稠密場景由直線段重建，LSD-SLAM 方法則由點云構(gòu)成，而單目ORB-SLAM 方法構(gòu)建的地圖為稀疏的點云地圖。實驗表明該改進的方法能產(chǎn)生由直線段重建的高質(zhì)量地圖，跟蹤建圖的穩(wěn)健性能比LSD-SLAM方法要好。

圖3 三維重建效果對比

為了分析本文提出的KRC算法對改進ORB-SLAM算法性能影響，將KRC 算法參與與否對整個方法性能的對比數(shù)據(jù)記錄于表1中。從表中可見，在運行同一個序列的情況下，不執(zhí)行KRC 算法時會減小整個方法三維重建的循環(huán)次數(shù)，但是執(zhí)行KRC算法后，關鍵幀數(shù)目會大量減小。另外，本文提出的改進的ORB-SLAM 方法在不執(zhí)行KRC 算法時運行一遍約需要166 s，而執(zhí)行KRC 算法時約需要173 s。同時注意到不執(zhí)行KRC 算法，會使得單次循環(huán)處理的視頻幀數(shù)量變得很大，致使單次直線段匹配過程十分耗時。KRC算法參與與否對改進ORB-SLAM算法在同一數(shù)據(jù)集上共計運行時長相差不大，但重建的循環(huán)次數(shù)在執(zhí)行KRC算法時增大，從而能使得改進算法的每次循環(huán)時間縮短，進而加快了三維地圖重建的刷新頻率。

如表2所示，對本文提出的方法的關鍵幀定位精度進行評估。由于程序進程某些階段的隨機性，例如初始化、位置優(yōu)化和直線段重建等，所有實驗運行11 次，然后報告記錄實驗結(jié)果的中位數(shù)。表中顯示了執(zhí)行5 個測試序列的關鍵幀軌跡的均方根誤差（RMSE）中位數(shù)。請注意，本文提出的方法在大多數(shù)序列中不斷提高了LSD-SLAM的軌跡精度。除了1個序列外，其他在所有序列中都能產(chǎn)生最佳結(jié)果，PTAM在這方面的表現(xiàn)稍好一些。然而，PTAM變得不那么可靠，因為其中2個測試序列失去了跟蹤。而且LSD-SLAM 也在1 個測試中丟失位姿。實驗結(jié)果表明本文提出的方法的魯棒性要強于其他三種SLAM方法。

表2 關鍵幀絕對定位精度對比cm

為了評估本文所提出的方法的實時性能，表3中給出了改進的ORB-SLAM 方法與原來的ORB-SLAM 方法實驗條件下的運行時長對比和數(shù)據(jù)集拍攝時長數(shù)據(jù)。從表中可以看出本文改進的方法在5 個數(shù)據(jù)集上運行時間都較原來的ORB-SLAM 方法要長，這主要是改進的方法不僅運行了基于特征點的ORB-SLAM 方法，而且還在不降低定位精度的前提下構(gòu)建了半稠密的三維場景地圖。受限于計算條件，如普通的筆記本電腦的低計算能力、沒有使用圖形處理器（GPU）算力等條件，雖然本文方法的運行時間對比各個序列的視頻采集時間都有一定的延長，但是在較短的時間內(nèi)構(gòu)建的場景包含了豐富的直線段信息，而原來的單目ORB-SLAM方法只能構(gòu)建稀疏的點云地圖。

表3 實時運行時長對比s

5 結(jié)束語

本文提出一種用于半稠密三維重建的改進單目ORB-SLAM 算法。實驗表明該算法能生成精確的、定位良好的直線段，且能高效地匹配和聚類直線段來進行半稠密三維場景重建。為了方便更好地解釋該改進的單目ORB-SLAM 方法的工作原理，讀者可以在GitHub項目托管平臺上下載本文提出的SLAM 方法的C++代碼。開源項目網(wǎng)址為https：//github.com/BTREE-C802/3DLine-SLAM。

目前，改進的方法只能對二維直線段進行三維重建。未來，計劃持續(xù)改進該方法使其能對圓弧進行三維重建，因為使用直線段與圓弧的組合幾乎可以構(gòu)造出所有常見事物的三維結(jié)構(gòu)。此外，對于局部特定目標的三維重建將是今后的一個研究方向。