基于噪聲點云的三維場景重建方法

張 健，李新樂，宋 瑩，王 仁，朱 凡，趙曉燕

(1.中國航天科工集團第二研究院 706所，北京 100854；2.華中科技大學 計算機科學與技術學院，湖北 武漢 430074)

0 引 言

基于深度相機的三維重建技術在紋理特征較少的情景下仍能保持較高的重建精度，具有較好的建模效果，因此得到成為眾多學者的熱門研究方向[1,2]。許多學者基于深度相機圖像提出多種模型重建、優化方法。

目前基于深度相機的三維重建方法多采用TSDF表達模型[3]，聚合速度快，對于模型空洞等情況具有較好的魯棒性。但深度相機返回圖像數據所包含的大量噪聲數據信息會導致重建模型細節失真乃至重建失敗[4]。同時傳統TSDF模型存儲空間過大，在大場景重建時急劇消耗存儲空間，缺失靈活性[5]。

同時在閉環檢測處理方面，當前的研究方法多基于點云特征或彩色(RGB)特征實現閉環檢測[6]，這種算法計算量過大，并且使用場景受限，在處理器性能有限的情況下無法保證實時性，因此一般只能用于離線重建，其實用性則大大降低。

為了解決上述問題，本文使用統一的基于概率點云的方法，從點云模型位姿估計與聚合、子圖構建和閉環檢測3個方面解決基于深度相機的三維重建問題，從而提高建模的實時性，增強模型質量，以增強實用性。

1 基于概率點云模型的位姿估計與聚合

(1)

三維點通過相機投影得到深度圖，相機的內參矩陣如式(2)所示

(2)

設相機坐標系內點的坐標為：pk=(x,y,z)T， 根據射影關系，得到深度圖內的點到三維點的變換關系，結果如式(3)所示

(3)

(4)

(5)

綜合以上結果，測量點pk同時受表面采樣噪聲和測量點噪聲影響，使用混合高斯分布描述，如式(6)所示

(6)

在基于深度相機的三維重建中，通常使用ICP算法，將位姿估計轉化為非線性優化問題，通過迭代的方法求得位姿。當存在大量誤匹配的情況時，可以采用隨機抽樣一致算法RANSAC(random sample consensus)[8]，在每次迭代的過程中隨機抽取3對點計算位姿，計算內點數目，使用內點數目最多的位姿作為最終結果。這種做法在位姿估計的過程中沒有充分考慮數據噪聲的影響，導致精確度不足。采用SLAM算法中圖優化的思想，充分考慮點云的噪聲，通過最小化馬氏距離的方法進行位姿求解。

輸入連續的兩幀深度圖，記對應的點云為Vs和Vt， 計算兩者中點的對應關系。對于Vs中的點ps，Vt中離ps最近的點pt為相應的對應點，使用近似最近鄰算法得到。優化算法中使用的能量函數為對應點之間的歐式距離，設Vs到Vt的變換矩陣為T，其中旋轉部分為R，平移部分為t，則對應點之間的誤差如式(7)所示

Δp=Rps+t-pt

(7)

(8)

求出誤差式和協方差矩陣后，使用協方差矩陣的逆作為權重，得到馬氏距離[7]，結果如式(9)所示

E=(Δp)T∑-1(Δp)

(9)

使用高斯牛頓法或者LM法最小化馬氏距離求得變換矩陣。由于旋轉矩陣R屬于特殊正交群，不構成子空間，優化算法中增加步長的步驟會導致求得的R不滿足旋轉矩陣的條件，使用指數映射的方法，引入3維向量φ， 令

(10)

Δp=e(x)+JΔx

(11)

其中，J為雅各比矩陣。將e(x)簡記為e，把式(11)帶入式(9)，得

Ek=ck+2bkΔx+ΔxTHkΔx

(12)

其中，ck=eT∑-1e，bk=eT∑-1J，Hk=JT∑-1J。

以上是對一對對應點之間的馬氏距離的推導以及相應的線性化，實際優化的目標為所有對應點之間的馬氏距離

E=∑Ek=∑ck+2∑bkΔx+ΔxT∑HkΔx

