基于稀疏直接法閉環(huán)檢測定位的視覺里程計

汝少楠，何元烈，葉星余

（廣東工業(yè)大學 計算機學院，廣東 廣州 510006）

同時定位與地圖構(gòu)建[1](Simultaneous Location And Mapping，SLAM)是指搭載傳感器的主體，在沒有環(huán)境先驗信息的情況下，于運動過程中跟蹤主體的位置，并同時構(gòu)建環(huán)境結(jié)構(gòu)一致性地圖的技術(shù)。近年來視覺同時定位與地圖構(gòu)建技術(shù)已經(jīng)在室內(nèi)服務(wù)機器人、自動駕駛汽車、無人機導航定位以及增強現(xiàn)實設(shè)備中得到一定的應(yīng)用。

視覺里程計[2](Visual Odometry，VO)可分為兩類：基于特征的方法和直接方法。基于特征的方法在每個圖像中提取顯著的圖像特征，使用不變的特征描述符在連續(xù)幀中進行匹配，使用極線幾何約束恢復(fù)相機位姿和結(jié)構(gòu)，通過最小化投影誤差來優(yōu)化位姿和結(jié)構(gòu)提取的特征也可以用于閉環(huán)和重定位，從而使視覺里程計成為一個完整的SLAM。

Klein和Murray[2]于2007年開發(fā)出實時的PTAM(Parallel Tracking and Mapping)。該算法對于單目SLAM的發(fā)展具有里程碑式的意義。算法將SLAM框架中的位姿跟蹤和建圖分離成兩個線程單獨運行，讓離線的SFM(Structure From Motion)算法可以在設(shè)備上實時的運行。Gomez-Ojeda等[3]使用聯(lián)合點線特征進一步提升精確度。UcoSLAM[4]在圖像中提取特征點和分布大量的標記，然而，特征提取和匹配非常耗時，因此基于特征的方法比直接方法慢。

直接方法通過光度誤差直接恢復(fù)相機的位姿和結(jié)構(gòu)，不需要特征提取。不同于PTAM、ORB_SLAM(Oriented FAST and Rotated BRIEF-Simultaneous Localization and Mapping)[5]的低幀率，由Forster[6]開源的SVO(Semi-direct Visual Odometry)系統(tǒng)采用直接法或特征法來獲取場景中的特征信息。對于其中深度估計問題，岑仕杰[7]結(jié)合了注意力與無監(jiān)督深度學習方法，但是在遠景的部分泛化能力不夠好，估計的深度比實際要近。然而，直接SLAM的閉環(huán)檢測仍然是一個開放的課題。長期移動的累積漂移是直接法所遇到的主要問題之一。

閉環(huán)檢測算法在過去十幾年里取得卓越的進展，對解決自主定位中的漂移和丟失跟蹤時的重定位至關(guān)重要。Angeli[8]首次提出將閉環(huán)檢測算法模塊加入視覺SLAM算法系統(tǒng)，并成功運用詞袋模型解決當移動機器人運行于大型室內(nèi)環(huán)境中，視覺特征的差異使得算法能有效地尋找出場景間的不同，并識別出之前曾訪問過的視覺場景，進而消除累積誤差。近年來何元烈等[9]嘗試使用神經(jīng)網(wǎng)絡(luò)方法檢測閉環(huán)，但受環(huán)境光照影響較大。

為保持直接方法的快速性和基于特征的方法的高精度和閉環(huán)檢測優(yōu)化，本文提出一種新的基于稀疏直接方法的混合型梯度-特征點，該方法提取兩種特征：梯度點和ORB(Oriented FAST and Rotated BRIEF)特征[10]。首先，使用梯度點和少量ORB特征點對齊圖像序列。然后，選取關(guān)鍵幀中ORB特征點，這些特征點在普通幀中動態(tài)更新深度，收斂后加入地圖。并且ORB特征還用來檢測和閉環(huán)優(yōu)化。本文方法相比特征法節(jié)省時間；與直接方法相比，該方法具有導航必須的閉環(huán)檢測功能。此外，本文對所提出的稀疏直接法視覺里程計閉環(huán)定位系統(tǒng)與最新方法在開放數(shù)據(jù)集中進行了比較。

