范涵奇 吳錦河

視覺里程計(Visual odometry,VO)是在未知環(huán)境中只通過相機獲得的圖像信息來實現(xiàn)機器人定位的技術(shù).近些年來,VO 廣泛應(yīng)用于機器人導航[1]、無人機[2-3]和虛擬現(xiàn)實/增強現(xiàn)實[4]等領(lǐng)域.

視覺里程計使用單目或多目相機作為視覺傳感器.單目相機為主要傳感器的VO 系統(tǒng)雖然成本較低,但面臨的主要問題是單目尺度的不確定性,在計算過程中會發(fā)生尺度漂移[5-6].該問題通常使用多傳感器來解決,例如雷達和多目相機.相比于單目相機,雙目相機可以直接測量三維空間點的位置,避免了尺度的不確定性,因此本文使用雙目相機作為視覺傳感器.

VO 系統(tǒng)分為前端和后端[7].前端通過特征點匹配構(gòu)造數(shù)據(jù)關(guān)聯(lián)來為后端優(yōu)化提供初始位姿.數(shù)據(jù)關(guān)聯(lián)(Data association)是指在幀與幀之間的特征點、特征點與地圖點以及地圖點與地圖點之間構(gòu)建特征對應(yīng)關(guān)系[8].數(shù)據(jù)關(guān)聯(lián)錯誤是VO 系統(tǒng)失敗的主要原因之一.基于圖像特征點的VO 系統(tǒng)數(shù)據(jù)關(guān)聯(lián)方式主要分為兩種.一種是通過計算特征點的描述子來構(gòu)建數(shù)據(jù)關(guān)聯(lián).采用描述子匹配特征點的準確性與魯棒性較高,但是特征描述子的計算非常耗時.Mur-Artal等[9]測試尺度不變特征變換(Scaleinvariant feature teansform,SIFT)[10]和加速健壯特征(Speed-up robust feature,SURF)[11]提取耗時約為300 ms,像這類比較耗時的特征提取算法會影響VO 系統(tǒng)的實時性.為了提高實時性,Mur-Artal等在ORB (Oriented fast and rotated brief)-SLAM(Simultaneous localization and mapping)[9]中選擇計算速度較快的ORB 特征點作為圖像特征,幀與幀之間特征點通過特征描述子匹配.Cvi?i?等[12]在SOFT (Stereo odometry based on feature tracking)中提取Corner 角點和Blob 角點同時計算特征描述子,并通過在連續(xù)幀中追蹤同一特征點,如果該特征能被追蹤到則使用初始的描述子來提高數(shù)據(jù)關(guān)聯(lián)的精確性.由于相機幀率和圖像分辨率越來越高,導致特征提取的計算量越來越大,即使使用ORB 這類速度較快的特征描述子也可能會影響VO 系統(tǒng)的實時性.另一種方式只提取角點而不計算描述子,角點之間的匹配關(guān)系通過稀疏的光流(Optical flow)跟蹤來構(gòu)建[13-14].稀疏的光流算法計算速度快,但光流容易導致特征點誤匹配從而使得數(shù)據(jù)關(guān)聯(lián)錯誤,因此本文使用光流來構(gòu)建數(shù)據(jù)關(guān)聯(lián)的同時采用其他技術(shù)盡可能剔除錯誤的數(shù)據(jù)關(guān)聯(lián).

VO 系統(tǒng)的后端優(yōu)化前端估計的相機初始位姿,本文在后端只優(yōu)化相機位姿而不維護一個全局地圖.在優(yōu)化過程中目標函數(shù)的設(shè)計影響著系統(tǒng)魯棒性.在經(jīng)典的Bundle adjustment (BA)和擴展的卡爾曼濾波(Extended Kalman filter,EKF)算法中都假設(shè)誤差服從高斯分布,優(yōu)化過程中對噪聲敏感,因而導致位姿估計的誤差較大.與高斯分布相比,拉普拉斯分布對大噪聲不敏感.同時,對于長尾數(shù)據(jù)(Long tail data)來說,拉普拉斯分布比高斯分布更適合對大幅噪音的似然描述[15],從而對異常點數(shù)據(jù)更加魯棒.

