融合點(diǎn)、面特征的RGB-D 視覺里程計(jì)?

范都耀 宋勇磊

（南京理工大學(xué)計(jì)算機(jī)科學(xué)與工程學(xué)院 南京 210094）

1 引言

在機(jī)器人智能化、自主化及三維建模等領(lǐng)域利用低成本的RGB-D 傳感器產(chǎn)生的序列景深和色彩圖像構(gòu)建精確的場景三維模型是基礎(chǔ)且關(guān)鍵的技術(shù)，也是當(dāng)前學(xué)者們研究的熱點(diǎn)。特別是在機(jī)器人領(lǐng)域，能否構(gòu)建場景的精確模型，直接影響到機(jī)器人能否在已創(chuàng)建的場景模型中實(shí)現(xiàn)精確定位，繼而影響到機(jī)器人的自主導(dǎo)航移動。應(yīng)用同時(shí)定位與地圖構(gòu)建（Simultaneous Localization and Mapping，SLAM）［1］技術(shù)構(gòu)建場景模型，是當(dāng)前學(xué)者廣泛使用與研究的一個(gè)方向。

基于RGB-D 的SLAM［2］技術(shù)經(jīng)過多年的發(fā)展，沉淀出了一系列的成熟算法，主要分為基于泊松曲面［3～4］的重建、基于SIFT、SURF 等特征的稀疏重建或半稠密重建算法和以KinectFusion［5］和StaticFusion［6］為代表的稠密重建算法?；诓此汕娴倪@類算法常用于離線下的點(diǎn)云數(shù)據(jù)，用向量表示物體的內(nèi)外表面方向，通過優(yōu)化指標(biāo)函數(shù)生成一個(gè)平滑的表面估計(jì)?；谙∈韬桶氤砻苤亟ㄋ惴ㄓ蠴RB-SLAM［7］在全局都使用ORB 特征是基于稀疏特征的代表性三維重建方法；SVO［8］將角點(diǎn)作為關(guān)鍵點(diǎn)，利用關(guān)鍵點(diǎn)鄰域內(nèi)的信息跟蹤相機(jī)位姿，并在后端BA優(yōu)化過程中使用了最小重投影誤差是經(jīng)典的半直接法重建方法；DSO［9］是直接法三維重建方法。

以上方法雖然無需GPU 加速即可達(dá)到實(shí)時(shí)性，但獲取的重建信息較少，缺失大量場景中的細(xì)節(jié)信息，很難準(zhǔn)確表達(dá)場景。2010 年后隨著以Kinect為代表的消費(fèi)級深度相機(jī)的出現(xiàn)，稠密三維重建方法得到廣泛發(fā)展。2011 年Newcombe 等提出了Kinect Fusio［5］方法，該方法是小范圍室內(nèi)場景實(shí)時(shí)稠密三維重建的開山之作，同時(shí)也極大促進(jìn)了RGB-D 稠密三維重建算法研究的發(fā)展，相繼出現(xiàn)了ElasticFusion［10］、Voxel Hashing［11］、BundleFusion［12］等一系列算法。KinectFusion［5］利用深度相機(jī)獲取深度圖像，采用隱式表面模型（TSDF）表示小范圍場景的三維模型，通過最近點(diǎn)迭代（ICP）算法將當(dāng)前深度信息對齊到TSDF［13］模型，從而獲取相機(jī)的位姿估計(jì)實(shí)現(xiàn)定位。為了加速運(yùn)算過程并實(shí)時(shí)顯示場景的三維模型，該方法首次利用GPU 加速運(yùn)算，在實(shí)現(xiàn)精細(xì)三維重建的同時(shí)兼顧了較好的實(shí)時(shí)性。但該方法依然存在許多缺陷：因其使用TSDF作為地圖表示方法，所以此方法只能在小范圍場景中應(yīng)用；KinectFusion［5］緊密依賴于ICP 算法，所以其在特征點(diǎn)較少的場景中不能很好地工作并且沒有回環(huán)檢測，所以在大范圍移動后其累積誤差會被放大，出現(xiàn)較為嚴(yán)重的飄移情況。

