基于局內點分布的圖優化方法

翁秀玲,王云峰,吳　煒,郭東輝

(廈門大學信息科學與技術學院,福建 廈門 361005)

圖優化是即時定位與地圖構建(simultaneous localization and mapping,SLAM)[1]的后端．它利用序列圖像的閉環對前端視覺里程計獲得的相機軌跡進行優化,對消除位姿估計的累積誤差和構建一致性的地圖至關重要．

圖優化問題通常被歸為非線性最小二乘問題,采用高斯-牛頓法(Gauss-Newton,GN)或列文伯格-馬夸爾特法(Levenberg-Marquardt,LM)進行求解．其求解思路為在當前解處對目標函數進行如一階泰勒展開近似的線性化操作,從而得到線性最小二乘方程．求解線性系統只需令導函數為零,通過迭代直至達到最大迭代次數或收斂．在迭代求解的問題上,研究者發現SLAM問題的稀疏性可以被充分利用,從而提高求解效率．Dellaert等[2]通過稀疏喬列斯基分解對線性系統進行求解．Kaess等[3]對系統的信息矩陣作正交三角分解,并選擇性地對其進行增量式更新,從而避免了每次重新計算系統的信息矩陣,提高了求解效率．Konolige等[4]提出了一種根據給定圖的約束關系快速構造稀疏矩陣的方法．Kummerle等[5]公開了基于稀疏矩陣分解的通用圖優化庫G2O,得到了廣泛的應用．但迭代求解問題[2-5]對初始值較敏感,易落入局部極值;并且求解效率與稀疏矩陣的稀疏程度相關,最佳情況下時間復雜度為O(n),最壞情況下時間復雜度為O(n3),無法保證實時優化,不適合在線應用．

針對非線性最小二乘問題迭代求解存在的效率和初值敏感問題,Dubbelman等[6-7]提出COP-SLAM方法(closed-form online pose-chain SLAM,COP-SLAM),可以實現實時優化．該方法根據視覺里程計約束的信息矩陣設置位姿鏈中邊的權重,從而將單個閉環約束的誤差項分配到視覺里程計的總約束上，直接得到解析解．在保證位姿鏈優化精度的情況下,大大地縮短了優化所消耗的時間．該方法基于局內點數量設置視覺里程計約束的信息矩陣,但是當局內點分布不均勻即集中位于圖像的特定區域時,視覺里程計精度下降,故該信息矩陣不能很好地衡量視覺里程計的精度,進而影響優化精度．

為了進一步提高COP-SLAM方法的優化結果,本研究提出了基于局內點分布的后端圖優化方法．定義局內點集的面積表示局內點的分布,并通過局內點分布結合局內點數量設置位姿圖中約束邊的信息矩陣,提高了信息矩陣對視覺里程計精度的衡量準確度．實驗結果表明,該優化方法有效地降低了絕對軌跡誤差．

1　COP-SLAM[7]

COP-SLAM[7]屬于輕量型的圖優化算法,是根據單個閉環約束對環路內的位姿節點進行優化．算法采用位姿鏈模型,設相機的位姿用4×4的矩陣Ai表示,連續位姿Ai-1和Ai之間的視覺里程計約束用4×4的變換矩陣Mi表示,則n時刻的位姿可由式(1)表示．式中相機的初始位姿A0一般設為4×4的單位矩陣I4×4．

(1)

圖1　COP-SLAM的優化示意圖Fig.1 Optimization diagram of COP-SLAM

(2)

(3)

(4)

(5)

(6)

(7)

(8)

(9)

Mi←Mi×Ui.

(10)

(11)

2　基于局內點分布的圖優化方法

2.1　基于特征點分布的信息矩陣

視覺里程計的精度不僅受局內特征點數目影響,還受局內特征點分布影響,因此本研究根據局內特征點的分布和數目設置信息矩陣．通過局內特征點之間的面積來表示局內特征點的分布,在局內點數量的基礎上引入局內點集的面積,提高了對視覺里程計精度的衡量．

COP-SLAM一般根據圖像特征局內點數量N按式(12)對信息矩陣的σ2進行設置,當局內點對分布不均勻,即局內點集中分布于圖像的特定區域時,視覺里程計精度下降,該σ2構成的信息矩陣則不能很好地衡量視覺里程計的精確程度,影響優化效果．

σ2=1/N.

(12)

對于2幀圖像的視覺里程計結果的準確度可由平移誤差ε[8]衡量,