本文在VO 后端假設(shè)誤差服從拉普拉斯分布,進而構(gòu)造優(yōu)化問題的目標函數(shù).Casafranca等[16]在此假設(shè)下構(gòu)造因子圖優(yōu)化問題.Bustos等[17]通過構(gòu)造旋轉(zhuǎn)的平均只優(yōu)化相機朝向,在相機朝向已知的情況下得到全局最優(yōu)的相機位置和三維空間點.該方法優(yōu)化相機朝向時不受相機位置與三維空間點的影響因此更簡單并且能夠處理純旋轉(zhuǎn)的相機運動,但是該方法運算速度慢,并且在求解相機位置時由于同時優(yōu)化三維空間點和相機位置,誤差較大的三維點會影響相機位置的求解.與Casafranca等[16]和Bustos等[17]的方法不同,本文在后端分開優(yōu)化求解相機朝向、三維空間點以及相機平移,在此過程中假設(shè)相機位姿與三維空間點的誤差都服從拉普拉斯分布.SOFT 等多個不同的算法證實分開估計相機的朝向與位置可以提高相機位姿估計的精確性.

近些年來,基于直接法的雙目視覺里程計越來越受研究者的歡迎,例如,Stereo DSO (Stereo direct sparse odometry)[18]、SVO2 (Semidirect visual odometry 2)[19]和FMD stereo SLAM (Fusing MVG (multiple view geometry)and direct SLAM)[20].Stereo DSO 算法通過采用梯度較大的稀疏像素,使用Bundle adjustment優(yōu)化來得到精度較高的相機位姿.該算法速度較快、魯棒性好,并且可以生成稠密的三維點云,但是對于場景的光照變化比較敏感.SVO2 擴展了半稠密直接法單目視覺里程計SVO[13],SVO2 算法只對選擇的關(guān)鍵幀提取特征點并且采用稀疏圖像對齊算法匹配特征點,因此SVO2 速度快適用于計算平臺受限場合的定位.SVO 采用深度濾波器估計深度,由于初始深度估計具有較大誤差在優(yōu)化時可能無法收斂到真實深度,進而影響相機位姿估計.FMD stereo SLAM 方法融合了多視角幾何和直接法,在前端采用直接法估計初始位姿而在后端采用多視角幾何的方法來估計三維結(jié)構(gòu),這種直接法和多視角幾何法融合的方法同時兼顧了速度與精度.與以上幾種方法相比,本文算法采用特征點法并且引入了誤差服從拉普拉斯分布的假設(shè)來優(yōu)化相機位姿.

本文的組織結(jié)構(gòu)如下:第1 節(jié)簡要介紹本文算法的框架.第 2 節(jié)中詳細闡述特征點的提取以及如何剔除錯誤的特征匹配并篩選穩(wěn)定的特征點.第3節(jié)為相機位姿的估計與優(yōu)化.第4 節(jié)中通過實驗驗證了本文算法的有效性.第5 節(jié)為本文的結(jié)論.

1 系統(tǒng)概述

本節(jié)主要從整體上概述本文提出的算法.算法主要由數(shù)據(jù)關(guān)聯(lián)和相機位姿優(yōu)化估計兩部分組成.數(shù)據(jù)關(guān)聯(lián)是相機位姿估計的預處理過程.在構(gòu)造數(shù)據(jù)關(guān)聯(lián)時,特征點的選擇與數(shù)據(jù)關(guān)聯(lián)的準確性影響著相機位姿估計的精度.在兼顧速度與精度的情況下如何選擇穩(wěn)定的特征點并剔除錯誤的關(guān)聯(lián)是VO 算法重要的一步.位姿估計部分接收篩選的穩(wěn)定特征點來優(yōu)化求解相機位姿并重建出稀疏的環(huán)境地圖.

1.1 特征選擇與數(shù)據(jù)關(guān)聯(lián)

為了提高VO 算法的實時性,需要盡可能快地提取每幀圖像的特征點,因此本文選擇FAST(Features from accelerated segment test)[21]角點作為圖像特征點.數(shù)據(jù)關(guān)聯(lián)主要通過稀疏光流算法來構(gòu)建.雖然稀疏的光流計算速度快,但是往往會導致特征點的誤匹配,因此本文采用三個策略,即平滑的運動約束、視差一致性檢測以及環(huán)形匹配來盡可能剔除錯誤的關(guān)聯(lián),進而提高算法的魯棒性,并在此基礎(chǔ)上選擇穩(wěn)定的特征點.

1.2 相機位姿優(yōu)化

