融合光度和深度的視覺(jué)里程計(jì)改進(jìn)算法

黃宴委，董文康，王 俊，陳少斌

(福州大學(xué)電氣工程與自動(dòng)化學(xué)院,福建 福州 350108)

0 引言

同步定位與繪圖(simultaneous location and mapping，SLAM)算法中的視覺(jué)里程計(jì)(visual odometry，VO)主要有特征點(diǎn)法和直接法實(shí)現(xiàn)[1].相較于特征點(diǎn)法[2]，直接法對(duì)特征較少的環(huán)境有較好的魯棒性，并且可以構(gòu)建稠密和半稠密地圖.文[3]利用RGB-D相機(jī)(深度相機(jī))獲取的深度圖像，通過(guò)迭代最近點(diǎn)算法估計(jì)相機(jī)位姿，構(gòu)建三維稠密地圖.文[4]提出在假設(shè)光度誤差服從t分布模型，采用光度誤差能夠較好地構(gòu)建紋理豐富環(huán)境的地圖，但是對(duì)紋理較少而結(jié)構(gòu)較明顯的環(huán)境建圖精度不高.文[5]在文[4]基礎(chǔ)上同時(shí)融合了深度和光度，提出一種DVO SLAM方法，其結(jié)果比文[3]中的絕對(duì)路徑誤差(absolute trajectory error,ATE)降低了83%.文[6]在保留光度誤差的條件下，通過(guò)逆深度誤差實(shí)現(xiàn)前端視覺(jué)里程計(jì)的設(shè)計(jì)，比直接采用深度值降低了相對(duì)位姿誤差(relative pose error,RPE).文[7]采用t分布模型擬合光度誤差，用高斯噪聲模型擬合深度誤差，提高了位姿的準(zhǔn)確性.文[8]提出一種基于概率面元的RGB-D SLAM算法提高位姿精度.本研究提出一種融合光度和深度的改進(jìn)視覺(jué)里程計(jì)(modified visual odometry，MVO)算法，提高SLAM算法對(duì)紋理較少環(huán)境的建圖精度.在文[5]基礎(chǔ)上，考慮不同場(chǎng)景下的紋理和結(jié)構(gòu)情況，采取不同權(quán)重構(gòu)建目標(biāo)函數(shù)，求解每個(gè)像素點(diǎn)的光度和深度誤差權(quán)重，提高VO估計(jì)的位姿的準(zhǔn)確性.在后端算法中，采用基于g2o框架的圖優(yōu)化方法得到全局一致的相機(jī)位姿[9].對(duì)10個(gè)標(biāo)準(zhǔn)數(shù)據(jù)集的性能測(cè)試[10]結(jié)果顯示，MVO SLAM具有很高的建圖精度，在紋理較少的環(huán)境時(shí)優(yōu)勢(shì)明顯.

1 基于光度和深度的VO

深度相機(jī)獲取的一幀RGB-D圖像，包含光度圖像L和深度圖像D.已知光度圖像L上的像素點(diǎn)x，像素坐標(biāo)為[x,y]T，x，y分別是像素坐標(biāo)系下的橫、縱坐標(biāo).從深度圖像D中，得到像素點(diǎn)x的深度值d.由針孔相機(jī)模型得到像素點(diǎn)x在相機(jī)坐標(biāo)系下的相機(jī)坐標(biāo)為P=[XYX]T，X，Y，Z表示相機(jī)坐標(biāo)系下的坐標(biāo)軸.

(1)

(2)

式中：π(P)為相機(jī)坐標(biāo)變換到齊次像素坐標(biāo)的函數(shù).

1.1 相機(jī)位姿變換函數(shù)

圖1 相鄰兩幀圖像位姿變換Fig.1 Pose transformation of two adjacent frames

相機(jī)的位姿變換如圖1表示.對(duì)于相鄰兩個(gè)時(shí)刻的RGB-D圖像數(shù)據(jù)幀L1，D1和L2，D2,其中:L1，D1表示前一時(shí)刻的光度圖和深度圖;L2，D2表示后一時(shí)刻的光度圖和深度圖.