1 系統(tǒng)概述

本文方法流程圖如圖1所示。該系統(tǒng)使用3個并行線程，分別用于估計相機運動、建圖和閉環(huán)檢測優(yōu)化。

圖1 系統(tǒng)線程圖Fig.1 System thread diagram

運動估計線程使用稀疏直接方法實現(xiàn)相對位姿估計。(1) 通過基于稀疏模型的圖像對齊初始化位姿：通過最小化與相同3D點的投影位置相對應(yīng)的像素之間的光度誤差來找到相對于前一幀的相機位姿。(2) 通過對齊相應(yīng)的特征塊，對于重新投影點對應(yīng)的2D坐標進行細化。運動估計是通過最小化前一個特征對齊步驟中引入的重投影誤差細化位姿。

在建圖線程中，圖像幀被分為關(guān)鍵幀和普通幀，在關(guān)鍵幀上選取特征點，作為種子，并且初始化一個非常不確定深度估計[6]。在傳入新的普通幀時，融合更新這些種子的深度。當種子深度收斂時，把這個特征點插入到地圖，用于運動估計。否則繼續(xù)等待下一次深度更新。傳入新的關(guān)鍵幀時，再次提取新的種子，整個過程處于動態(tài)更新。

最后是閉環(huán)優(yōu)化線程，每一關(guān)鍵幀成為候選檢測幀。這樣，如果檢測并驗證閉環(huán)候選，則計算其相對于當前關(guān)鍵幀的相似變換群Sim(3)約束并將其添加到全局位姿圖中，然后對全局位姿圖進行優(yōu)化以獲得更精確的長期相機位姿估計。

2 稀疏直接法視覺里程計中的閉環(huán)

2.1 運動估計

利用直接法對相機的相對運動和特征對應(yīng)關(guān)系進行初始估計，最后得到基于特征的非線性重投影誤差。如圖2所示，在運動估計線程中，相機運動到當前幀 Ik時，已知上一幀 Ik－1、 上一幀的特征點u 形成的圖像區(qū)域 R，和u投影到三維空間的地圖點p，計算相機從 Ik－1到 Ik的位姿變換Tk,k－1：

圖2 兩圖像幀間的灰度誤差Fig.2 Gray error between two image frames

這里使用逆合成Lucas-Kanade算法來解決這種對齊問題。與前面的步驟相反，對參考特征塊應(yīng)用仿射變換 Ai，因為關(guān)鍵幀通常比上一幀圖像更遠，使用較大的圖像塊(8 ×8)易于觀察。

使用約束調(diào)整和迭代非線性最小二乘最小化算法求解，如高斯牛頓迭代，這樣就可以得到基于直接法的相機位姿估計。一般來說，在相機緩慢運動時，此估計較為準確，但環(huán)境劇烈變化時跟蹤的相機軌跡容易丟失，因此在2.2節(jié)介紹本文提出的特征點提取策略彌補這一不足。

2.2 混合梯度-特征點

當相機運動劇烈時，采用基于特征的方法來保證魯棒性。即使是基于直接的方法也提取一些像素點，直接方法不考慮這些點在未來的圖像中能否再次出現(xiàn)，為了有效解決直接方法選取的點不具有可重復(fù)性的問題，本文提出一種新的特征點提取方法：混合型梯度-特征點?；旌闲吞荻?特征點由兩部分組成，分別是圖像梯度點和ORB特征點。梯度點指的是該圖像中梯度明顯的像素，用于直接法的相機追蹤；另一部分是ORB特征點，它在圖像隊列中多次出現(xiàn)，用來檢測閉環(huán)和跟蹤。

對于梯度點的選取，算法模型將圖像轉(zhuǎn)換成圖像金字塔，本文為4層。每一層劃分成多個圖像塊，從頂層每個圖像塊中使用像素差平方和求光度誤差的方法提取梯度較大的點作為特征點 Ugrad，依次傳遞到底層。如果圖像塊中沒有梯度明顯的像素，比如墻面等紋理稀疏的環(huán)境，則降低梯度要求直到選取足夠的點，這樣可以提升系統(tǒng)的魯棒性，避免系統(tǒng)在跟蹤時丟失。