本文算法的主要流程如圖1 所示.其中,P表示三維空間點的位置,p表示三維空間點對應(yīng)的在二維圖像上的投影坐標,R,t分別表示相機的旋轉(zhuǎn)和平移.下標l,r,k為左右相機和圖像幀的索引.在相機位姿估計的過程中,通常對于相機朝向的優(yōu)化估計是比較困難的,因此本文首先去優(yōu)化估計相機的朝向以及三維空間點,然后固定已經(jīng)求得的相機朝向和三維空間點來優(yōu)化求解相機平移.在光流構(gòu)建數(shù)據(jù)關(guān)聯(lián)的過程中,同一特征點可以連續(xù)地在多幀中被跟蹤.本文基于特征點能被連續(xù)跟蹤的幀數(shù)選擇不同的參考幀從而獲得當前幀相機位姿的多個估計.對于當前幀的多個相機位姿估計,在相機位姿誤差服從拉普拉斯分布的假設(shè)下構(gòu)造優(yōu)化問題的目標函數(shù),進而得到位姿的最優(yōu)估計.

2 圖像特征匹配

光流是三維運動在二維圖像平面上的投影.在使用光流構(gòu)建數(shù)據(jù)關(guān)聯(lián)時,需要剔除錯誤的關(guān)聯(lián)以提高數(shù)據(jù)關(guān)聯(lián)的精確性,并且需要在此基礎(chǔ)上篩選穩(wěn)定的特征點.如果一個特征點能在連續(xù)多幀中跟蹤到,那么該特征點是穩(wěn)定的.

一般而言,相機連續(xù)幀之間的運動是平滑的,即前后幀之間的運動量較小,那么圖像幀Ik的運動與前一幀Ik-1的運動是高度相似的,這種現(xiàn)象稱為平滑的運動約束(Smooth motion constraint,SMC).Badino等[22]將該思想應(yīng)用到三維空間中來剔除三維點的誤匹配,本文應(yīng)用到二維的圖像特征點上以提高光流跟蹤的準確性.

由于當前幀的運動與前一幀是相似的,因此可以使用前一幀的運動去預測特征點在當前幀上的位置.即

式中,Pk,Pk-1分別代表同一組三維空間點在當前幀Ik和前一幀Ik-1視角下的位置表示,表示已經(jīng)估計的前一幀的相對運動,即幀Ik-1相對于幀Ik-2的相對位姿變換,π(·)表示投影函數(shù),將三維空間點投影到二維的圖像上,pk為三維空間點Pk在圖像幀Ik上的投影.

由于pk更接近于光流收斂的位置,因此本文使用pk作為光流跟蹤過程中預測的前一幀圖像特征點在當前幀上的初始位置,在特征跟蹤過程中可以消除一定特征點的誤匹配,從而提供更為可靠的特征點匹配信息.

在平滑的運動約束下單純使用光流跟蹤特征點依然存在誤匹配,為了剔除誤匹配增加特征點匹配的正確性,本文采用強約束環(huán)形匹配(Circle matching)[12].圖2 中描述了環(huán)形匹配的整個過程.如圖2 所示,假設(shè)在圖像幀上檢測到某一特征點x,在連續(xù)幀之間的左右圖像上使用光流按照的順序跟蹤特征點x,跟蹤結(jié)束時在幀上的特征點記為x′.本文通過計算特征點x與x′之間的距離d(x,x′)來判斷該環(huán)能否閉合.如果距離d不大于一個像素,則認為該環(huán)能閉合,否則認為該特征點匹配出錯,進而剔除誤匹配的特征點.

圖2 環(huán)形匹配 (使用光流依照箭頭所示順序跟蹤特征點)Fig.2 Circular matching (Use optical flow to track feature points following the order as arrows direct)

雙目相機系統(tǒng)中,視差是一個非常重要的量.在使用以上兩種方法剔除特征點的誤匹配后,為了進一步提高特征點匹配的精確性,通過檢測雙目相機的視差來再次剔除誤匹配.本文在光流跟蹤過程中只計算稀疏特征點之間的視差.對于已經(jīng)校正好的左右相機的圖像幀來說,左右匹配的特征點一定位于水平的極線(Epipolar line)上.從圖像特征點的角度來看,假設(shè) (xl,yl)與 (xr,yr)是左右相機上一對匹配的特征點,則視差的y分量應(yīng)等于0.即:yl=yr,但實際上在圖像校正與特征點跟蹤過程中由于噪聲的存在,等式不是嚴格成立的.因此,本文采用閾值thd來篩除掉大于閾值的匹配點.此外,本文以左邊相機為參考系,則視差在x方向上的分量應(yīng)該為正.本文使用視差一致性(Disparity consistency)來描述這種現(xiàn)象.即