為解決上述問題，infiniTAM［14］使用了Voxel-Hashing 算法，將TSDF 數(shù)據(jù)融合到哈希表中，來減少TSDF模型的數(shù)據(jù)量。BundleFusion［12］使用八叉樹數(shù)據(jù)結(jié)構(gòu)來減少對內(nèi)存空間的使用。Kintinuous［15］提出了詞袋法（Distributed Bag of Words，DBoW）檢測閉環(huán)回路。ElasticFusion［10］舍棄了預(yù)設(shè)網(wǎng)格的場景表達(dá)方式，轉(zhuǎn)為使用基于面元（surfel）［16］的表達(dá)方式，使其能夠重建更大范圍的場景，不受TSDF的小場景限制，并且該算法除使用深度信息外也關(guān)聯(lián)了光度空間中信息，同時(shí)引入回環(huán)檢測。其在前端部分使用通過局部閉環(huán)與全局閉環(huán)的融合，利用隨機(jī)蕨算法實(shí)現(xiàn)了在較好的小范圍場景三維重建。但上述方法依然存在定位失效，局部重建效果差的問題。

針對定位丟失和模型存在空洞兩個(gè)問題，本文提出一種通過增加特征點(diǎn)和特征面輔助原始ICP算法提高魯棒性的SLAM 系統(tǒng)。利用RGB 圖像提取改進(jìn)ORB 特征來確定初始位姿變化，融合深度圖像提取到的特征平面［19］作為幾何約束，實(shí)現(xiàn)三維場景的重建，通過公共數(shù)據(jù)集我們?nèi)〉昧烁玫男Ч?/p>

2 特征點(diǎn)與特征平面檢測

2.1 改進(jìn)的ORB特征點(diǎn)檢測

ORB 特征提取由oFAST 特征點(diǎn)提取和rBRIEF特征描述計(jì)算兩部分組成，因此ORB 具有旋轉(zhuǎn)不變性。通過空間圖像金字塔，在不同尺度的圖像中提取oFAST 特征點(diǎn)，使ORB 特征同時(shí)具有旋轉(zhuǎn)不變性。為了實(shí)現(xiàn)均勻的特征點(diǎn)分布，避免特征點(diǎn)聚集的情況，我們在每層圖像金字塔上進(jìn)行基于四叉樹的網(wǎng)格劃分。

2.1.1 多尺度空間特征點(diǎn)檢測

ORB算法在FAST［17］算法的基礎(chǔ)上添加方向信息來檢測特征點(diǎn)。通過在圖像I(x,y)選定像素點(diǎn)P(x,y)及圓上相鄰的16 個(gè)像素點(diǎn)的灰度值差異確定特征點(diǎn)，若圓上存在連續(xù)N個(gè)點(diǎn)灰度值差異超過閾值Ip，則認(rèn)為P是一個(gè)特征點(diǎn)。我們利用高斯函數(shù)：

其中σ為尺度因子，x,y為像素坐標(biāo)點(diǎn)，構(gòu)建多尺度空間L(x,y,σ)=G(x,y,σ)*L(x,y)，在每一尺度空間上都計(jì)算FAST 角點(diǎn)，從而使特征點(diǎn)具有尺度不變性。為讓特征點(diǎn)同時(shí)具有旋轉(zhuǎn)不變性，我們使用灰度質(zhì)心法計(jì)算FAST 角點(diǎn)的主方向，從而給FAST角點(diǎn)添加方向信息。角點(diǎn)P鄰域S范圍內(nèi)灰度矩為

則其質(zhì)心為C，主方向?yàn)棣取?/p>

2.1.2 基于Q-tree的自適應(yīng)網(wǎng)格劃分

傳統(tǒng)的ORB 算法在檢測角點(diǎn)時(shí)會出現(xiàn)分布不均勻的情況，在特征響應(yīng)不明顯區(qū)域也會浪費(fèi)大量計(jì)算時(shí)間，或是出現(xiàn)角點(diǎn)聚簇的現(xiàn)象。為提高算法效率，我們在Q-tree均勻算法的基礎(chǔ)上引入自適應(yīng)劃分，根據(jù)每個(gè)尺度圖像所需要的角點(diǎn)數(shù)量動態(tài)調(diào)整網(wǎng)格大小W，則每一尺度空間圖像上應(yīng)提取的角點(diǎn)數(shù)量為Ni。

其中width為尺度上圖像寬度，height為尺度上圖像高度，N為所需特征點(diǎn)個(gè)數(shù)，ε為比例系數(shù)。

我們將每個(gè)網(wǎng)格均勻劃分為4 塊，在每一塊中分別檢測角點(diǎn)，若塊中角點(diǎn)數(shù)量不大于1，則停止分割，若塊中角點(diǎn)數(shù)量大于1，則繼續(xù)將當(dāng)前塊劃分為四塊檢測角點(diǎn)，如此循環(huán)，直至提取到的角點(diǎn)數(shù)量為Ni或全部塊中角點(diǎn)數(shù)量均不大于1。在實(shí)際操作中我們發(fā)現(xiàn)，很難出現(xiàn)全部塊中角點(diǎn)數(shù)量均不大于1 的情況，在提取角點(diǎn)數(shù)量達(dá)到Ni后會出現(xiàn)角點(diǎn)聚簇的情況，遂采用了非極大值抑制的方式來優(yōu)化聚簇角點(diǎn)。