混合型梯度-特征還提取一些ORB特征，ORB特征由FAST(Features from Accelerated Segment Test)角點和BRIEF(Binary Robust Independent Elementary Features)描述子組成，具有256位描述符，特征提取和匹配速度快，具有較高的魯棒性、高效性，可應(yīng)對平移、旋轉(zhuǎn)、光照變化等情況。FAST角點本身不具有方向性和尺度，系統(tǒng)構(gòu)建圖像金字塔，在每一層檢測角點實現(xiàn)尺度不變性；通過指定灰度質(zhì)心為旋轉(zhuǎn)方向可得到具有旋轉(zhuǎn)不變性的oFAST(oriented FAST)描述符。假設(shè)灰度質(zhì)心偏移圖像重心，根據(jù)偏移構(gòu)建向量定義為特征點方向。連接圖像塊S 的幾何中心與質(zhì)心，構(gòu)成一個方向向量c， 定義特征點方向θ 為

其中m 是圖像的矩。

BRIEF描述子是一種沒有方向信息的二進制編碼，利用oFAST角點的方向信息 θ可以計算出Steer BRIEF，使其具有旋轉(zhuǎn)不變性。

視覺里程計前端使用梯度點和ORB特征點跟蹤相機，這樣閉環(huán)線程的特征提取步驟的開銷保持在最小。

2.3 閉環(huán)幀檢測

本文通過在圖像幀中選擇符合條件的幀為關(guān)鍵幀，并以其作為閉環(huán)檢測的圖像幀進行檢索、對比、匹配，減少地圖生成累計誤差，校正位姿估計誤差。如圖3所示，模型使用稀疏直接法視覺里程計的選擇關(guān)鍵幀策略[6]，用稀疏直接法粗略估計圖像幀的位姿，并判斷是不是關(guān)鍵幀。另外再增加兩條關(guān)鍵幀選擇的條件：相機旋轉(zhuǎn)大于30度、當前幀與上一關(guān)鍵幀間隔30幀，這兩個條件保證在快速運動時跟蹤的魯棒性和在非關(guān)鍵幀下跟蹤的準確性。

圖3 關(guān)鍵幀特征匹配和局部閉環(huán)Fig.3 Feature matching and local loop-closure of key frame

閉環(huán)檢測算法計算每個關(guān)鍵幀的特征描述子時，使用最新DBoW3庫構(gòu)建視覺詞袋模型[11]。前端視覺里程計篩選出的關(guān)鍵幀組成候選隊列，候選檢測是當前關(guān)鍵幀與候選隊列中的幀尋找相匹配，這里優(yōu)先選擇遠離當前幀的關(guān)鍵幀。如圖3所示，當前關(guān)鍵幀(右側(cè)深色圖像幀)與候選隊列中某個(左側(cè)橫線條紋圖像幀)關(guān)鍵幀有一定數(shù)量的共視特征點(圖中黑色地圖點)，相匹配的關(guān)鍵幀數(shù)目大于閾值，執(zhí)行RANSAC(RANdomSAmple Consensus，隨機抽樣一致)算法解PnP(Perspective-n-Point)得到相機位姿的初始估計，利用高斯牛頓法對Sim(3)優(yōu)化，使3D和2D幾何約束最小化。本文使用文獻[12]計算位姿從閉環(huán)候選到當前關(guān)鍵幀Scr的Sim(3)轉(zhuǎn)換估計：

2.4 位姿圖優(yōu)化

在上一步閉環(huán)檢測得到的閉環(huán)幀以及閉環(huán)候選中的關(guān)鍵幀可以看作是一個位姿圖，頂點是待優(yōu)化的相機位姿，邊表示誤差項。采用圖優(yōu)化庫g2o[13]對位姿進行優(yōu)化。