其中,(xl,yl),(xr,yr)為左右圖像上匹配的特征點.如圖3 所示,使用特征選擇策略后能明顯地剔除誤匹配,進而提高數(shù)據(jù)關(guān)聯(lián)的精確性.

圖3 特征點匹配的對比Fig.3 Comparison of feature matching

為了精確地估計相機位姿,本文盡可能選擇穩(wěn)定的特征點.特征點的穩(wěn)定性通過該特征能被觀測到的圖像幀數(shù)來度量.一個特征點能在連續(xù)幀中被觀測到的次數(shù)越多,那么該特征點越穩(wěn)定.如圖4所示,使用光流構(gòu)建數(shù)據(jù)關(guān)聯(lián)時同一特征點可以被連續(xù)跟蹤多幀.SOFT 使用age來描述特征點的穩(wěn)定性,本文同樣采用age來表示一個特征點的穩(wěn)定性.以往的幾項研究[12,23]證實選擇穩(wěn)定的特征點可以提高相機位姿估計的精確性.

圖4 光流特征跟蹤與age說明(同一特征點可以在連續(xù)幀中被跟蹤,age值越大該特征點越穩(wěn)定,T表示兩幀之間的位姿變換)Fig.4 Optical flow feature tracking andagedescription(The same feature point can be tracked in consecutive frames.The larger theage,the more stable the feature point,Trepresents the pose transformation between two frames)

3 相機位姿估計

本節(jié)主要描述了相機位姿估計與稀疏點云的重建.本文將位姿估計分為兩部分.首先,根據(jù)幀與幀之間二維的匹配點直接估計相機的相對旋轉(zhuǎn);然后固定已知的相機旋轉(zhuǎn)來估計平移.

3.1 拉普拉斯分布假設(shè)下目標函數(shù)的構(gòu)造

一般而言,SLAM 可以表示為最大后驗估計(Maximum a posteriori estimation,MAP)問題.假定要估計的未知變量為X,其中,X包括相機位姿和環(huán)境中路標點的位置.由觀測方程zj=hj(Xj)+?j可得到一個觀測集合Z={zj:j=1,2,···,m},其中,Xj?X是估計變量的子集,hj(·)為觀測模型,?j為觀測的噪音.

在MAP 估計中,通過最大化后驗概率來估計變量X,即

其中,p(Z|X)為似然,p(X)為先驗.在沒有先驗信息的情況下,p(X)是一個常量,MAP 退化為最大似然估計(Maximum likelihood estimation,MLE).假定觀測的噪音互不相關(guān),即觀測Z相互獨立,則式(3)分解為

假設(shè)觀測的噪音?j服從具有信息矩陣 Ωj的零均值拉普拉斯分布,則式(4)中的似然為

由于最大后驗估計等價于最小化后驗的負對數(shù),則有

由式(6)可知,在噪音服從拉普拉斯分布的假設(shè)下構(gòu)造了以L1范數(shù)來度量誤差的非線性優(yōu)化問題.因此在相機位姿與三維點云優(yōu)化時,本文在拉普拉斯分布的假設(shè)下采用L1范數(shù)來度量誤差.

傳統(tǒng)上在高斯分布假設(shè)下采用L2范數(shù)作為目標函數(shù)來優(yōu)化估計相機位姿.盡管L2范數(shù)應(yīng)用廣泛,但是拉普拉斯分布假設(shè)下的L1范數(shù)提供了另一種選擇.Moreno等[24]詳細對比了L1范數(shù)和L2范數(shù)在不同噪音水平下的表現(xiàn),證實了L1范數(shù)對較大的異常值更加魯棒.

