一種基于點面特征的低漂移視覺里程計

金 犇，貝紹軼，尹明鋒，李凌巖

(江蘇理工學院， 江蘇 常州 213000)

0 引言

視覺里程計(visual odometry，VO)旨在根據周圍環境信息估計出相機在未知環境中的運動位姿，被廣泛運用于無人駕駛、移動機器人、虛擬現實(VR)、增強現實(AR)等領域。

基于特征的視覺里程計主要跟蹤連續幀之間的特征，并最小化重投影誤差來估計相機的運動。在長走廊、白墻等低紋理環境下，PTAM、ORB-SLAM2等[1]基于點特征的開源視覺里程計算法，無法有效跟蹤特征點，導致算法精度和魯棒性很差。針對這一問題，邊緣、線、面等具有更強魯棒性的高維特征被廣泛使用。Gomez-Ojeda等[2]在SVO[3]框架上提出基于雙目相機的視覺里程計PL-SVO，其思路是檢測圖像中的LSD線特征并進行匹配，基于光度不變假設最小化各點與重投影點之間的光度誤差，計算相機位姿。孫沁璇等[4]提出了一種基于平面直線特征的視覺里程計，充分考慮平面與平面、平面與直線之間的幾何關系，以及特征之間的關聯。Kim等[5]提出了一種利用室內場景結構的無旋轉漂移的視覺里程計OPVO，通過計算環境的曼哈頓主方向來確定相機的絕對無漂移方向，通過深度信息最小化跟蹤點的去旋轉重投影誤差，找到最優平移。LPVO[6-8]在OPVO的基礎上聯合RGB圖中的線特征和深度圖中的表面法向量來穩定識別環境的正交結構，防止未識別到兩個及以上的平面造成跟蹤丟失的情況。Li等[9-10]提出了一種融合點線面特征的PlanarSLAM，考慮了曼哈頓主方向丟失的情況下，利用點線面聯合誤差函數計算旋轉和平移，增強了在弱結構環境下的魯棒性。

綜合分析上述算法，根據曼哈頓世界(Manhattan world，MW)假說[11]計算的是一種低漂移旋轉矩陣。進一步考慮對MW假說下低漂移旋轉矩陣的利用，提出一種基于點面特征的低漂移視覺里程計方法。該方法在ORB-SLAM2框架基礎上，將通過MW假說計算的旋轉矩陣與點面特征聯合重投影誤差函數計算的變換矩陣進行關聯，在提高精度的基礎上消除累計誤差，提高系統的魯棒性。

1 算法框架

基于點面特征的低漂移視覺里程計的工作流程如圖1所示，分為3個主要模塊，特征匹配模塊、曼哈頓主方向計算模塊、相機位姿估計模塊。

圖1 低漂移視覺里程計工作流程框圖

在特征匹配模塊中，根據當前幀的RGB圖像提取并篩選ORB特征點，構成點特征集；根據時間戳上對齊的深度圖計算點云，提取平面特征，構成平面特征集。在曼哈頓主方向計算模塊中，將根據點云計算得到的表面法向量，投影到上一幀的曼哈頓主方向上，計算當前幀的曼哈頓主方向。在相機位姿估計模塊中，根據提取的點特征集和平面特征集構建聯合重投影誤差函數，利用高斯牛頓法進行迭代優化，求出當前相機的位姿。分別計算前后兩幀的曼哈頓主方向，通過相機坐標系與曼哈頓主方向之間的變換計算得到低漂移旋轉矩陣。將聯合重投影誤差函數計算的6DoF相機位姿中的旋轉矩陣和低漂移旋轉矩陣進行關聯，得到當前新的6DoF相機位姿。

2 位姿計算

2.1 點特征提取

視覺里程計中點特征采用ORB[12]特征(圖2)，ORB特征由改進的Oriented FAST角點和BRIEF描述子組成，點特征的二維坐標用pi=(ui,vi)表示，三維坐標用Pi=(Xi,Yi,Zi)表示。

圖2 ORB特征點示意圖

2.2 平面特征提取

平面使用AHC方法[13]提取(圖3)。相比較RANSAC、基于法向信息的區域生長平面分割等平面提取算法，AHC方法精度高且能滿足實時性需求，在640×480分辨率的情況下可達到35幀。將提取出的平面參數用π=(nπ,d)表示，其中nπ=(nx,ny,nz)表示平面的法向量，d是相機坐標原點到平面的垂直距離。

圖3 AHC平面提取示意圖

2.3 曼哈頓主方向計算