(13)

圖2　KITTI00匹配特征點Fig.2 Matched feature points of KITTI00

以KITTI數據集[10]為例,采用開源的雙目視覺里程計libviso2[11]對相機位姿進行估計．圖2為雙目數據集KITTI00中2對連續圖像,圖2(a)和(b)所示的圓點為連續對圖像根據斑點和角點算子檢測出來并經過匹配的特征點,匹配特征點集中分布在圖像中上方,通過隨機抽樣一致性算法 (random sample consensus,RANSAC)[12]進行位姿求解得到局內點數目為128,根據式(13)得到視覺里程計的平移誤差為0.037 m．圖2(c)和(d)為另一對分布更加均勻匹配特征點的連續圖像對,其局內點數目為125,根據式(13)得到視覺里程計的平移誤差為0.011 m．由此可見,盡管圖2(a)和(b)中圖像設置的σ2小,但里程計的準確度更差．故基于局內點數量得到的信息矩陣并不能很好地衡量視覺里程計的精確程度．

在局內點數量的基礎上,本研究考慮局內點占據的平均面積并對信息矩陣進行設置．雙目視覺里程計libviso2[11]首先通過斑點和角點算子對連續2幅圖像提取特征點并進行匹配,獲得當前圖像匹配點PMatch,再通過RANSAC[12]對PMatch進行位姿估計,得到局內點集合PIn,如下所示:

PIn={P1,P2,…，PN},

(14)

其中,Pi的坐標為(xi,yi)．

(15)

最后按式(16)設置信息矩陣唯一的參數σ2．

(16)

2.2　基于局內點分布的圖優化

基于局內點分布的圖優化算法通過閉環檢測獲得構成閉環的兩個位姿節點,采用視覺里程計得到2個位姿節點的變換矩陣,通過式(16)基于局內點的平均分布面積計算該變換矩陣對應的信息矩陣中的σ2．如果2個位姿節點為連續節點,則通過式(1)根據Mi計算最后一個位姿節點的絕對位姿;如果2個位姿節點為非連續節點,則根據Mm,n優化環路內位姿節點．

(17)

其中,R為3×3階的旋轉矩陣,t為3×1階的平移向量．

由式(7)得到的更新量Ui也可以表示成如式(18)所示的分塊矩陣．

(18)

其中,UR為3×3階的旋轉矩陣,Ut為3×1階的平移向量．

3　實驗結果與分析

圖像數據集KITTI[10]是雙目SLAM系統研究應用最多的圖像集,包含4組圖像數據集的詳細信息,如表1所示．在此圖像集合上通過雙目視覺里程計libviso2[11]和閉環檢測算法[14]實現了基于局內點分布的圖優化,最終輸出相機運動軌跡,如圖3所示．

表1　數據集的詳細信息

軌跡的度量標準是絕對軌跡誤差(absolute trajectory error,ATE)[15],其計算式如式(19)所示,是對估計軌跡與真實軌跡之間的絕對整體偏差進行度量．

(19)

其中,tOi表示真實位姿的平移向量,tei表示估計位姿的平移向量．

以圖2為例,以局內點數目作為信息矩陣的設置方法,優化后圖2(a)和(b)的絕對位姿的平移誤差分別為3.425和3.426 m,圖2(c)和(d)的絕對位姿的平移誤差分別為2.231和2.281 m;而以局內點數目結合分布設置信息矩陣,則得到圖2(a)和(b)的絕對位姿的平移誤差分別為3.268和3.272 m,圖2(c)和(d)的絕對位姿的平移誤差分別為1.977和2.026 m．因此,基于局內點分布的圖優化方法優化效果好于COP-SLAM．

視覺里程計得到的相機運動軌跡、相機的真實軌跡以及本研究優化后得到的相機運動軌跡如圖4所示．可以看出,基于局內點分布的圖優化得到的運動軌跡更加接近于真實軌跡,取得了優化效果．

基于局內點分布的SLAM后端圖優化算法所需時間如表2所示,在KITTI數據集上的平均優化時間約為G2O的1/10,與COP-SLAM相當,故仍屬于輕量型后端圖優化算法,可實現實時優化．