2.1.3 rBRIER描述子提取

在上一節(jié)提取的角點(diǎn)周圍31×31 正方形區(qū)域S中選擇M組5×5 的子窗口對并分別計(jì)算灰度積分，則其描述向量可表示為

其中p(x)為S中點(diǎn)x的灰度值。則BRIER 描述子可表示為

為了使BRIER 描述子具有旋轉(zhuǎn)不變性，我們同樣為其添加方向信息Dθ：

其中Rθ為由角點(diǎn)的主方向θ確定的旋轉(zhuǎn)向量。

最終我們得到具有方向信息的rBRIER描述子gn。

2.2 基于深度信息的特征平面檢測檢測

RGB-D 傳感器提供的色彩和深度圖像包含很多噪聲和冗余信息，并且沒有語義信息，為了在原始信息的基礎(chǔ)上充分利用現(xiàn)有信息達(dá)到場景精確重建的目的。我們在原始深度圖像生成的點(diǎn)云中提取平面，作為結(jié)構(gòu)化信息。在室內(nèi)三維重建中，存在大量的平面，如墻面、桌面、地面等，這些面能夠構(gòu)建出場景的基本框架。所以此節(jié)我們使用一個(gè)基于圖優(yōu)化的有效的平面提取方法。

首先，我們從RGB-D 相機(jī)得到的深度信息為2D 的深度圖像，需要將其轉(zhuǎn)化為3D 的點(diǎn)云數(shù)據(jù)，即Γ={pi,j=(xi,j,yi,j,zi,j)T}，i,j為2D 圖像的坐標(biāo)索引。再將點(diǎn)云數(shù)據(jù)根據(jù)圖像空間索引均勻劃分為大小為H×W的塊，每個(gè)塊作為圖的一個(gè)節(jié)點(diǎn)，值得注意的是若當(dāng)前塊缺失數(shù)據(jù)、深度不連續(xù)或是節(jié)點(diǎn)剛好處于平面邊界時(shí)需要?jiǎng)h除節(jié)點(diǎn)與邊。

第二步，為實(shí)現(xiàn)快速高效的平面分割，使用AHC（層次聚類，Agglomerative Hierarchical Clustering）融合所有共面的點(diǎn)。即我們需要在圖中找到計(jì)算擬合平面具有MSE（最小均方差）的節(jié)點(diǎn)，并將其與相鄰較小節(jié)點(diǎn)合并。若此時(shí)合并后的MSE 超過給定閾值，則我們認(rèn)為找到了一個(gè)平面，并提取出來，否者將合并后的節(jié)點(diǎn)加入圖中，重復(fù)此步的運(yùn)算。平面擬合我們使用PCA（主成分分析，Principal Component Analysis）［18］來完成，讓所有樣本點(diǎn)到某個(gè)超平面的距離足夠近，即最小化投影距離。我們記點(diǎn)云為X={xi…}，投影坐標(biāo)空間為W，則投影后坐標(biāo)為Z=WT X，則點(diǎn)集到超平面距離D：

通過最小化-tr(WT XXTW)，即可求得最優(yōu)平面法向量，繼而求得平面方程。

2.3 特征點(diǎn)與特征平面配準(zhǔn)

2.3.1 ORB特征點(diǎn)匹配及誤匹配剔除

ORB特征點(diǎn)匹配對的選擇，通過計(jì)算兩幀之間描述子的漢明距離來確定，距離越小則描述子的相似度越高，即越匹配。通過這種方法選取的匹配對存在誤匹配的情況，所以需要剔除誤匹配對來減小誤差。首先我們通過粗匹配去除部分誤匹配對，即給匹配對設(shè)定一個(gè)閾值α，若匹配對間距離大于α×dmin，則剔除該匹配對，dmin為所有候選匹配對的最小距離。接下來，對所有粗匹配剔除后的匹配點(diǎn)，以點(diǎn)為圓心，3 為半徑的圓上像素點(diǎn)構(gòu)造鄰接矩陣R：

其中Ii為特征點(diǎn)灰度差，i,j為像素點(diǎn)編號。然后根據(jù)鄰接矩陣計(jì)算其特征向量P與對應(yīng)特征點(diǎn)特征向量Q，cosθ大于閾值β時(shí)認(rèn)為是合理的匹配對，否則剔除。

2.3.2 特征平面匹配對的選擇