以光度圖L1，L2為例，用變換矩陣T表示L1幀到L2幀的變換，

(3)

式中：R是旋轉(zhuǎn)矩陣；t是平移向量；T∈SE(3)，SE(3)是特殊歐式群.采用相對(duì)于SE(3)空間的李代數(shù)se(3)=[ξ∈R6,ξ^∈R]表示相機(jī)位姿變換，其中符號(hào)^表示將向量轉(zhuǎn)換成矩陣.則ξ為，

ξ=(v1,v2,v3,w1,w2,w3)T

(4)

式中：v1,v2,v3稱為平移線速度；w1,w2,w3稱為旋轉(zhuǎn)角速度.則式(3)經(jīng)指數(shù)矩陣變換得:

T=exp(ξ＾)

(5)

對(duì)光度圖L1上的任一像素點(diǎn)x，根據(jù)深度圖像得到相應(yīng)的深度值Z=Z(x)，其中Z(·)表示取像素點(diǎn)的深度值.首先根據(jù)式(1)將該像素點(diǎn)映射到對(duì)應(yīng)于L1幀的相機(jī)坐標(biāo)P=[XYZ]，然后根據(jù)相機(jī)變換矩陣T，將L1幀下的相機(jī)坐標(biāo)P變換到L2幀下的相機(jī)坐標(biāo)P′.

P′=TP

(6)

x′=τ(x,T)=π(Tπ-1(x,z(x)))

(7)

1.2 融合光度和深度的位姿目標(biāo)函數(shù)

定義L1幀中的像素點(diǎn)x由式(7)中的變換函數(shù)τ變換到L2幀下的像素點(diǎn)后，兩像素點(diǎn)的光度誤差rI為

rI=I(τ(x,T))-I(x)

(8)

式中：I為像素點(diǎn)的光度值.也可得到像素點(diǎn)x經(jīng)變換后的深度誤差rZ為

rZ=Z(τ(x,T))-[Tπ-1(x,z(x))]Z

(9)

對(duì)于相鄰兩幀RGB-D圖像，設(shè)第i個(gè)像素點(diǎn)的光度誤差為rI,i，深度誤差為rZ,i，令ri=(rI,i，rZ,i)T，用向量ξ∈R6及其矩陣指數(shù)函數(shù)來(lái)表示相機(jī)位姿.文[5]采用v=5，均值u=0，協(xié)方差為∑的二維t分布模型擬合光度和深度誤差，構(gòu)建目標(biāo)函數(shù)為

(10)

式中：n表示每幀圖像所取的像素點(diǎn)總數(shù).式(10)只是將光度和深度以相同的比例進(jìn)行融合，并未考慮兩者之間的權(quán)重.由于不同場(chǎng)景的結(jié)構(gòu)和紋理不同，相鄰兩幀圖像的光度誤差和深度誤差之間的權(quán)重應(yīng)根據(jù)環(huán)境的結(jié)構(gòu)、紋理的不同而有所區(qū)別.為平衡兩幀RGB-D圖像的光度誤差rI=(rI,1,…,rI,n)和深度誤差之間rZ=(rZ,1,…,rZ,n)的權(quán)重，在每幀深度圖的深度誤差前加入權(quán)重系數(shù)λ，即λrZ，通過(guò)最小二乘構(gòu)建相機(jī)位姿的目標(biāo)函數(shù)為

(11)

對(duì)每個(gè)像素的光度誤差和深度分別加上權(quán)重WI,i和WZ,i，最終構(gòu)建目標(biāo)函數(shù)為

(12)

式中：WI,i和WZ,i分別表示第i個(gè)像素點(diǎn)的光度誤差和深度誤差的權(quán)重；n表示每幀圖像所取的像素點(diǎn)總數(shù)；λ是相鄰兩幀RGB-D圖像的深度誤差相較于光度誤差所占的幀權(quán)重.式(12)是關(guān)于位姿ξ的非線性函數(shù)，分別將光度誤差函數(shù)rI和深度誤差函數(shù)rZ進(jìn)行線性化，迭代得到最優(yōu)的ξ.假定函數(shù)rI和rZ是局部連續(xù)且光滑可導(dǎo)的，分別將rI和rZ關(guān)于ξ進(jìn)行一階泰勒展開(kāi)，將所有n個(gè)像素，代入式(12)中，用雅可比矩陣的形式表示，

