融合光度和深度的視覺里程計改進算法

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

(福州大學電氣工程與自動化學院,福建 福州 350108)

0 引言

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

1 基于光度和深度的VO

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

(1)

(2)

式中：π(P)為相機坐標變換到齊次像素坐標的函數.

1.1 相機位姿變換函數

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

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

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

(3)

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

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

(4)

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

T=exp(ξ＾)

(5)

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

P′=TP

(6)

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

(7)

1.2 融合光度和深度的位姿目標函數

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

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

(8)

式中：I為像素點的光度值.也可得到像素點x經變換后的深度誤差rZ為

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

(9)

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

(10)

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

(11)

對每個像素的光度誤差和深度分別加上權重WI,i和WZ,i，最終構建目標函數為

(12)

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

JTWJΔξ=-JTWr

(13)

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

(14)

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

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

(15)

當迭代次數大于100次，或者Δξ小于510-7時，迭代停止，將此時的ξ(k)作為最終估計的相機位姿ξ.

1.3 基于t分布的像素點光度和深度誤差權重

采用自由度為v，方差為σ2的t分布誤差模型來計算第i個像素點相應的WI,i和WZ,i.以光度誤差為例，通過t分布模型推導出每個像素點的權重.設第i個像素光度誤差為rI,i，光度誤差權重為WI,i，每個像素點的權重為：

(16)

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

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

注：黑體部分表示性能指標的優勢.

(17)

(18)

1.4 基于中值法的幀權重λ

由式(13)得到矩陣H：

(19)

從式(19)中可以看出，λ值起著平衡光度誤差和深度誤差的作用，該值對相機位姿估計的準確性有重要的影響.當環境的紋理較多時，應該增加光度誤差所占的權重；當環境的紋理較少時，應增加深度誤差所占的權重.通過中值動態地獲取λ，即將每一幀光度圖像的光度中值和深度圖像的深度中值的比值作為λ的值，即

(20)

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

2 實驗結果與分析

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

為驗證本算法的有效性，采用10個TUM標準數據集進行實驗，計算機CPU為Intel core i7八核3.40 GHz，內存8 GB，運行Ubuntu 16.04操作系統.

2.1 標準數據集實驗

采用fr1，fr2，和fr3系列RGB-D數據集進行建圖實驗，如圖2所示.各個數據集的結構、紋理狀況不相同，圖2(a)是紋理豐富的環境，圖2(b)是紋理較少的環境，比如墻壁、地面等地方.

MVO SLAM在fr2/xyz數據集上構建的三維地圖如圖3所示，該數據集是對一個桌面的重構.由圖3可見，MVO SLAM較好地構建了桌面的三維稠密點云地圖，并且能夠還原較小的物件如鼠標，可樂瓶等.MVO SLAM在fr2/xyz數據集下形成的軌跡和真實軌跡的比較如圖4所示.從圖4可見，MVO SLAM較準確地估計了相機的運動軌跡，尤其是在相機旋轉較小的地方軌跡擬合的更好，但是當相機旋轉較大時，仍會存在相對較大的誤差.

圖3 fr2/xyz稠密點云圖Fig.3 Dense point cloud of fr2/xyz

圖4 fr2/xyz估計軌跡與真實軌跡對比Fig.4 Trajectory of MVO plotted against ground truth for the fr2/xyz

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

表2 DVO SLAM與MVO SLAM的性能指標ATE和RPETab.2 ATE and RPE between DVO SLAM and MVO SLAM

注：黑體部分表示性能指標的優勢.

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

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

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

注：黑體部分表示性能指標的優勢.

2.2 實際建圖實驗

為進一步驗證本算法的有效性，在真實室內環境下進行建圖實驗.實驗采用搭載Xtion2深度攝像頭的小車，小車實驗平臺如圖5所示，控制小車在實驗室中運動.

圖5 小車實驗平臺Fig.5 Experiment platform

采用本方法構建的實驗室地點云地圖如圖6所示.從圖6可見，本方法對實際環境建圖仍然有較好的效果，構建的三維稠密點云地圖與實際環境相比并無太大偏差，并且環境的邊緣拼接較好，未出現變形、重疊的情況，對于電腦主機等物件也能較好地構建出來.在精度方面，由于沒有高精度的運動捕捉裝置，因此無法得到確切的精度，但是從構建的點云圖可見，本算法完全可以滿足后續導航或路徑規劃等要求.

圖6 構建的實驗室稠密點云圖Fig.6 Constructed dense point cloud map of laboratory

3 結語

提出一種融合光度和深度的視覺RGB-D SLAM方法.為平衡兩誤差權重，加入幀權重系數λ構建目標函數，在視覺里程計部分同時最小化光度誤差和深度誤差.通過t分布模型得到每個像素點的光度和深度誤差權重，并采用中值法得到幀權重λ，最終得到相機位姿.通過TUM標準數據集實驗和實際實驗驗證MVO SLAM算法能得到較準確的相機位姿，尤其對紋理較少的環境比經典的DVO SLAM有更好的精度.