為了提高L2范數(shù)對較大誤差的魯棒性,通常結(jié)合魯棒的核函數(shù)(例如Huber)來減小誤差較大的數(shù)據(jù)對優(yōu)化結(jié)果的影響.Casafranca等[25]在后端采用L1范數(shù)度量誤差實現(xiàn)了因子圖SLAM,并在實驗中證實了L1范數(shù)在消除大噪音數(shù)據(jù)影響方面比Huber 核更加魯棒.同時相比于Huber 核函數(shù),L1范數(shù)在優(yōu)化時并不需要調(diào)整任何內(nèi)核參數(shù).Bahreinian等[26]研究了在位姿圖優(yōu)化中通過最小化L1范數(shù)能有效剔除離群點的影響.綜上,盡管與魯棒Huber 核函數(shù)類似,但是L1范數(shù)在消除大噪音數(shù)據(jù)的影響方面仍然更加有效,即L1范數(shù)對大噪音數(shù)據(jù)優(yōu)化的魯棒性更好.

3.2 旋轉(zhuǎn)的估計

不同于一般的雙目VO 算法只使用左側(cè)圖像信息來估計相機位姿,為了盡可能多地估計當前幀的旋轉(zhuǎn),本文使用左右相機的圖像信息來分別估計當前幀的旋轉(zhuǎn).

如圖4 所示,本文根據(jù)當前幀上特征點的age值選擇不同的參考幀來估計當前幀的相機朝向.對于首次提取的某一特征點的age值記為0,如果該特征點能跟蹤到下一幀則age值加1,以此類推,age值越大說明該特征點能被連續(xù)跟蹤的幀數(shù)越多.如果當前幀中某一特征點的age值為n,那么說明該特征點可以在前n幀中被跟蹤到,因此本文選擇前n個圖像幀分別作為參考幀來估計當前幀相機的朝向.對于當前幀Ik相機朝向的n個不同估計,使用集合{1Rk,2Rk,···,nRk}來表示,其中Rk代表當前幀Ik相機的絕對朝向.為了得到相機朝向的最優(yōu)估計,假設(shè)噪聲服從拉普拉斯分布,則有

其中,Ri∈{1Rk,2Rk,···,n-1Rk,nRk},S? 為相機旋轉(zhuǎn).通過采用SO(3)上的迭代加權(quán)最小二乘(Iterative reweighted least squares,IRLS)算法獲得相機朝向的最優(yōu)估計S*.算法1 描述了SO(3)上的IRLS 算法.

算法 1.SO(3)上的 IRLS 算法

為了估計當前幀Ik的絕對旋轉(zhuǎn)Rk,本文首先估計選擇的參考幀Ik-i與當前幀Ik之間的相對旋轉(zhuǎn)由于參考幀Ik-i的絕對相機旋轉(zhuǎn)Rk-i已經(jīng)求得,因此可以得到當前幀Ik絕對的相機旋轉(zhuǎn),即:

對于參考幀與當前幀之間的相對旋轉(zhuǎn),傳統(tǒng)上應(yīng)用對極約束根據(jù)幀與幀之間二維特征點的匹配關(guān)系,通過分解本質(zhì)矩陣(Essential matrix)[27]來恢復,但是對于純旋轉(zhuǎn)的相機運動來說,無法從本質(zhì)矩陣中恢復相機的運動.針對這個問題,Fathian等[28]使用四元數(shù)來表示相機的相對旋轉(zhuǎn),提出了QuEst算法來從二維的匹配點中估計相機的運動.該算法需要5 對點來求解相機旋轉(zhuǎn),但通常匹配特征點的數(shù)量多于5 對,因此本文采用QuEst 結(jié)合隨機采樣一致性算法(Random sample consensus,RANSAC)[29]來精確地估計兩個相機視角之間相對的旋轉(zhuǎn)變換.

3.3 稀疏點云重建

重建的三維空間點的精確性影響著相機位置的估計.由第3.2 節(jié),在相機運動的過程中基于特征點的age值,可以在前n幀中多次觀測到同一三維空間點.如圖5 所示,首先根據(jù)雙目相機的視差可以三角化得到同一三維空間點的n個位置表示.然后由于前n幀的相機位姿已知,因此可以使用左右相機前后幀之間特征點的匹配關(guān)系再次三角化得到 2n-2 個同一三維空間的位置,即三角化得到同一三維空間點的 3n-2 個位置.本文使用集合{1P,2P,···,3n-2P}表示同一空間點的多次觀測.為了得到最優(yōu)三維空間點的位置,本文將三維空間點視為歐氏空間 R3中的三維向量,在同一三維空間點的多次觀測中得到該三維空間點的最優(yōu)位置,即