JTWJΔξ=-JTWr

(13)

式中：J是n(n是所取的像素點(diǎn)總數(shù))個(gè)像素點(diǎn)的光度誤差和深度誤差關(guān)于ξ的雅可比矩陣，維數(shù)為2n6，W是n個(gè)像素點(diǎn)的權(quán)重矩陣，其維數(shù)是2n2n，Δξ是每次迭代的增量，為61向量.J的具體形式為

(14)

式中：J1和J2分別是n個(gè)像素點(diǎn)的光度誤差和深度誤差關(guān)于ξ的雅可比矩陣；JI和JZ稱為像素點(diǎn)的亮度和深度梯度矩陣，其維數(shù)是n2n；Jw稱為變換函數(shù)τ關(guān)于相機(jī)坐標(biāo)系下的三維點(diǎn)的導(dǎo)數(shù)，其維數(shù)是2n3n；JT稱為變換后的三維點(diǎn)對(duì)相機(jī)位姿ξ的導(dǎo)數(shù)，其維數(shù)是3n6.式(13)通過(guò)迭代求解，在第k次迭代ξ(k)，首先將等式左邊經(jīng)過(guò)柯列斯基分解，然后使用高斯-牛頓迭代算法[11]求解位姿增量Δξ，并根據(jù)下式更新相機(jī)位姿ξ：

ξ(k+1)=lg(exp(ξ(k))exp(Δξ))

(15)

當(dāng)?shù)螖?shù)大于100次，或者Δξ小于510-7時(shí)，迭代停止，將此時(shí)的ξ(k)作為最終估計(jì)的相機(jī)位姿ξ.

1.3 基于t分布的像素點(diǎn)光度和深度誤差權(quán)重

采用自由度為v，方差為σ2的t分布誤差模型來(lái)計(jì)算第i個(gè)像素點(diǎn)相應(yīng)的WI,i和WZ,i.以光度誤差為例，通過(guò)t分布模型推導(dǎo)出每個(gè)像素點(diǎn)的權(quán)重.設(shè)第i個(gè)像素光度誤差為rI,i，光度誤差權(quán)重為WI,i，每個(gè)像素點(diǎn)的權(quán)重為：

(16)

為確定自由度v的值，選擇TUM提供的RGB-D數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)，該數(shù)據(jù)集提供了由Kinect相機(jī)采集的RGB圖像序列和深度圖像序列，以及由高精度運(yùn)動(dòng)捕捉系統(tǒng)獲得的相機(jī)真實(shí)位姿，并提供了用于評(píng)估所得位姿的絕對(duì)軌跡誤差(ATE)和相對(duì)位姿誤差(RPE)的工具.分別取v為1～10十個(gè)整數(shù)在fr1，fr2系列數(shù)據(jù)集上進(jìn)行實(shí)驗(yàn)，計(jì)算視覺(jué)里程計(jì)所得位姿漂移量的均方根誤差(root mean square error,RMSE)，單位是m·s-1，該值越小表明估計(jì)的位姿越準(zhǔn)確，具體如表1所示.表1第一列為fr1，fr2系列數(shù)據(jù)集，第2至11列為不同v值下位姿漂移量的RMSE.

表1 不同v值下視覺(jué)里程計(jì)算法所得相機(jī)位姿漂移量的RMSETab.1 RMSE of camera pose drift obtained by VO under different vvalues

注：黑體部分表示性能指標(biāo)的優(yōu)勢(shì).

(17)

(18)

1.4 基于中值法的幀權(quán)重λ

由式(13)得到矩陣H：

(19)

從式(19)中可以看出，λ值起著平衡光度誤差和深度誤差的作用，該值對(duì)相機(jī)位姿估計(jì)的準(zhǔn)確性有重要的影響.當(dāng)環(huán)境的紋理較多時(shí)，應(yīng)該增加光度誤差所占的權(quán)重；當(dāng)環(huán)境的紋理較少時(shí)，應(yīng)增加深度誤差所占的權(quán)重.通過(guò)中值動(dòng)態(tài)地獲取λ，即將每一幀光度圖像的光度中值和深度圖像的深度中值的比值作為λ的值，即