本文的閉環(huán)檢測方法是計算閉環(huán)候選幀和當前幀之間的相對位姿約束，所以還利用兩兩相對位姿之間的觀測值來近似為候選關(guān)鍵幀之間的約束。具體來說，優(yōu)化算法計算從前端的當前全局位姿估計這些觀測值。另外，在位姿圖優(yōu)化中將修改當前幀的位姿估計，但全局優(yōu)化不能干擾包含絕對位姿信息的閉環(huán)優(yōu)化結(jié)果。因此，位姿圖優(yōu)化將傾向于修改閉環(huán)以外部分的全局位姿。在位姿圖優(yōu)化后，不更新閉環(huán)內(nèi)相機的全局位姿，以進一步確保閉環(huán)內(nèi)相機位姿的約束調(diào)整不受全局優(yōu)化的影響。

3 實驗結(jié)果

為驗證混合梯度-特征點和閉環(huán)優(yōu)化給稀疏直接里程計的影響，設(shè)計實驗運行EuRoC數(shù)據(jù)集和TUM數(shù)據(jù)集評估系統(tǒng)性能。本文實驗所用計算機CPU為AMD 2600，主頻3.4 GHz，內(nèi)存8 GB，搭載Ubuntu16.04。實驗主要分為兩部分：(1) 比較本文混合特征梯度-特征點算法、SVO的對視覺里程計前端時間、特征點數(shù)、精確度的影響；(2) 利用EuRoC數(shù)據(jù)集及測試工具，比較本文方法、SVO、ORB_SLAM2[14]等的估計軌跡與真實軌跡誤差。

為減少計算機系統(tǒng)的不確定性對算法的影響，每個序列運行5次記錄平均值。通過測試工具(evaluation of odometry and SLAM)對齊所有軌跡，再與真實軌跡進行比較計算出均方根誤差(Root Mean Square Error，RMSE)。

3.1 特征點選取對比

將本文方法(無回環(huán))與SVO進行均方根誤差、前端平均每幀運行時間做對比試驗，使用TUM數(shù)據(jù)集中的RGB-D數(shù)據(jù)集。均方根誤差和平均每幀運行時間如表1所示。

表1 本文算法與SVO運行TUM數(shù)據(jù)集結(jié)果對比Table 1 Comparison of the results of our algorithm and SVO algorithm running the TUM dataset

從表1中看出，測試同樣的數(shù)據(jù)集序列，本文方法的每幀運行時間只比SVO略微增加2～7 ms，但均方根誤差小于SVO。fr1_xyz和fr2_xyz序列僅包含沿運動主軸的平移運動，而相機的朝向保持固定，屬于簡單數(shù)據(jù)集，SVO處理這種場景的誤差已經(jīng)足夠小，因此提升很有限。fr1_floor序列掃過的辦公室地板上有幾個結(jié)洞，本文方法提取ORB特征點很容易追蹤軌跡。此外，地板都是紋理稀疏的平面，視覺里程計易于陷入局部最優(yōu)的情況。fr2_desk序列記錄一個典型的辦公室場景，本文方法相較SVO精度有所提高，代價是運行時間略微提升。

3.2 軌跡對比

軌跡對比實驗使用公共數(shù)據(jù)集EuRoC[15]，EuRoC收集自20 Hz的微型飛行器。數(shù)據(jù)集分為兩部分，第一部分記錄在一個工業(yè)機器大廳中，其中包含由激光跟蹤儀測量的毫米級精度位置坐標。數(shù)據(jù)集的第二部分記錄在具有兩個不同場景的室內(nèi)。根據(jù)軌跡長度、光照和飛行動態(tài)，所有序列分為3個級別：簡單、中等和困難。表2列出了本文方法、SVO、DSO[16](Direct sparse odometry)和ORB_SLAM2的均方根誤差、每幀運行時間。

表2 各方法運行EuRoC數(shù)據(jù)集的均方根誤差和平均每幀運行時間對比Table 2 Comparison of the root mean square error and the average running time per frame of the various methods running the EuRoC datasets

表2中本文方法、SVO和ORB保存了關(guān)鍵幀軌跡，根據(jù)DSO開源代碼保存所有幀軌跡的特性，為了進行有效的比較，DSO每幀的閾值數(shù)設(shè)置為800。

