基于ORB特征的改進RGB-D視覺里程計

劉慧杰，陳 強

（上海工程技術大學 電子電氣工程學院，上海 201620）

0 引言

基于視覺的同時定位與建圖（Visual Simultaneous Localization and Mapping，VSLAM），是一種采用視覺傳感器實現(xiàn)自主移動機器人定位和構建地圖的技術，其中定位的精確性和實時性至關重要。視覺里程計（Visual Odometry，VO）[1]作為VSLAM的重要組成部分，利用單個或多個相機采集的圖像信息估計智能體的運動過程，已成功運用于自主移動機器人的導航定位中。

視覺里程計主要包括單目里程計、雙目里程計和RGB-D里程計。自從微軟發(fā)布第一款RGB-D相機Kinect以來，因其價格低廉、獲取信息豐富、直接獲取像素深度等優(yōu)點，深受研究者的青睞。RGB-D里程計的實現(xiàn)方法有兩種：一種是基于特征點的間接法，另一種是基于像素的直接法。基于特征點法的RGB-D里程計以其直觀的框架、較低的計算成本和運行穩(wěn)定的優(yōu)點，廣泛應用于機器人實時導航和三維重建中[2，3]。

學者們對基于特征點法的視覺里程計作了大量研究。Peter Henry[4]等使用SIFT方法提取特征點，使用隨機采樣一致性（Random Sample Consensus，RANSAC）方法對3D-3D匹配點對進行配準，通過ICP[5]方法求解相機的運動。不足之處是SIFT特征計算量較大，影響了系統(tǒng)的實時性。仇翔等人[6]提出PROSAC算法迭代估計機器人位姿，綜合考慮深度信息和特征匹配質量。但是其提取的ORB特征點分布密集，導致圖像局部特征信息丟失以及過多的誤匹配，影響位姿估計的精度。張兆博等人[7]提出基于圖優(yōu)化的視覺里程計算法，結合關鍵幀初始位姿和位姿變換構建圖模型，采用非線性最小二乘優(yōu)化初始位姿。但是其估計模型單一致使位姿估計誤差較大。

針對以上問題，本文提出一種基于ORB特征的改進RGB-D視覺里程計。由改進的四叉樹均勻算法和RANSAC完成ORB特征的提取和匹配，然后對深度圖進行處理，求解PnP完成3D-2D特征點的初始位姿估計，用三角測量更新深度缺失點的深度信息，對更新的3D-3D點構建最小二乘問題，采用光束平差法建立BA優(yōu)化模型，使用g2o完成機器人的位姿優(yōu)化。通過對比實驗結果證明該方法在保證系統(tǒng)實時性的同時有效提高了視覺里程計的精確性。

1 RGB-D視覺里程計系統(tǒng)框架

本文RGB-D視覺里程計系統(tǒng)包含特征提取與匹配、位姿估計、BA優(yōu)化三個部分，系統(tǒng)框圖如圖1所示。RGB-D視覺里程計實現(xiàn)步驟如下：

圖1 RGB-D視覺里程計系統(tǒng)框圖

1）由Kinect相機采集環(huán)境的彩色RGB圖像和深度圖，將其按照時間戳對齊。采用基于四叉樹的ORB均勻算法和快速近似最近鄰（Fast Library for Approximate Nearest Neighbors，F(xiàn)LANN）算法進行特征提取和匹配。

2）根據(jù)已知匹配的特征點，建立3D-2D和3D-3D運動估計模型，采用PnP和ICP的融合算法完成機器人位姿估計。

3）利用光束平差法建立BA問題，構建優(yōu)化模型，使用g2o優(yōu)化庫求解得到位姿最優(yōu)估計。

2 RGB-D視覺里程計算法

針對ORB圖像特征，本文在得到由RANSAC剔除誤匹配后的點對之后，提出基于3D-3D模型的ICP配準算法和結合三角測量的3D-2D模型PnP算法的融合優(yōu)化算法。