(20)

式中：median表示取中值.從每幀光度圖像的光度直方圖得到光度中值，范圍是[0～255]，從深度圖像得到深度數(shù)據(jù)值并等比到[0～255]的值域，并構(gòu)建深度直方圖得到深度中值.實(shí)驗(yàn)表明該方法得到的λ值與人工選擇的最佳λ值最為接近.

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

圖2 不同紋理環(huán)境示例Fig.2 Example images of the datasets with varying texture

為驗(yàn)證本算法的有效性，采用10個(gè)TUM標(biāo)準(zhǔn)數(shù)據(jù)集進(jìn)行實(shí)驗(yàn)，計(jì)算機(jī)CPU為Intel core i7八核3.40 GHz，內(nèi)存8 GB，運(yùn)行Ubuntu 16.04操作系統(tǒng).

2.1 標(biāo)準(zhǔn)數(shù)據(jù)集實(shí)驗(yàn)

采用fr1，fr2，和fr3系列RGB-D數(shù)據(jù)集進(jìn)行建圖實(shí)驗(yàn)，如圖2所示.各個(gè)數(shù)據(jù)集的結(jié)構(gòu)、紋理狀況不相同，圖2(a)是紋理豐富的環(huán)境，圖2(b)是紋理較少的環(huán)境，比如墻壁、地面等地方.

MVO SLAM在fr2/xyz數(shù)據(jù)集上構(gòu)建的三維地圖如圖3所示，該數(shù)據(jù)集是對(duì)一個(gè)桌面的重構(gòu).由圖3可見(jiàn)，MVO SLAM較好地構(gòu)建了桌面的三維稠密點(diǎn)云地圖，并且能夠還原較小的物件如鼠標(biāo)，可樂(lè)瓶等.MVO SLAM在fr2/xyz數(shù)據(jù)集下形成的軌跡和真實(shí)軌跡的比較如圖4所示.從圖4可見(jiàn)，MVO SLAM較準(zhǔn)確地估計(jì)了相機(jī)的運(yùn)動(dòng)軌跡，尤其是在相機(jī)旋轉(zhuǎn)較小的地方軌跡擬合的更好，但是當(dāng)相機(jī)旋轉(zhuǎn)較大時(shí)，仍會(huì)存在相對(duì)較大的誤差.

圖3 fr2/xyz稠密點(diǎn)云圖Fig.3 Dense point cloud of fr2/xyz

圖4 fr2/xyz估計(jì)軌跡與真實(shí)軌跡對(duì)比Fig.4 Trajectory of MVO plotted against ground truth for the fr2/xyz

MVO SLAM與DVO SLAM兩種方法建圖性能指標(biāo)對(duì)比如表2所示.性能指標(biāo)有2個(gè)：1)相對(duì)位姿誤差(RPE，[m/s])的均方根，用來(lái)評(píng)估在一定的時(shí)間間隔內(nèi)估計(jì)位姿的局部精度，該值越小表示漂移越少；2)絕對(duì)路徑誤差(ATE，[m])的均方根，該值是估計(jì)的位姿與真實(shí)位姿之間的誤差，用于評(píng)估整個(gè)SLAM系統(tǒng)的精度，該值越小表示估計(jì)的位姿與真實(shí)位姿越接近.

表2 DVO SLAM與MVO SLAM的性能指標(biāo)ATE和RPETab.2 ATE and RPE between DVO SLAM and MVO SLAM

注：黑體部分表示性能指標(biāo)的優(yōu)勢(shì).