圖5 同一三維空間點的多次三角化(基于特征點的age值,在前n幀中根據(jù)雙目相機的視差以及左右相機連續(xù)幀間特征點的匹配關(guān)系多次三角化同一三維空間點)Fig.5 Multiple triangulations of the same 3D space point (Based on theageof the features,the same 3D space point is triangulated multiple times in the firstnframes according to the disparity of the stereo camera and the matching relationship between the feature points of the left and right camera consecutive frames)

其中,Pi∈{1P,2P,···,3n-2P}.通過采用 R3上的IRLS 算法優(yōu)化三維空間點,得到三維空間點的最優(yōu)位置P*.

3.4 平移的估計

本文使用優(yōu)化后的三維點云與二維點的匹配關(guān)系來求相機的平移.三維空間點投影到二維圖像平面的整個過程為

其中,[u,v,1]T為投影點的齊次坐標表示,f為相機焦距,投影函數(shù)用π來表示,(αx,αy)為圖像的主點,[R|t]為相機兩個視角之間的運動.[x,y,z,1]T為三維點云的齊次坐標表示.

本文固定已經(jīng)估計的旋轉(zhuǎn)和優(yōu)化后的三維空間點云來優(yōu)化估計相機的平移,即

其中,l,r表示左右相機.整個過程中IRLS 算法只迭代優(yōu)化相機的平移,通常僅需迭代少數(shù)幾次即可收斂.與SO(3)上的 IRLS 算法相比,對于R3上的IRLS 算法,只在于計算誤差以及迭代更新的方式不同,R3上直接通過兩個向量之差來定義誤差.

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

本文在KITTI 數(shù)據(jù)集[30]以及New Tsukuba數(shù)據(jù)集[31]上測試提出的算法.實驗環(huán)境為裝有Ubuntu 18.04,配置為Intel? CoreTMi7-4 770 CPU,8 GB RAM 的計算機.KITTI 數(shù)據(jù)集提供了大尺度動態(tài)場景下的22 個立體圖像序列.其中前11 序列(00～10)提供了真實軌跡.New Tsukuba 數(shù)據(jù)集為人工合成的小場景的靜態(tài)室內(nèi)環(huán)境.

本文主要在精度與速度兩個方面來評估算法.在精度方面,主要通過評估估計軌跡與真實軌跡之間的均方根誤差(Root mean square error,RMSE)、均值(Mean)與標準差(Standard deviation,STD)來反映本文算法的精確性,其中均方根誤差代表絕對軌跡誤差,均方根誤差越小則位姿估計越準確.本文使用光流跟蹤特征點選擇的窗口塊大小為9×9,視差一致性檢測的閾值thd=1,驗證是否構(gòu)成回環(huán)匹配的閾值d ≤1.

由于VISO2-S[32]與ORB-SLAM2[33]的作者開放了源代碼,因此本文選擇這兩種算法來與本文算法(算法1)進行對比.表1 中對比了本文算法與ORB-SLAM2在KITTI 數(shù)據(jù)集00～10 序列共11個序列上的測試結(jié)果.從表1 中可知,除了01 序列外,本文算法在其余的10 組序列上位姿估計的結(jié)果較精確.01 序列采集的是高速公路的場景,由于場景紋理較少并且場景紋理相似,導致提取的特征點分布不均勻,且大部分集中在中間的一條線上,只使用圖像特征點在此場景中估計相機位姿是困難的,其他算法在此序列上也存在同樣的問題,導致位姿估計的誤差較大.相比于ORB-SLAM2,本文算法的RMSE 在多數(shù)序列上更小.由于在05和06序列上有閉環(huán),ORB-SLAM2 通過閉環(huán)檢測與校正來進一步提高位姿估計的準確性,而本文算法沒有回環(huán)校正.雖然在05和06 序列上ORB-SLAM2的RMSE 較小,但是本文算法的RMSE 非常接近ORB-SLAM2.

表1 中同時對比了本文算法與VISO2-S 在KITTI數(shù)據(jù)集00～10 序列上估計的相機運動軌跡與真實軌跡的RMSE、Mean和STD.VISO2-S 算法通過最小化重投影誤差并采用高斯牛頓法迭代優(yōu)化求解相機的旋轉(zhuǎn)與平移.通常對于相機的旋轉(zhuǎn)優(yōu)化是比較困難的,相機旋轉(zhuǎn)影響著相機平移的估計,因此采用迭代法同時優(yōu)化求解得到相機旋轉(zhuǎn)與平移的誤差較大.本文算法通過分開獨立的優(yōu)化相機的旋轉(zhuǎn)與平移提高來位姿估計的精確性,從表中數(shù)據(jù)可知本文算法優(yōu)于VISO2-S 算法.