MW假說假定城市場景中具有3個相互正交的主方向形成的坐標系，建筑平面的法向量與坐標軸的其中一個主方向保持平行(見圖4)。對于服從MW假說的結構，表面法向量ni在單位球S2(S2表示三維歐氏空間R3中的單位球面)上是一個有組織的分布，可以用于識別曼哈頓主方向。可以合理地假設表面法向量ni是一個概率密度函數的樣本，它們更有可能分布在MW的主軸上。因此，尋找主軸的過程等價于在這個概率密度分布函數中尋找局部極大值。

圖4 曼哈頓主方向

表面法向量ni[14]通過計算2個與點Pi所在局部曲面相切的向量以及計算兩個切向量的叉積得到點Pi的法向量，由于深度數據存在噪聲以及易丟失等問題，用積分圖像的方法對切向量進行平滑。用提取的法向量計算曼哈頓主方向[15]，具體算法流程如算法1所示。

算法1曼哈頓主方向計算

輸入：表面法向量ni

輸出：旋轉矩陣Rci,m

1. for inter=1:NUM

2. fori=1∶3

10.Rci,m=UVT

11. end for

12.end for

(1)

其中:Rw,c1是相機與世界坐標系之間的旋轉矩陣，其初始值為單位矩陣；Rc1,m在初始化時計算得到：

(2)

相機的旋轉矩陣Rci,c(i+1)由前后兩關鍵幀的相機與曼哈頓主方向的旋轉計算得到：

(3)

2.4 點面特征聯合誤差函數計算位姿

在一些特殊的環境中(如圖5所示)，初始幀為單一平面，無法計算曼哈頓主方向，利用上文中提取的ORB特征和平面特征構建聯合重投影誤差函數。

圖5 TUM RGB-D數據集 Fr3-n-t-f和Fr3-n-nf

將特征點的三維坐標Pj按照當前估計的位姿投影得到的位置與該點的像素坐標pk做差，得到如下的重投影誤差：

(4)

其中δ是投影函數。

平面進一步用q(π)=(φ,ψ,d) 表示，其中φ=arctan(ny/nx)，ψ=arcsin(nz)。平面之間的相似度匹配先比較兩平面的法向量的夾角，是否低于閾值9°；再比較坐標原點到平面的距離是否小于閾值8 cm；如果兩者皆滿足則判定2個平面相似。

觀測平面πk和對應的平面πj構建如下的平面特征誤差函數：

(5)

根據式(4)(5)構建如下的點面特征聯合重投影誤差函數：

(6)

其中:Λpk, j、Λπk, j是點、平面的協方差矩陣，ρp、ρπ是魯棒核函數，最終用高斯牛頓法迭代優化出當前相機位姿[R,t]。

在位姿估計中，充分考慮了特征之間的關聯，對于上文用平面特征計算得到的曼哈頓世界主方向進行充分利用。

當前幀未檢測到場景的曼哈頓主方向，則用式(6)計算得到的[R,t]作為變換矩陣。

若當前幀中檢測到曼哈頓主方向，則根據檢測到的正交平面的數量進行計算。通過計算相機與曼哈頓主方向之間的旋轉來計算相機前后之間的旋轉，是一種低累計漂移誤差的旋轉矩陣計算方式。利用這種計算方式可以有效地消除累計的漂移誤差。對最終的旋轉矩陣Rnew進行計算，具體計算公式如下：

(7)

其中:nπ為正交平面的個數，nt為每十幀中檢測到的曼哈頓主方向的幀數。

3 實驗以及算法評估

為了驗證本文算法的有效性，在合成數據集ICL-NUIM[16]和真實世界數據集TUM RGB-D[17]上進行算法精度測試。

為了評價算法估計軌跡的效果，用相對位姿誤差(relative pose error，RPE)[18]的均方根誤差(root-mean-squared error，RMSE)值來量化算法的精度，相比較絕對軌跡誤差(absolute trajectory error，ATE)，RPE更適合用來評估視覺里程計算法的漂移程度。

(8)

運行環境的電腦具體配置如下：CPU為Intel(R) Core(TM) i7-9750H CPU、主頻2.60 GHz、內存8 GB、顯卡為NAIDIA GeForce GTX1650、系統版本為Ubuntu 16.04 LTS 64位+ROS Kinetic。

3.1 ICL-NUIM數據集定位結果與分析

ICL-NUIM數據集是由客廳和辦公室序列組成，其中合成的RGB和深度圖像是在虛擬環境中以30 Hz頻率捕獲，通常用于評估三維重構以及SLAM算法。為了進行算法的性能評估，將本文算法與ORB-SLAM2、OPVO算法在合成數據集ICL-NUIM上進行測試。