表2中有10個(gè)標(biāo)準(zhǔn)數(shù)據(jù)集，其中fr1/360，fr1/desk，fr1/desk2，fr1/room，fr3/office等五個(gè)數(shù)據(jù)集含有豐富的紋理；fr1/floor，fr2/flowerbouquet，fr2/brownbackground，fr2/desk，fr2/rpy等五個(gè)數(shù)據(jù)集紋理較少，結(jié)構(gòu)較豐富.從表2可見(jiàn)，MVO SLAM在紋理較豐富的環(huán)境下如(fr1/room等五個(gè)數(shù)據(jù)集)所估計(jì)的位姿比DVO SLAM稍差，但劣化程度不多，基本上能夠達(dá)到DVO SLAM算法的精度，可以滿足導(dǎo)航和路徑規(guī)劃的要求；而對(duì)紋理較少的環(huán)境(如fr1/floor等五個(gè)數(shù)據(jù)集)有更高的精度，五個(gè)數(shù)據(jù)集的絕對(duì)路徑誤差(ATE)相較DVO SLAM平均降低了31.6%，相對(duì)位姿誤差(RPE)平均降低了19.4%.

MVO SLAM算法與一些先進(jìn)算法在不同數(shù)據(jù)集下ATE的比較如表3所示.MRSMap[12]是一種基于面元的SLAM算法，該算法根據(jù)全局地圖結(jié)構(gòu)進(jìn)行優(yōu)化，RGB-D SLAMV2[13]是一種基于特征點(diǎn)的的SLAM算法，Kintinuous[14]和ElasticFusion[15]是基于GPU的稠密SLAM算法.從表3中可見(jiàn)，本算法在多數(shù)數(shù)據(jù)集上的ATE相比其他算法要低，即精度更高，說(shuō)明算法具有更高的性能.

表3 MVO SLAM與先進(jìn)算法在ATE上的比較Tab.3 Comparison of MVO with respect to the existing state-of-the-art SLAM approaches in ATE

注：黑體部分表示性能指標(biāo)的優(yōu)勢(shì).

2.2 實(shí)際建圖實(shí)驗(yàn)

為進(jìn)一步驗(yàn)證本算法的有效性，在真實(shí)室內(nèi)環(huán)境下進(jìn)行建圖實(shí)驗(yàn).實(shí)驗(yàn)采用搭載Xtion2深度攝像頭的小車，小車實(shí)驗(yàn)平臺(tái)如圖5所示，控制小車在實(shí)驗(yàn)室中運(yùn)動(dòng).

圖5 小車實(shí)驗(yàn)平臺(tái)Fig.5 Experiment platform

采用本方法構(gòu)建的實(shí)驗(yàn)室地點(diǎn)云地圖如圖6所示.從圖6可見(jiàn)，本方法對(duì)實(shí)際環(huán)境建圖仍然有較好的效果，構(gòu)建的三維稠密點(diǎn)云地圖與實(shí)際環(huán)境相比并無(wú)太大偏差，并且環(huán)境的邊緣拼接較好，未出現(xiàn)變形、重疊的情況，對(duì)于電腦主機(jī)等物件也能較好地構(gòu)建出來(lái).在精度方面，由于沒(méi)有高精度的運(yùn)動(dòng)捕捉裝置，因此無(wú)法得到確切的精度，但是從構(gòu)建的點(diǎn)云圖可見(jiàn)，本算法完全可以滿足后續(xù)導(dǎo)航或路徑規(guī)劃等要求.

圖6 構(gòu)建的實(shí)驗(yàn)室稠密點(diǎn)云圖Fig.6 Constructed dense point cloud map of laboratory

3 結(jié)語(yǔ)

提出一種融合光度和深度的視覺(jué)RGB-D SLAM方法.為平衡兩誤差權(quán)重，加入幀權(quán)重系數(shù)λ構(gòu)建目標(biāo)函數(shù)，在視覺(jué)里程計(jì)部分同時(shí)最小化光度誤差和深度誤差.通過(guò)t分布模型得到每個(gè)像素點(diǎn)的光度和深度誤差權(quán)重，并采用中值法得到幀權(quán)重λ，最終得到相機(jī)位姿.通過(guò)TUM標(biāo)準(zhǔn)數(shù)據(jù)集實(shí)驗(yàn)和實(shí)際實(shí)驗(yàn)驗(yàn)證MVO SLAM算法能得到較準(zhǔn)確的相機(jī)位姿，尤其對(duì)紋理較少的環(huán)境比經(jīng)典的DVO SLAM有更好的精度.