(13)

令c=∑ck，b=∑bk， 其中b為1×3維矩陣，H為3×3維矩陣。由于式(13)求和過程中的耦合度較低，直接用CUDA加速的算法進行矩陣的求和。求解方程HΔx=-bT得到本次迭代的步長Δx，使用步長更新x。

為了提高GPU的利用率，使用一種全局的注冊方法，將所有幀的位姿同時優化，增加GPU數據的吞吐量，提高硬件的使用效率。設需要對N幀的位姿進行估計，第i幀記為Di， 求出第i幀和i-1幀之間點的對應關系，使用Di到Di-1的位姿關系Ti,i-1作為約束。則優化目標函數為所有對應點之間的馬氏距離之和，使用高斯牛頓法對能量函數進行優化，得到幀與幀之間的位姿關系，進而通過累乘得到幀與模型之間的位姿關系。這種全局的注冊方法使用累乘計算位姿，但是幀與幀之間的位姿變化通過全局優化得到，不存在累積誤差的問題。

通過位姿估計算法得到每一幀的位姿，將點云變換到世界坐標系中。為了減少點云的冗余度，使用聚合算法將多個注冊后的點云融合成全局、統一的點云。聚合算法中關鍵步驟是根據在位姿估計算法中得到的當前幀和上一幀之間點的對應關系，求得當前幀和當前模型的對應關系，使用哈希表記錄幀坐標和模型中點索引的對應關系。哈希表鍵的類型是二元組 (u,v)， 表示深度圖中像素的坐標，值的類型是整型，表示對應點在點云模型中的索引，用F表示哈希函數。哈希表有以下操作：

2 基于場景特征的子圖構建

為了處理深度相機掃描過程中誤差累積的問題，將輸入幀根據一定規則劃分為片段，每個片段稱之為子圖[9]，子圖內部使用第二節敘述的基于點云的位姿估計和聚合算法重建得到模型，整個場景的模型由若干模型片段組成，子圖之間采用一定的關系進行約束。引入姿態圖來表達子圖之間復雜的約束關系。姿態圖包括以下部分：稀疏點云和關鍵幀以及兩者之間的約束關系。稀疏點云由關鍵幀上提取的特征點通過運算得到。姿態圖中的約束關系包括以下兩種：

(1)點和關鍵幀之間的約束關系。如果關鍵幀可以看到稀疏點云中的某個點，則兩者之間存在約束關系。對于每一張關鍵幀，和多個點存在約束關系，對于稀疏點云中的每個點，和多張關鍵幀存在約束關系。

(2)關鍵幀和關鍵幀之間的約束關系。關鍵幀和關鍵幀之間的約束關系又稱為共視關系。當兩張關鍵幀之間可以看到的共同點的數目超過一定的閾值時，則稱這兩張關鍵幀之間擁有共視關系。

基于場景特征進行子圖的劃分，主要目標是盡量減少冗余子圖。當冗余子圖過多時，增加了存儲空間以及優化算法的時間復雜度，同時對閉環檢測造成了干擾。劃分的核心思想如下：當相機在掃描過程中走出當前稀疏點云的范圍時，當前關鍵幀對位姿的約束力會變弱，導致累積誤差變大，此時應插入關鍵幀，同時生成新的稀疏，擴大稀疏點云的范圍。輸入深度圖，位姿估計算法只在子圖內部進行，累積誤差只在子圖之間存在。通過對姿態圖的優化，調整子圖之間的相對位姿關系，消除累積誤差。對于每一張關鍵幀，有位姿T，表示關鍵幀的點云到稀疏點云之間的變換關系。優化的思想是最小化關鍵幀到稀疏點云對應點之間的歐式距離，設稀疏點云的集合為 {pi}， 關鍵幀的集合為 {Ki,Ti}， 其中Ti為關鍵幀到稀疏點云的位姿變換，通過最小化稀疏點云到對應點的歐式距離之和進行位姿優化。設pi在關鍵幀中對應點的集合為 (mij,Tij)， 則能量函數為

(14)

在稀疏點云中存在雜點，為了消除雜點的影響，每次優化后使用以下條件進行雜點過濾：