如圖6所示，本文算法軌跡與數據集真實軌跡基本相符。表1中RPE的RMSE值和最大值(MAX)數據表明，相較于ORB-SLAM2、OPVO算法，本文算法在大部分序列中的軌跡與真實軌跡的RPE更小。這是因為ICL-NUIM數據集圖片中多為白色沙發、墻面、木桌等缺乏紋理物體，ORB-SLAM2算法無法提取足夠的特征點進行跟蹤，使得RPE的RMSE值較大；而OPVO利用平面的正交性解算旋轉和平移，沒有關鍵幀選擇以及閉環檢測機制來降低累計漂移誤差，導致精度較低。與ORB-SLAM2和OPVO算法相比，本文算法在弱紋理的環境中能夠有效利用環境正交平面等結構信息對相機位姿進行不斷修正，在弱紋理的結構環境中精度更高。

表1 各算法在ICL-NUIM數據集上運行軌跡的RPE數據 m·s-1

圖6 本文算法在ICL-NUIM數據集上估計的相機軌跡

具體運行數據如表2所示，其中加粗數字為評測算法在該數據集中的最優數據，-表示該算法在當前數據集無法運行。

表2 各算法在TUM RGB-D數據集上運行軌跡的RPE數據 m·s-1

3.2 TUM RGB-D數據集定位結果與分析

TUM RGB-D數據集是慕尼黑工業大學著名VO/V-SLAM評估數據集，包含了在各種室內環境中用微軟Kinect RGB-D相機采集的RGB-D圖像。本文算法在TUM RGB-D數據集上與ORB-SLAM2、OPVO、LPVO等算法做對比分析。

從表2RPE的RMSE數據和圖7的直方圖可以明顯看出在本文算法在大多數序列下有著更高的精度。

圖7 各算法在TUM RGB-D數據集上的RMSE值直方圖

在Fr3-s-t-far和Fr3-s-t-near這2個具有很強結構性的序列中，相比于ORB-SLAM2算法，本文算法的精度提高了8.47%和10.59%，而在Fr1-desk、Fr1-room、Fr3-n-n-far等序列下分別提高了7.61%、7.60%、3.57%，提高較多。在結構環境中曼哈頓主方向計算得到的相機旋轉矩陣比較可靠。OPVO和LPVO算法在Fr1-room和Fr3-n-n-far序列下，因為序列第一幀為單一平面，無法完成初始化，程序無法運行。LPVO算法在OPVO算法的基礎上加入了線特征來穩定識別環境中的正交結構，在大多數序列下精度要高于OPVO算法。

由圖8(a)中軌跡可知，ORB-SLAM2算法在軌跡的最下方以及右上方的轉角處發生了較大偏移，而本文算法軌跡在轉角處較真實軌跡無較大偏差，說明在強旋轉的情況下，本文算法的精度和魯棒性更優秀。據圖中整體軌跡分析，本文算法的軌跡明顯更為平滑，波動更小。在圖8(b)中，本文算法以及ORB-SLAM2算法在相機運動軌跡較為平緩的序列下，估計軌跡與真實軌跡十分接近。

圖8 本文算法以及ORB-SLAM2算法在TUM RGB-D數據集上估計的相機軌跡

由圖9可以明顯看出，在Fr1-room序列，ORB-SLAM2算法總體的RPE偏差值比本文算法高，與表2中的數據相符合。其中ORB-SLAM2算法在第598幀出現了最大RPE偏差值0.135 7。598幀所對應的圖片中背景為白色桌面，圖片中物體都較為模糊，缺乏紋理提取點特征進行跟蹤。

圖9 各算法在Fr1-room序列上的RPE偏差值曲線

由圖10可以看出，在x軸上600～800幀，由于當前序列圖片環境中的弱結構性，本文算法與真實軌跡存在較大偏差；而在z軸以及歐拉角上的偏差，因為對旋轉矩陣進行了自適應加權，由MW假說計算的幀間旋轉有效地降低了由聯合誤差函數計算位姿的累計漂移誤差，兩者的加權降低了出現較大偏差的可能性，進一步增強了在弱紋理環境下的魯棒性。相較于ORB-SLAM2算法，本文算法的軌跡波動更小。

圖10 各算法在Fr1-room序列上的參數曲線

4 結論

針對傳統視覺里程計在室內弱紋理環境中定位精度差等問題，提出一種基于點面特征的低漂移視覺里程計。充分利用點面特征以及環境結構信息，設計自適應權重對旋轉矩陣進行優化，并在合成數據集ICL-NUIM和真實數據集TUM RGB-D上分別進行測試，從相機軌跡以及6DoF上偏差等多重角度進行分析，結果表明本文算法因加入曼哈頓主方向計算的旋轉矩陣，而在弱紋理環、結構化環境中具有更高的定位精度。