整體算法流程圖如圖2所示。輸入數(shù)據(jù)為Kinect深度相機采集的彩色RGB圖像和深度圖，利用時間戳將其對齊。由基于四叉樹的ORB均勻算法提取特征，采用FLANN算法和RANSAC算法完成特征匹配和內點篩選。對相鄰幀的深度圖進行分析，特征點深度數(shù)據(jù)d=0時，利用直接線性變換求解3D-2D對應點的PNP問題，得到機器人的粗略位姿，然后利用位姿信息和三角測量更新3D-2D對應點的3D相機坐標；若深度信息無缺失，保留3D-3D匹配對。對混合3D-3D匹配點對進行ICP優(yōu)化，利用光束平差法建立優(yōu)化模型，使用g2o優(yōu)化庫求解得到特征點及位姿最優(yōu)估計。

圖2 融合算法流程圖

2.1 ORB特征提取與匹配

計算機視覺領域的研究者設計了許多穩(wěn)定的局部圖像特征，先后提出了SIFT（Scale-Invariant Feature Transform）、SURF（Speed-up Robust Features）、ORB（Oriented FAST and Rotated BRIEF）[8]等特征點提取算法，ORB算法能夠滿足VO的精度和實時性要求。因此，本文選擇ORB算法進行特征提取和匹配。

對于場景特征分布均勻的圖像中提取的特征點分布均勻，有利于匹配。但在實驗中發(fā)現(xiàn)提取出的特征點呈現(xiàn)集群分布，不利于后續(xù)特征匹配。本文改進四叉樹均勻算法，在算法[9]的基礎上添加節(jié)點邊緣檢測，均勻提取特征點并減少特征點冗余。

初始化整個圖像為父節(jié)點O，將RGB圖像按面積四等分為四個區(qū)域，得到A，B，C，D子節(jié)點，構成四叉樹初始結構。基于四叉樹的ORB均勻算法步驟如下：

1）對RGB圖像構建n層高斯圖像金字塔，按照尺度因子設置每層提取的特征點數(shù)量。對每層圖像提取的“Oriented FAST”關鍵點做四叉樹劃分。第α層應分配的特征點數(shù)量為：

式(1)中s為尺度因子（0

2）在4個子節(jié)點中進行ORB特征點提取，檢測跨節(jié)點分布的冗余特征點，將其剔除。判斷子節(jié)點中的特征點數(shù)量，若子節(jié)點所含特征點個數(shù)大于1，再次對各子節(jié)點四分割并提取特征點。反之，停止四叉樹劃分。

3）重復步驟2）。直到各子節(jié)點所含特征點個數(shù)小于等于1或者達到期望提取的ORB特征點數(shù)量為止。

4）對四叉樹子節(jié)點中的提取的關鍵點計算Harris響應值，選擇該子節(jié)點中響應值最大的關鍵點為最終提取結果。

提取了圖像的ORB特征后，需要確定兩幀圖像的對應關系，實現(xiàn)特征點的匹配。隨著特征點的數(shù)量的增加，常用的暴力匹配法的計算開銷已經(jīng)不能滿足VO的實時性，本文采用快速近似最近鄰FLANN算法[10]實現(xiàn)特征匹配。但是經(jīng)過以上處理的匹配中存在大量的誤匹配，錯誤的匹配輸入會導致PnP和ICP算法優(yōu)化迭代陷入局部最優(yōu)或無法收斂，不能達到全局最優(yōu)值。首先對匹配對采取預處理，選出描述子之間漢明距離小于最小距離兩倍的匹配對，之后使用RANSAC算法剔除離群點，篩選出正確的匹配對。

2.2 運動估計與位姿優(yōu)化

