基于拉普拉斯分布的雙目視覺(jué)里程計(jì)

范涵奇 吳錦河

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

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

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

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

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

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

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

1 系統(tǒng)概述

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

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

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

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

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

2 圖像特征匹配

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

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

由于當(dāng)前幀的運(yùn)動(dòng)與前一幀是相似的,因此可以使用前一幀的運(yùn)動(dòng)去預(yù)測(cè)特征點(diǎn)在當(dāng)前幀上的位置.即

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

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

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

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

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

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

圖3 特征點(diǎn)匹配的對(duì)比Fig.3 Comparison of feature matching

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

圖4 光流特征跟蹤與age說(shuō)明(同一特征點(diǎn)可以在連續(xù)幀中被跟蹤,age值越大該特征點(diǎn)越穩(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 相機(jī)位姿估計(jì)

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

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

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

在MAP 估計(jì)中,通過(guò)最大化后驗(yàn)概率來(lái)估計(jì)變量X,即

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

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

由于最大后驗(yàn)估計(jì)等價(jià)于最小化后驗(yàn)的負(fù)對(duì)數(shù),則有

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

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

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

3.2 旋轉(zhuǎn)的估計(jì)

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

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

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

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

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

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

3.3 稀疏點(diǎn)云重建

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

圖5 同一三維空間點(diǎn)的多次三角化(基于特征點(diǎn)的age值,在前n幀中根據(jù)雙目相機(jī)的視差以及左右相機(jī)連續(xù)幀間特征點(diǎn)的匹配關(guān)系多次三角化同一三維空間點(diǎ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}.通過(guò)采用 R3上的IRLS 算法優(yōu)化三維空間點(diǎn),得到三維空間點(diǎn)的最優(yōu)位置P*.

3.4 平移的估計(jì)

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

其中,[u,v,1]T為投影點(diǎn)的齊次坐標(biāo)表示,f為相機(jī)焦距,投影函數(shù)用π來(lái)表示,(αx,αy)為圖像的主點(diǎn),[R|t]為相機(jī)兩個(gè)視角之間的運(yùn)動(dòng).[x,y,z,1]T為三維點(diǎn)云的齊次坐標(biāo)表示.

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

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

4 實(shí)驗(yàn)結(jié)果與分析

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

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

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

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

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

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

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

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

在圖6 (a)和圖7 (a)中,給出了本文算法、ORBSLAM2和VISO2-S 估計(jì)的相機(jī)運(yùn)動(dòng)軌跡與真實(shí)相機(jī)軌跡在KITTI 數(shù)據(jù)集上的對(duì)比.而圖6 (b)和圖7 (b)為本文算法在KITTI 數(shù)據(jù)集上重建的周?chē)h(huán)境的稀疏地圖.

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

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

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

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

5 結(jié)束語(yǔ)

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