(1)能夠看到當前點的關鍵幀數目小于3。說明該點只在兩張關鍵幀中被看到，那么有很大的可能該點是由于誤匹配所產生的雜點。

(2)當前點到到關鍵幀中對應點的平均歐式距離大于閾值。說明該點與各個關鍵幀中對應點的匹配度較差，如果不去除會導致整體誤差較大，對其它點的優化造成影響。

使用兩段式的方法與稀疏點云進行匹配，對以下條件進行判斷：

(1)與上一張關鍵幀之間的距離超過20幀。一般在輸入深度圖的數量超過30幀后累積誤差的影響開始變大，如果在20幀以內插入關鍵幀說明是誤判，過多的關鍵幀會導致算法的時間復雜度變高。

(2)匹配配上的點的數目小于50。說明此時相機已經走出了稀疏點云的范圍，稀疏點云對位姿的約束變弱。

(3)匹配上的點的數目比當前關鍵幀匹配點的數目少90%。說明當前關鍵幀對輸入幀的約束變弱，應當插入新的關鍵幀，否則當相機繼續采集深度圖時，會導致匹配不到點，跟蹤失敗。

3 GPU加速的FPFH算法

FPFH是一種基于直方圖的算法[10]，通過對點周圍的集合特征進行描述，構造描述子。一共有4個步驟：法線估計、構造Darboux frame、計算SPF描述子、計算FPFH描述子。

為了提高FPFH算法的實時性，使用CUDA進行加速。FPFH中有兩個關鍵算法：①鄰域搜索算法[11]。鄰域搜索算法指求以給定點為中心，半徑為r的球形區域內的點，用于法線估計等過程。②k近鄰算法[12]。k近鄰算法指給定描述子，根據其它描述子到該描述子的距離從小到大進行排序，求出前k個描述子。用于特征點的匹配。在基于中央處理器(central processing unit，CPU)的算法中，這兩者均用kd樹實現。GPU使用流式計算單元對數據進行處理，同時并行處理的線程可以達到上百個。流式計算單元具有單指令集多數據流的特點，在某一時刻，并行線程處理同樣的指令。kd樹算法分支太過復雜，具有大量的判斷結構存在。故當其中一個線程處理某個分支時，其它線程會陷入等待，導致算法的時間復雜度變高，并且kd樹的訓練過程中數據之間的耦合度較高，無法取得較大的并行度。故kd樹這種數據結構并不適合GPU，需要設計新的數據結構。在GPU算法的設計中，遵循的原則是盡量使用邏輯簡單的算法，在訪存上做優化。因為GPU總體頻率高，每秒鐘處理的指令數目多，同時處理的數據量太大，使得流式計算單元和線程之間的帶寬不夠。

對于GPU加速的鄰域搜索算法，核心思想是將點云劃分為小方格，通過在小方格內并行搜索提高算法的性能。將點云劃分為邊長為m的小方格，稱為體素(voxels)。為了將點云表示為網格形式，需要計算每個網格內有哪些點。引入新型數據結構，稱之為網格點云，如圖1所示。

圖1 網格點云

得到網格點云的數據結構后，使用數據結構進行范圍搜索。設點云中有n個點，對每個點進行范圍搜索，搜索半徑為r(單位為網格)。對于每個點，需要在 (2r+1)3個網格中進行搜索，則一共需要在n×(2r+1)3個網格中進行搜索。對每個網格使用一個線程進行搜索，將結果存儲到對應的數組中。

d=NQ+NR-2QTR

(15)

d的大小為3n×m， 第i行第j列元素表示Q中第i個數據到R中第j個數據的值。對于k近鄰算法，需要對d的第i行進行排序，取前k個值即為離對應點最近的k個點。

4 實驗結果與分析

使用基于點云的三維重建系統對在真實環境中采集的數據和公開數據集TUM-RGBD數據集進行三維重建，效果圖如圖2所示。

圖2 建模效果

建模算法完全基于深度信息構建，僅使用深度圖作為數據的輸入，故重建得到的模型沒有紋理信息。圖2中右下角為TUM-RGBD數據集中桌面數據重建結果，其它圖片為在各種施工環境下采集的數據的重建結果。