對特征平面匹配，我們采用多維度評判方式來決定匹配對，綜合考慮兩幀之間的平面的法向量夾角、重心距離和平面間距離的影響。假設(shè)平面間的法向量夾角為Φ，平面重心距離為dis，平面間的距離為L，則特征間差異度記為V=χΦ+δdis+εL，若兩平面間差異度最小，則認(rèn)為兩平面是匹配的。

2.3.3 旋轉(zhuǎn)矩陣與平移向量求解

在連續(xù)兩幀RGB-D數(shù)據(jù)之間存在如下關(guān)系：

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

本文實(shí)驗(yàn)所用電腦配置為3.4GHz Intel Core i5-7500 CPU，內(nèi)存大小為8G，Nvidia GTX 1650 GPU，算法運(yùn)行系統(tǒng)為Ubuntu 16.04。我們使TUM公開的真實(shí)場景RGB-D 數(shù)據(jù)集，對本文方法進(jìn)行量化評估，彩色和深度圖像分辨率均為640×480。此外，我們將本文提出的方法與當(dāng)前流行的ElasticFusion算法進(jìn)行實(shí)驗(yàn)對比。

首先使用dyson_lab 樣例分別在ElasticFusion和本文提出方法上進(jìn)行三維重建實(shí)驗(yàn)，因該樣例沒有真實(shí)相機(jī)軌跡數(shù)據(jù)，所以只對重建結(jié)果在效果上進(jìn)行評價(jià)。在圖1 上半部分中可以看出，ElasticFusion 方法的重建結(jié)果顯示器上方的柜子存在明顯裂痕，柜子右側(cè)的紙箱區(qū)域也有些許雜亂，并且中心區(qū)域存在移動人員的偽影，與實(shí)際場景不一致。本文提出的方法充分利用了平面特征，在重建結(jié)果上有較好的一致性，沒有出現(xiàn)斷裂或扭曲的情況。再對比圖1 下半部分，可以發(fā)現(xiàn)ElasticFusion 方法對桌面以下區(qū)域沒有很好的進(jìn)行重建，存在部分白色區(qū)域。本文方法完整恢復(fù)出了桌面下方的實(shí)際場景，模型更接近真實(shí)場景具有更好的可用性。

圖1 dyson_lab樣例上建圖結(jié)果對比

圖2 為使用fr2_rpy 樣例的建圖結(jié)果對比。該樣例的相機(jī)除平移外還存在大量選擇場景。從重建結(jié)果圖中能夠明顯看出本文方法較ElasticFusion在重建上有更好的抗旋轉(zhuǎn)性。ElasticFusion方法重建桌面出現(xiàn)了明顯的扭曲，沒有很好地重構(gòu)出桌面的矩形結(jié)構(gòu)，桌面書籍的長方體結(jié)構(gòu)也存在扭曲情況，本文方法因加入了平面幾何約束，重建的桌面和書籍更加平整，對實(shí)際場景的表達(dá)更加準(zhǔn)確。在圖3 的相機(jī)軌跡對比圖中，可以看出本文方法的相機(jī)軌跡預(yù)測更接近真實(shí)相機(jī)軌跡。

圖2 fr2_rpy樣例建圖結(jié)果對比

圖3 fr2_rpy樣例相機(jī)軌跡預(yù)測對比

通過圖4 可以看出本文方法在室內(nèi)能夠取得良好的建圖結(jié)果，尤其對周圍墻壁及桌面等平面結(jié)構(gòu)的重建較好。

圖4 dyson_lab全場景重建結(jié)果

4 結(jié)語

本文提出了一種融合室內(nèi)點(diǎn)特征和面特征的視覺里程計(jì)算法。以O(shè)RB 算法為基礎(chǔ)，增加自適應(yīng)的網(wǎng)格劃分，加入了特征點(diǎn)匹配對剔除來提取特征點(diǎn)，利用層次聚類和主成分分析提取點(diǎn)云中場景平面信息。通過數(shù)據(jù)關(guān)聯(lián)算法、位姿模型實(shí)時(shí)解算RGB-D 設(shè)備的位姿變化，完成了對場景的實(shí)時(shí)三維重建。公開數(shù)據(jù)集的實(shí)驗(yàn)結(jié)果表明本文算法優(yōu)于傳統(tǒng)的ElasticFusion 算法，具有更好的匹配精度，證明了多特征融合的優(yōu)勢。

目前方法捕獲的特征平面比較粗糙，存在空洞，在求解位姿變化時(shí)，偶然會存在負(fù)面影響，未來需要提高平面提取的精度，以實(shí)現(xiàn)更精確的三維重建。