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

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

0 引 言

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

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

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

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

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

(1)

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

(2)

設(shè)相機坐標(biāo)系內(nèi)點的坐標(biāo)為：pk=(x,y,z)T， 根據(jù)射影關(guān)系，得到深度圖內(nèi)的點到三維點的變換關(guān)系，結(jié)果如式(3)所示

(3)

(4)

(5)

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

(6)

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

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

Δp=Rps+t-pt

(7)

(8)

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

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

(9)

使用高斯牛頓法或者LM法最小化馬氏距離求得變換矩陣。由于旋轉(zhuǎn)矩陣R屬于特殊正交群，不構(gòu)成子空間，優(yōu)化算法中增加步長的步驟會導(dǎo)致求得的R不滿足旋轉(zhuǎn)矩陣的條件，使用指數(shù)映射的方法，引入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。

以上是對一對對應(yīng)點之間的馬氏距離的推導(dǎo)以及相應(yīng)的線性化，實際優(yōu)化的目標(biāo)為所有對應(yīng)點之間的馬氏距離

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

(13)

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

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

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

2 基于場景特征的子圖構(gòu)建

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

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

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

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

(14)

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

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

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

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

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

(2)匹配配上的點的數(shù)目小于50。說明此時相機已經(jīng)走出了稀疏點云的范圍，稀疏點云對位姿的約束變?nèi)酢?/p>

(3)匹配上的點的數(shù)目比當(dāng)前關(guān)鍵幀匹配點的數(shù)目少90%。說明當(dāng)前關(guān)鍵幀對輸入幀的約束變?nèi)?，?yīng)當(dāng)插入新的關(guān)鍵幀，否則當(dāng)相機繼續(xù)采集深度圖時，會導(dǎo)致匹配不到點，跟蹤失敗。

3 GPU加速的FPFH算法

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

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

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

圖1 網(wǎng)格點云

得到網(wǎng)格點云的數(shù)據(jù)結(jié)構(gòu)后，使用數(shù)據(jù)結(jié)構(gòu)進(jìn)行范圍搜索。設(shè)點云中有n個點，對每個點進(jìn)行范圍搜索，搜索半徑為r(單位為網(wǎng)格)。對于每個點，需要在 (2r+1)3個網(wǎng)格中進(jìn)行搜索，則一共需要在n×(2r+1)3個網(wǎng)格中進(jìn)行搜索。對每個網(wǎng)格使用一個線程進(jìn)行搜索，將結(jié)果存儲到對應(yīng)的數(shù)組中。

d=NQ+NR-2QTR

(15)

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

4 實驗結(jié)果與分析

使用基于點云的三維重建系統(tǒng)對在真實環(huán)境中采集的數(shù)據(jù)和公開數(shù)據(jù)集TUM-RGBD數(shù)據(jù)集進(jìn)行三維重建，效果圖如圖2所示。

圖2 建模效果

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

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

表1 RMSE實驗結(jié)果

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

圖3 相機運行軌跡

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

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

圖4 點云注冊效果

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

表2 注冊算法耗時

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

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

表3 召回度和精確度測量結(jié)果

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

5 結(jié)束語

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