點云位姿估計算法體現的是局部性，使用TUM RGB-D 數據集，數據集提供深度圖以及對應位姿的ground truth。從數據集中截取3段，每段的長度為10幀，分別使用ICP、點到平面的ICP算法以及我們的算法進行位姿估計，使用ground truth位姿以及估計位姿分別對深度圖進行變換，然后求得對應點之間的平均歐式距離(RMSE)，用于衡量算法的質量，結果見表1。從實驗結果中可以看出，點到平面的ICP算法比起點到點的ICP算法具有更好的魯棒性。同時，由于兩種ICP算法均未對數據中的噪聲進行合適的建模，在有噪聲的情況下性能均弱于我們提出的方法。

表1 RMSE實驗結果

子圖構建算法表現的是算法的全局性，當掃描軌跡過長時，若不引入子圖進行約束，會導致累積誤差越來越大。使用TUM-RGB數據集中的desk數據，分別用不帶子圖構建的算法(即整個跟蹤過程中單純的使用基于點云的位姿估計算法，不進行子圖劃分)和帶子圖構建的算法對數據進行處理，繪制出相機運行的軌跡，如圖3所示，圖3左側為不帶子圖構建的相機軌跡，圖3右側為帶子圖構建相機軌跡。當不使用子圖時，位姿的誤差會逐漸累積，最終導致相機跟丟，使得重建失敗。

圖3 相機運行軌跡

對于FPFH算法，可以直接用于點云注冊也可以用于閉環檢測，從這兩個方面進行實驗。

對于點云注冊，使用GPU加速的FPFH算法對不同的物體進行注冊，將不同部分用顏色標出，結果如圖4所示。從圖中可以看出，GPU加速的FPFH算法對于各種形狀的物體都能得到正確的注冊結果。

圖4 點云注冊效果

目前常用的點云注冊算法有FPFH(pcl庫中的CPU實現版)以及基于神經網絡的3DMatch算法。在實際工程中發現，對于點云的注冊問題，這幾種算法的準確度相差不大，對于特征豐富的點云都可以取得不錯的結果，對于特征缺失的點云則會注冊失敗。從時間復雜度對這3種算法進行分析，對圖4中最右邊的水管模型進行注冊，兩個點云中點的數目分別為5.6萬和6.3萬，時間見表2。

表2 注冊算法耗時

從表2中可以看出，對比CPU實現的FPFH算法，GPU加速的FPFH算法具有顯著的加速比，3DMatch算法最慢。

對于GPU加速的FPFH算法在閉環檢測中的性能，使用Choi的數據集進行定量分析。Choi從不同得三維場景的數據集中采集片段構造數據集，通過召回率和精度兩個指標來衡量不同匹配算法的性能。對pcl中的FPFH算法、GPU加速的FPFH算法和3DMatch算法進行測量，見表3。

表3 召回度和精確度測量結果

從結果中可以看出，雖然理論上GPU加速的FPFH算法和pcl中的FPFH算法準確度應該是一樣的，但是實際上GPU加速的FPFH算法性能稍好一點，這是數據誤差造成的。3DMatch算法對比我們的算法優勢不明顯，但是3DMatch算法時間復雜度太高，且無法用GPU加速(因為基于神經網絡的算法原本使用GPU實現)，同時3DMatch的處理對象是TSDF模型，空間復雜度較高，故GPU加速的FPFH算法在實用性方面更勝一籌。

5 結束語

基于RGB-D的三維重建算法具有精確度高、視覺效果強等優點。目前，雖然基于RGB-D的三維重建技術已經取得了不錯的發展，但是還存在一些不足之處。本文設計了基于點云的三維重建算法，使用概率點云表示模型，充分考慮噪聲影響，優化了全局點云注冊算法；通過優化子圖構建和閉環檢測算法，提高了模型建模質量，具有更好的魯棒性；通過設計基于GPU加速的FPFH算法，保證了建模的實時性要求。后續在GPU顯存調度方面仍具有一定的優化空間。