表1 本文算法、ORB-SLAM2 以及VISO2-S 估計的軌跡與真實軌跡之間RMSE、Mean、STD 的對比Table 1 Comparison of RMSE,Mean,STD between the trajectory estimated by ours,ORB-SLAM2,and VISO2-S and the real trajectory

表2 中對比了不同算法在大場景的 KITTI數(shù)據(jù)集上的運行時間,部分數(shù)據(jù)來源于文獻[34]與KITTI Benchmark[30].本文只統(tǒng)計特征處理與位姿優(yōu)化估計所耗費的時間.從表中數(shù)據(jù)可知,由于ORB-SLAM2 采用局部BA 與全局BA 來優(yōu)化相機位姿,因此ORB-SLAM2 位姿優(yōu)化估計部分耗時較多.由于VISO2-S 對左右每幅圖像提取Coner角點和Blob 角點并計算描述子,通過描述子來構(gòu)建圖像幀之間的數(shù)據(jù)關(guān)聯(lián),最后通過最小化重投影誤差求解相機位姿,因此VISO2-S 主要耗時在特征處理部分.FB-KLT 方法提取Shi-Tomasi 角點并通過高斯牛頓法求解相機位姿,從表中實驗數(shù)據(jù)看出該方法主要耗時在特征處理部分.本文算法在特征處理部分速度最快,耗時主要在位姿優(yōu)化估計部分.總體而言,從表2 中可以看出本文算法耗時最少.

表2 計算時間的對比(部分數(shù)據(jù)摘自KITTI Benchmark[30])(ms)Table 2 Computation time comparison(Partial data from KITTI Benchmark[30])(ms)

在室內(nèi)場景的New Tsukuba 數(shù)據(jù)集上,本文算法估計的RMSE 為0.389,ORB-SLAM2 估計的RMSE 為1.015,VISO2-S 估計的RMSE 為0.916,可以看出本文算法表現(xiàn)較好.

在圖6 (a)和圖7 (a)中,給出了本文算法、ORBSLAM2和VISO2-S 估計的相機運動軌跡與真實相機軌跡在KITTI 數(shù)據(jù)集上的對比.而圖6 (b)和圖7 (b)為本文算法在KITTI 數(shù)據(jù)集上重建的周圍環(huán)境的稀疏地圖.

圖6 在 KITTI 數(shù)據(jù)集序列 00～05 上的實驗Fig.6 Experiments on the KITTI sequence 00～05

圖7 在 KITTI 數(shù)據(jù)集序列 06～10 上的實驗Fig.7 Experiments on the KITTI sequence 06～10

在圖8(a)中,給出了New Tsukuba 數(shù)據(jù)集上本文算法、ORB-SLAM2 以及VISO2-S 估計的軌跡與真實軌跡的對比.從軌跡對比圖中可以看出,本文算法估計的軌跡更接近于真實軌跡.圖8(b)為本文算法在New Tsukuba 數(shù)據(jù)集上重建的稀疏環(huán)境地圖.

圖8 在 New Tsukuba 數(shù)據(jù)集上的實驗Fig.8 Experiments on New Tsukuba sequence

5 結(jié)束語

本文基于拉普拉斯分布提出了一種快速精確的雙目視覺里程計算法.為了提高相機位姿與三維空間點估計的精確性,在相機旋轉(zhuǎn)、三維空間點以及相機平移這些量的誤差服從拉普拉斯分布的假設(shè)下來構(gòu)造優(yōu)化問題的目標函數(shù).通過在SO(3)以及R3上的優(yōu)化來得到位姿和三維空間點的最優(yōu)估計.KITTI 以及New Tsukuba 數(shù)據(jù)集上的實驗結(jié)果表明本文提出的算法能夠快速準確地估計相機位姿與三維空間點的位置.本文算法尚有一定的局限性,在構(gòu)建數(shù)據(jù)關(guān)聯(lián)時只使用了光流跟蹤,由于光流自身的原因,對于亮度變化明顯的環(huán)境不夠魯棒.未來的工作中,將進一步研究提高光流跟蹤的精確性并嘗試將視覺信息與慣性測量結(jié)合起來,增加系統(tǒng)的魯棒性.