融合點、面特征的RGB-D 視覺里程計?

范都耀 宋勇磊

（南京理工大學計算機科學與工程學院 南京 210094）

1 引言

在機器人智能化、自主化及三維建模等領域利用低成本的RGB-D 傳感器產生的序列景深和色彩圖像構建精確的場景三維模型是基礎且關鍵的技術，也是當前學者們研究的熱點。特別是在機器人領域，能否構建場景的精確模型，直接影響到機器人能否在已創建的場景模型中實現精確定位，繼而影響到機器人的自主導航移動。應用同時定位與地圖構建（Simultaneous Localization and Mapping，SLAM）［1］技術構建場景模型，是當前學者廣泛使用與研究的一個方向。

基于RGB-D 的SLAM［2］技術經過多年的發展，沉淀出了一系列的成熟算法，主要分為基于泊松曲面［3～4］的重建、基于SIFT、SURF 等特征的稀疏重建或半稠密重建算法和以KinectFusion［5］和StaticFusion［6］為代表的稠密重建算法。基于泊松曲面的這類算法常用于離線下的點云數據，用向量表示物體的內外表面方向，通過優化指標函數生成一個平滑的表面估計。基于稀疏和半稠密重建算法有ORB-SLAM［7］在全局都使用ORB 特征是基于稀疏特征的代表性三維重建方法；SVO［8］將角點作為關鍵點，利用關鍵點鄰域內的信息跟蹤相機位姿，并在后端BA優化過程中使用了最小重投影誤差是經典的半直接法重建方法；DSO［9］是直接法三維重建方法。

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

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

針對定位丟失和模型存在空洞兩個問題，本文提出一種通過增加特征點和特征面輔助原始ICP算法提高魯棒性的SLAM 系統。利用RGB 圖像提取改進ORB 特征來確定初始位姿變化，融合深度圖像提取到的特征平面［19］作為幾何約束，實現三維場景的重建，通過公共數據集我們取得了更好的效果。

2 特征點與特征平面檢測

2.1 改進的ORB特征點檢測

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

2.1.1 多尺度空間特征點檢測

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

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

則其質心為C，主方向為θ。

2.1.2 基于Q-tree的自適應網格劃分

傳統的ORB 算法在檢測角點時會出現分布不均勻的情況，在特征響應不明顯區域也會浪費大量計算時間，或是出現角點聚簇的現象。為提高算法效率，我們在Q-tree均勻算法的基礎上引入自適應劃分，根據每個尺度圖像所需要的角點數量動態調整網格大小W，則每一尺度空間圖像上應提取的角點數量為Ni。

其中width為尺度上圖像寬度，height為尺度上圖像高度，N為所需特征點個數，ε為比例系數。

我們將每個網格均勻劃分為4 塊，在每一塊中分別檢測角點，若塊中角點數量不大于1，則停止分割，若塊中角點數量大于1，則繼續將當前塊劃分為四塊檢測角點，如此循環，直至提取到的角點數量為Ni或全部塊中角點數量均不大于1。在實際操作中我們發現，很難出現全部塊中角點數量均不大于1 的情況，在提取角點數量達到Ni后會出現角點聚簇的情況，遂采用了非極大值抑制的方式來優化聚簇角點。

2.1.3 rBRIER描述子提取

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

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

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

其中Rθ為由角點的主方向θ確定的旋轉向量。

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

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

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

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

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

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

2.3 特征點與特征平面配準

2.3.1 ORB特征點匹配及誤匹配剔除

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

其中Ii為特征點灰度差，i,j為像素點編號。然后根據鄰接矩陣計算其特征向量P與對應特征點特征向量Q，cosθ大于閾值β時認為是合理的匹配對，否則剔除。

2.3.2 特征平面匹配對的選擇

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

2.3.3 旋轉矩陣與平移向量求解

在連續兩幀RGB-D數據之間存在如下關系：

3 實驗結果與分析

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

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

圖1 dyson_lab樣例上建圖結果對比

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

圖2 fr2_rpy樣例建圖結果對比

圖3 fr2_rpy樣例相機軌跡預測對比

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

圖4 dyson_lab全場景重建結果

4 結語

本文提出了一種融合室內點特征和面特征的視覺里程計算法。以ORB 算法為基礎，增加自適應的網格劃分，加入了特征點匹配對剔除來提取特征點，利用層次聚類和主成分分析提取點云中場景平面信息。通過數據關聯算法、位姿模型實時解算RGB-D 設備的位姿變化，完成了對場景的實時三維重建。公開數據集的實驗結果表明本文算法優于傳統的ElasticFusion 算法，具有更好的匹配精度，證明了多特征融合的優勢。

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