視覺里程計實現(xiàn)相鄰兩幀圖像間的機器人位姿估計。一般有兩種情形，一種是求解3D到2D匹配點對的PnP優(yōu)化問題，另一種就是3D-3D匹配點對的ICP優(yōu)化問題。一旦求得3D點的相機坐標，即可將3D-2D點轉換為3D-3D對應點，采取ICP優(yōu)化。根據(jù)3D-2D和3D-3D運動模型，設計基于PnP和ICP的融合算法。

1）3D-2D運動估計

對于經(jīng)RANSAC篩選的匹配對，考慮N個深度信息缺失的3D-2D特征點對，此時PnP問題描述為已知N個3D空間點及其投影位置時求解相機的位姿。已知某個3D空間點的齊次坐標為P=(x，Y，Z，1)T，其對應的2D投影點在歸一化平面的齊次坐標為x=[u，v，1]T。相機內參矩陣K為標定好的已知量。定義增廣矩陣[Rlt]為3×4的矩陣，含有12個未知參數(shù)A=[a1...a12]T，表示旋轉與平移信息。

那么3D點到2D點的變換關系為：

對于N個3D-2D特征點，可得到以下方程組：

A是待求變量，其中A1=[a1...a12]T，A2=[a1...a12]T，A3=[a1...a12]T。每對3D-2D特征點提供了兩個關于A的線性約束，由于A有12維，因此至少通過6對匹配點才能實現(xiàn)矩陣A的線性求解。當匹配點對大于6時，對超定方程使用SVD方法求其最小二乘解，得到初始相機位姿R，t，李代數(shù)表達為ζ0。

通過直接線性變換求解PnP問題得到了初始相機位姿，利用三角測量計算特征點的深度，更新特征點的空間位置。考慮相鄰兩幀圖像為I1和I2，以I1為參考，兩幀圖像間的相機運動的以R21，t21表示。

在I1的相機坐標系下，設P的空間位置為P=[x，Y，Z]T，取x1和x2為特征點的歸一化相機坐標，根據(jù)相機投影模型，兩個匹配的特征點的像素坐標為：

特征點在兩幀的深度值分別為S1和S2，變換關系為：

式兩邊左乘x1的反對稱矩陣x1^，有：

其中R21t21為PnP求解的已知量，解式(6)右側方程式，直接得到s2的值，同時s1的值亦可求得，從而得到了兩幀下的點的深度，確定了匹配點在相機坐標系下的坐標。

2）3D-3D運動估計

對深度信息缺失的3D-2D特征點對，在PnP求解的結果上進行三角測量，得到了兩個匹配特征點的深度信息，更新了特征點的相機空間坐標。結合深度信息已知的3D-3D匹配點對，將問題構建為混合3D-3D點的相機位姿優(yōu)化模型。對于已知匹配的3D點集和其中變換后的點為存在一個歐式變換R，t滿足：

對于匹配已知的3D-3D混合運動估計進行ICP優(yōu)化，通過代數(shù)方法SVD或者非線性優(yōu)化求解混合運動估計問題。

3）位姿優(yōu)化

對于混合3D-3D點的相機位姿優(yōu)化模型，采用位姿的李代數(shù)表示方法來構建無約束的優(yōu)化問題，式(7)的李代數(shù)位姿表達為：

引入最小化誤差函數(shù)優(yōu)化機器人位姿，對上述問題構建BA模型：

其中e為3維向量，采用高斯牛頓法求解。算法流程如下：

Step1：初始值給定為PnP結果ζ0。

Step2：對每次迭代，求出當前的雅可比矩陣J與誤差e。

e為空間坐標的誤差（3維），相機位姿為6維，則J為一個3×6的矩陣，位姿的李代數(shù)定義為平移在前，旋轉在后。在求導上使用李代數(shù)的左乘擾動模型[11]，對ζ∧左乘擾動量δξ，單個誤差項關于擾動量相機位姿的導數(shù)為：

由M個3D-3D匹配點對組成的誤差函數(shù)e的雅可比矩陣為：

