基于ORB特征的改進(jìn)RGB-D視覺里程計(jì)

劉慧杰，陳 強(qiáng)

（上海工程技術(shù)大學(xué) 電子電氣工程學(xué)院，上海 201620）

0 引言

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

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

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

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

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

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

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

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

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

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

2 RGB-D視覺里程計(jì)算法

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

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

圖2 融合算法流程圖

2.1 ORB特征提取與匹配

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

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

初始化整個(gè)圖像為父節(jié)點(diǎn)O，將RGB圖像按面積四等分為四個(gè)區(qū)域，得到A，B，C，D子節(jié)點(diǎn)，構(gòu)成四叉樹初始結(jié)構(gòu)?；谒牟鏄涞腛RB均勻算法步驟如下：

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

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

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

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

4）對(duì)四叉樹子節(jié)點(diǎn)中的提取的關(guān)鍵點(diǎn)計(jì)算Harris響應(yīng)值，選擇該子節(jié)點(diǎn)中響應(yīng)值最大的關(guān)鍵點(diǎn)為最終提取結(jié)果。

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

2.2 運(yùn)動(dòng)估計(jì)與位姿優(yōu)化

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

1）3D-2D運(yùn)動(dòng)估計(jì)

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

那么3D點(diǎn)到2D點(diǎn)的變換關(guān)系為：

對(duì)于N個(gè)3D-2D特征點(diǎn)，可得到以下方程組：

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

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

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

特征點(diǎn)在兩幀的深度值分別為S1和S2，變換關(guān)系為：

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

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

2）3D-3D運(yùn)動(dòng)估計(jì)

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

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

3）位姿優(yōu)化

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

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

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

Step1：初始值給定為PnP結(jié)果ζ0。

Step2：對(duì)每次迭代，求出當(dāng)前的雅可比矩陣J與誤差e。

e為空間坐標(biāo)的誤差（3維），相機(jī)位姿為6維，則J為一個(gè)3×6的矩陣，位姿的李代數(shù)定義為平移在前，旋轉(zhuǎn)在后。在求導(dǎo)上使用李代數(shù)的左乘擾動(dòng)模型[11]，對(duì)ζ∧左乘擾動(dòng)量δξ，單個(gè)誤差項(xiàng)關(guān)于擾動(dòng)量相機(jī)位姿的導(dǎo)數(shù)為：

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

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

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

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

3.1 實(shí)驗(yàn)環(huán)境

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

圖3 移動(dòng)平臺(tái)

3.2 特征提取與匹配對(duì)比實(shí)驗(yàn)

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

圖4 特征點(diǎn)分布對(duì)比

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

圖5 特征匹配對(duì)比

3.3 實(shí)時(shí)性和定位誤差對(duì)比

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

表1 三種方法性能對(duì)比

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

4 結(jié)語

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