圖4展示了本文方法運行MH_02和MH_04的關(guān)鍵幀軌跡圖與關(guān)鍵幀軌跡誤差圖，圖5展示本文方法與ORB_SLAM2運行V1_01_easy的關(guān)鍵幀軌跡對比。

EuRoC數(shù)據(jù)集中有相機快速旋轉(zhuǎn)導致的嚴重運動模糊和光照不足的環(huán)境，SVO追蹤時丟失相機軌跡，無法運行大部分序列。本文方法使用混合型梯度-特征點策略提取梯度點和ORB特征具有較強的魯棒性，因而可以運行。

由于加入回環(huán)檢測，本文方法相較于SVO有著很大的提升，特別是V1_01_easy和V2_01_easy兩個標記簡單的序列，本文方法性能優(yōu)于ORB_SLAM2，而有相機快速運動和光照不足的MH_03_medium、MH_04_difficult和MH_05_difficult序列也能夠運行。在每一個序列開始時有極不穩(wěn)定的相機運動，對DSO的影響比本文方法或ORB的影響更大。因此，DSO(特征數(shù)目為800)的精度低于其他兩種方法。本文方法不匹配非關(guān)鍵幀中的特征，精度低于ORB。然而，在相似的精度下，本文方法比其他兩種方法要快得多，ORB_SLAM2平均處理一幀圖像需要32.06 ms，DSO為23.66 ms，本文方法僅需要13.21 ms，圖像的處理速度約為ORB_SLAM2的2倍，實時性優(yōu)于ORB_SLAM2和DSO，因此本文方法更適合于輕型飛行器或機器人。

圖4 本文方法(our_method)與真實軌跡(ground truth)、ORB_SLAM2、DSO運行V2_01_easy的關(guān)鍵幀軌跡對比Fig.4 Our method is compared with the key frames trace of ground truth, ORB_Slam2 and DSO running V2_01_easy

圖5 本文方法運行數(shù)據(jù)集序列的軌跡圖，圖中g(shù)round truth和reference都為真實軌跡Fig.5 Our method runs the trajectory diagram of the data set sequence, in which the ground truth and reference are both real trajectories.

表3對比本文方法和Richard Elvira的ORBSLAMAtlas[17]，使用了地圖集合存儲了多張子圖。從表中看出MH_02_easy簡單序列ORBSLAM-Atlas更占優(yōu)勢，本文方法在復(fù)雜的序列中較優(yōu)。

本文方法可以認為是直接法和特征點法的結(jié)合，追蹤使用直接法，閉環(huán)優(yōu)化使用特征點法。實際實驗對原模型進行改進，以完成兩種方法的結(jié)合。混合型梯度-特征點提取兩種點，增加相對較少的時間，在紋理稀疏和復(fù)雜的環(huán)境中也能提升視覺里程計的精度。又結(jié)合閉環(huán)檢測，大幅提升直接法的精度，由于間接點法需要大量時間計算特征點描述子和特征匹配，本文方法的處理時間少于ORB_SLAM2的處理時間。相比之下，大部分幀將采用直接方法來估計相機運動，當相機快速運動時，采用基于特征的方法保證魯棒性。

表3 本文算法與ORBSLAM-Atlas運行EuRoC數(shù)據(jù)集結(jié)果對比Table 3 Comparison of the results of our algorithm and ORBSLAMAtlas algorithm running the EuRoC dataset

4 結(jié)束語

本文介紹一種新的特征點提取策略，即混合型梯度-特征點，在快速運動中使系統(tǒng)保持穩(wěn)定性。本文基于稀疏直接法加入閉環(huán)檢測和位姿優(yōu)化，明顯提升系統(tǒng)的定位精度。利用圖像像素強度來估計相機位姿。通過關(guān)鍵幀匹配ORB特征，使用視覺詞袋模型檢測閉環(huán)，再使用位姿圖優(yōu)化得到精確的軌跡。在實際環(huán)境中特征選擇策略非常重要，因為更多的有用特征可以提高系統(tǒng)的魯棒性和準確性，本文實驗結(jié)果為其他SLAM方法的設(shè)計和改進提供有價值的參考，未來還可以擴展包括通過減少關(guān)鍵幀的數(shù)量提高速度，提高運動模糊的魯棒性等。