本研究所提出的圖優化算法與其他一些優化算法性能比較如表3所示,優化結果不如LSD-SLAM[16],原因在于非線性迭代優化具有更好的優化效果,但該優化運行速度僅為本研究的1/50[7]．在所有數據集上絕對軌跡誤差均比Frost[17]的小,因為Frost采用單目視覺里程計估計相機運動,具有尺度不確定性,而本研究采用雙目視覺里程計,無此問題．與同樣屬于輕量型優化算法的COP-SLAM[7]相比,由于提高了對視覺里程計精度的衡量,因此4組圖像的優化效果均有提升．

圖4　本研究優化結果Fig.4 Optimization result of this research

數據集時間/msG2OCOP-SLAM本研究KITTI0025030.431KITTI0229614.715KITTI0512514.715KITTI066214.715

表3　KITTI的絕對軌跡誤差

4　結　論

圖優化作為SLAM的重要環節,對減小移動相機的累積誤差,實現地圖的一致性至關重要．為了提高優化效果,本研究提出了基于局內點分布的后端圖優化方法．在計算信息矩陣時,通過局內特征點之間的面積來表示局內特征點的分布,在局內點數量的基礎上引入局內點集的面積,提高了信息矩陣對視覺里程計精確度的衡量度．在標準雙目圖像數據集上進行了實驗,結果表明,提出的優化方法有效地降低了絕對軌跡誤差．

參考文獻：

[1] CADENA C,CARLONE L,CARRILLO H,et al.Past,present,and future of simultaneous localization and mapping:towards the robust-perception age[J].IEEE Transactions on Robotics,2016,32(6):1309-1332.

[2]DELLAERT F,KAESS M.Square root SAM:simultaneous localization and mapping via square root information smoothing[J].International Journal of Robotics Research,2006,25(12):1181-1203.

[3]KAESS M,RANGANATHAN A,DELLAERT F.ISAM:incremental smoothing and mapping[J].IEEE Transactions on Robotics,2008,24(6):1365-1378.

[4]KONOLIGE K,GRISETTI G,KUMMERLE R,et al.Efficient sparse pose adjustment for 2D mapping[C]∥IEEE International Conference on Intelligent Robots and Systems.Taiwan:IEEE,2010:22-29.

[5]KUMMERLE R,GRISETTI G,STRASDAT H,et al.G2O:a general framework for graph optimization[C]∥IEEE International Conference on Robotics and Automation.Piscataway:IEEE,2011:3607-3613.

[6]DUBBELMAN G,BROWNING B.Closed-form online pose-chain SLAM[C]∥IEEE International Conference on Robotics and Automation.Karlsruhe:IEEE,2013:5190-5197.

[7]DUBBELMAN G,BROWNING B.COP-SLAM:closed-form online pose-chain optimization for visual SLAM[J].IEEE Transactions on Robotics,2015,31(5):1194-1213.

[8]SUNDERHAUF N.Robust optimization for simultaneous localization and mapping[D].Chemnitz:Chemnitz University of Technology,2012:13-193.

[9]THRUN S,LIU Y,KOLLER D,et al.Simultaneous localization and mapping with sparse extended information filters[J].The International Journal of Robotics Research,2004,23(7/8):693-716.

[10]GEIGER A,LENZ P,STILLER C,et al.Vision meets robotics:the KITTI dataset[J].International Journal of Robotics Research,2013,32(11):1231-1237.

[11]GEIGER A,ZIEGLER J,STILLER C.StereoScan:dense 3d reconstruction in real-time[C]∥IEEE Intelligent Vehicles Symposium.Baden-Baden:IEEE,2011:963-968.

[12]HAST A,NYSJ? J,MARCHETTI A.Optimal RANSAC：towards a repeatable algorithm for finding the optimal set[J].Journal of Wscg,2013,21(1):21-30.

[13]BARBER C B,DOBKIN D P,HUHDANPAA H.The quickhull algorithm for convex hulls[J].Acm Transactions on Mathematical Software,1998,22(4):469-483.

[14]LABBE M,MICHAUD F.Appearance-based loop closure detection for online large-scale and long-term operation[J].IEEE Transactions on Robotics,2013,29(3):734-745.

[15]STURM J,ENGELHARD N,ENDRES F,et al.A benchmark for the evaluation of RGB-D SLAM systems[C]∥IEEE International Conference on Intelligent Robots and Systems.Algarve:IEEE,2012:573-580.

[16]ENGEL J,STüCKLER J,CREMERS D.Large-scale direct SLAM with stereo cameras[C]∥IEEE International Conference on Intelligent Robots and Systems.Hamburg:IEEE,2015:1935-1942.