Step3：第k次迭代，依據(jù)下式計算增量 Δζk：

Step4：如果Δζk足夠小，就停止迭代。否則，更新返回Step2。

3 實驗與結果分析

3.1 實驗環(huán)境

為了驗證本文算法的有效性，使用公開的RGB-D數(shù)據(jù)集進行對比實驗，全部實驗在配備小型上網(wǎng)本的TurtleBot2移動機器人平臺上完成，如圖3所示。實驗平臺支持ROS操作系統(tǒng)，上網(wǎng)本的配置為：Intel Core i5-7260U@2.20GHz×4CPU，238GB硬盤，操作系統(tǒng)為64位的Ubuntu16.04LTS。

圖3 移動平臺

3.2 特征提取與匹配對比實驗

采用RGB-D數(shù)據(jù)集fr1/xyz中的相鄰兩幀圖像進行對比實驗，特征點分布對比如圖4所示。圖4(a)為原始ORB算法提取的特征點分布情況，可見ORB特征點分布密集，冗余特征點較多。圖4(b)為采用四叉樹均勻算法提取結果，提取的ORB特征點在整張圖中分布均勻，其描述子更具有代表性，提高了像素信息的利用率，有利于良好的點對匹配。

圖4 特征點分布對比

融合算法求解依賴正確的特征點匹配，誤匹配會嚴重影響機器人的位姿優(yōu)化。特征匹配處理對比實驗結果如圖5所示。如圖5(a)匹配特征點未經(jīng)RANSAC篩選，可見其存在大量的誤匹配，會導致迭代優(yōu)化算法無法收斂。圖5(b)為采用RANSAC算法處理特征匹配，基本消除了誤匹配，點對匹配正確率得到提高，有利于提高里程計的精確性。

圖5 特征匹配對比

3.3 實時性和定位誤差對比

本文使用RGB-D數(shù)據(jù)集fr1/xyz、fr1/rpy、fr1/desk進行離線實驗。為了驗證實時性，以每幀運行時間為參考標準，包括特征提取、匹配時間和迭代優(yōu)化時間，三種方法都采用本文四叉樹均勻算法和FLANN算法進行ORB提取與匹配，只是位姿優(yōu)化算法不同。定位誤差采用均方根誤差（Root Mean Square Error，RMSE）作為評價標準。用RMSE表示真實軌跡和估計軌跡之間的偏差，以此來判斷算法的精度。將本文算法與原始ICP算法、RANSAC+ICP算法在不同數(shù)據(jù)集下的RMSE和耗時進行對比，結果如表1所示。

表1 三種方法性能對比

由表1直觀地看出，本文算法在滿足里程計實時性的同時，其定位誤差優(yōu)于傳統(tǒng)ICP和RANSAC+ICP算法。實時性方面，三種方法的平均每幀處理時間均在30ms左右，里程計計算迅速。定位準確性方面，本文算法的平均RMSE為0.036m，相比于原始ICP的0.078m和RANSAC+ICP算法的0.057m有著更小的誤差，降幅分別為56.4%和40.4%。本文算法均勻提取ORB特征點，剔除了較多的誤匹配，同時將深度信息缺失的特征點加入到位姿估計中，提高了位姿估計的精度。

4 結語

本文提出一種融合PnP與ICP的RGB-D視覺里程計方法，有效針對了深度信息缺失的問題，通過改進四叉樹均勻算法實現(xiàn)ORB特征均勻分布，有利于良好的圖像配準，減小了誤匹配引起的位姿估計誤差。在求解機器人位姿時，將深度缺失的特征點加入到運動估計中，在基于3D-3D模型的ICP配準算法和結合三角測量的3D-2D模型PnP算法的融合優(yōu)化基礎上，采用光束平差法建立BA模型，利用g2o求解器優(yōu)化機器人位姿。實驗結果表明該方法在滿足系統(tǒng)實時性要求的同時有效提高了視覺里程計的精確性。