汪 霖，郭佳琛，張 璞，萬 騰，劉 成，杜少毅

(1.西北大學(xué) 信息科學(xué)與技術(shù)學(xué)院,陜西 西安 710127;2.西安交通大學(xué) 人工智能學(xué)院,陜西 西安 710149)

對于不同光照、角度等條件下獲取的兩個三維點(diǎn)云,其剛體配準(zhǔn)的目的是建立兩個點(diǎn)云間的空間對應(yīng)關(guān)系，并尋找它們之間的最優(yōu)剛體變換關(guān)系[1],從而對齊空間中的兩個點(diǎn)云。三維點(diǎn)云剛體配準(zhǔn)廣泛應(yīng)用于智能機(jī)器人、無人車、醫(yī)學(xué)和文物保護(hù)等領(lǐng)域[2-5]。

為了求解上述優(yōu)化問題,Besl和Mckay于1992年提出了迭代最近點(diǎn)(iterative closest point,ICP)算法[6],其在解決噪聲較小的剛體點(diǎn)云配準(zhǔn)問題中具有一定的精度和速度優(yōu)勢。由于ICP算法要求點(diǎn)云是完全對應(yīng)的,因此該算法對于含有噪聲和外點(diǎn)的點(diǎn)云數(shù)據(jù)進(jìn)行配準(zhǔn)時,其配準(zhǔn)精度較低。針對ICP算法在點(diǎn)云配準(zhǔn)方面的不足,一些學(xué)者提出了其改進(jìn)方法。Bae等人通過點(diǎn)云曲率的計算,構(gòu)成空間內(nèi)各點(diǎn)與其鄰域點(diǎn)的協(xié)方差式,以提高匹配的收斂速度[7]；Zhang等人提出將噪聲點(diǎn)進(jìn)行刪除處理,以提高剛體配準(zhǔn)的魯棒性[8]；Yang等人在分支定界的基礎(chǔ)上,增加了點(diǎn)云運(yùn)動特征信息,一定程度上改進(jìn)了配準(zhǔn)效率[9]；Luo等人通過距離視差的計算提高了ICP算法的配準(zhǔn)精度[10]；Guan等人采用體素點(diǎn)陣對點(diǎn)云數(shù)據(jù)進(jìn)行重采樣[11],并使用kd-tree對點(diǎn)云對應(yīng)關(guān)系進(jìn)行優(yōu)化[12],以提高ICP算法的配準(zhǔn)精度。隨著深度相機(jī)的發(fā)展,RGB-D點(diǎn)云的配準(zhǔn)也逐漸受到學(xué)者的關(guān)注。MEN等人利用點(diǎn)云位置坐標(biāo)結(jié)合色調(diào)信息,提出了四維ICP算法,但是該方法對噪聲和外點(diǎn)的魯棒性較差[13]；Korn等人利用Lab顏色信息對ICP算法進(jìn)行改進(jìn),利用kd-tree搜索辦法提高點(diǎn)云的匹配度,但該方法對采集環(huán)境的光照有一定要求,對于光照變化較大的場景其點(diǎn)云配準(zhǔn)精度較低[14]；Danelljan等人結(jié)合顏色信息提出一種概率模型約束方法,但該方法不適合球面結(jié)構(gòu)的點(diǎn)云配準(zhǔn)[15]。

針對ICP算法對于存在噪聲和外點(diǎn)的三維點(diǎn)云剛體配準(zhǔn)的配準(zhǔn)精度較低、魯棒性差等問題,本文提出一種基于改進(jìn)ICP的三維點(diǎn)云剛體配準(zhǔn)方法。為了提高三維點(diǎn)云剛體配準(zhǔn)方法的魯棒性,建立了基于偽Huber損失函數(shù)的三維點(diǎn)云剛體配準(zhǔn)模型。在此基礎(chǔ)上,引入RGB-D點(diǎn)云顏色信息,輔助點(diǎn)云對應(yīng)關(guān)系的準(zhǔn)確建立,從而減少噪聲和外點(diǎn)對配準(zhǔn)結(jié)果的影響。由于Levenberg-Marquardt(LM)優(yōu)化三維點(diǎn)云剛體配準(zhǔn)模型時對初值較敏感,若初值選取不合適,將極大地影響點(diǎn)云配準(zhǔn)精度,為此,將奇異值分解(singular value decomposition,SVD)和LM算法相結(jié)合,利用SVD算法求解的剛體變換關(guān)系作為LM算法的初值,從而提高點(diǎn)云配準(zhǔn)精度。實(shí)驗(yàn)結(jié)果表明,本文所提三維點(diǎn)云剛體配準(zhǔn)方法能夠有效抑制噪聲和外點(diǎn)對配準(zhǔn)精度的影響,其三維點(diǎn)云配準(zhǔn)精度高。

1 ICP算法簡介

ICP算法,由于其迭代方式簡單、快速,被廣泛應(yīng)用于三維點(diǎn)云剛體配準(zhǔn)中。ICP算法的目標(biāo)是尋找一個最優(yōu)剛體變換,使得形狀點(diǎn)云S和模型點(diǎn)云M能夠在空間上對應(yīng)起來。

假設(shè)形狀點(diǎn)云和模型點(diǎn)云的剛體變換關(guān)系中,旋轉(zhuǎn)矩陣為R,平移向量為t,則三維點(diǎn)云剛體準(zhǔn)優(yōu)化問題可寫為

s.t.RTR=I3,det(R)=1。

(1)

其中,c(i)為i的對應(yīng)點(diǎn)。

ICP算法采用迭代求解旋轉(zhuǎn)矩陣R和平移向量t。在每一次的迭代中,都有以下3個步驟:

步驟1基于第k-1次的剛體變換Rk-1和tk-1,建立點(diǎn)云之間的點(diǎn)對應(yīng)點(diǎn)關(guān)系:

(2)

(R*,t*)=

(3)

更新第k步的剛體變換Rk和tk:

Rk=R*Rk-1,tk=R*tk-1+t*。

(4)

步驟1中,ICP算法采用點(diǎn)對點(diǎn)的搜索策略建立兩個點(diǎn)云之間的對應(yīng)關(guān)系,通常采用基于Delaunay三角化[16]的最近點(diǎn)搜索方法,以提高搜索效率。該方法首先對模型點(diǎn)云進(jìn)行Delaunay三角剖分,再采用三角劃分的搜索策略找到兩個點(diǎn)云的對應(yīng)點(diǎn)。

而對于步驟2中剛體變換求解問題,ICP算法通常采用SVD方法對其進(jìn)行求解。首先給出以下兩個定理。

s.t.RTR=Im,det(R)=1。

(5)

能夠得到R*=VDUT,其中U為m×m維正交矩陣,V為m×m維正交矩陣,D矩陣的定義為

(6)

求得最優(yōu)的旋轉(zhuǎn)矩陣R*后,最優(yōu)平移向量t*為

(7)

最后,根據(jù)式(4)更新第k步的剛體變換Rk和tk。

2 基于改進(jìn)ICP算法的三維點(diǎn)云剛體配準(zhǔn)方法

2.1 算法描述

在ICP算法的基礎(chǔ)上,本文提出了基于改進(jìn)ICP的三維點(diǎn)云剛體配準(zhǔn)方法。為了提高配準(zhǔn)方法對噪聲和外點(diǎn)的魯棒性,將對噪聲和外點(diǎn)不敏感的偽Huber損失函數(shù)引入到三維點(diǎn)云剛體配準(zhǔn)優(yōu)化模型中。考慮到基于Delaunay三角化[16]的最近點(diǎn)搜索方法建立點(diǎn)云之間的對應(yīng)點(diǎn)關(guān)系會受到點(diǎn)云中噪聲和外點(diǎn)的影響,可能形成錯誤的對應(yīng)關(guān)系,從而降低配準(zhǔn)精度。為此,本文方法引入顏色信息作為約束條件之一,結(jié)合RGB-D顏色信息和位置坐標(biāo)信息,建立點(diǎn)云之間更為精準(zhǔn)的對應(yīng)關(guān)系。最后通過迭代求解的方式,當(dāng)誤差達(dá)到設(shè)定閾值或迭代次數(shù)達(dá)到最大時,得到最優(yōu)的三維點(diǎn)云剛體變換關(guān)系。

2.2 基于偽Huber損失函數(shù)的點(diǎn)云剛體配準(zhǔn)優(yōu)化模型建立

為了提高三維點(diǎn)云剛體配準(zhǔn)方法在復(fù)雜噪聲環(huán)境下的魯棒性,需要有效抑制噪聲和外點(diǎn)對三維點(diǎn)云剛體配準(zhǔn)優(yōu)化模型的影響。為此,本文通過引入對噪聲和外點(diǎn)不敏感的偽Huber損失函數(shù)，建立兩個點(diǎn)云剛體配準(zhǔn)的數(shù)學(xué)模型。

偽Huber損失函數(shù)是Huber損失函數(shù)的平滑版,具有連續(xù)可導(dǎo)性,其函數(shù)定義為

(8)

其中：b為噪聲和外點(diǎn)閾值；a為誤差。對于較小的a值,該損失函數(shù)近似值為a/2，而對于較大的a值,該損失函數(shù)可近似為一條斜率為b的直線,因此，其對噪聲和外點(diǎn)不敏感。

將ICP算法的優(yōu)化函數(shù)(1)中的度量函數(shù)替換為偽Huber損失函數(shù),可得到如下的基于偽Huber損失函數(shù)的三維點(diǎn)云剛體配準(zhǔn)優(yōu)化模型:

s.t.RTR=I3,,det(R)=1。

(9)

2.3 基于改進(jìn)ICP的三維點(diǎn)云剛體配準(zhǔn)方法

2.3.1 總體流程圖 結(jié)合ICP算法的思想和本文方法的優(yōu)化目標(biāo)函數(shù),可以給出本文方法的總體流程圖，如圖1所示。本文方法的步驟與ICP算法相似,重點(diǎn)在于迭代過程中點(diǎn)云間對應(yīng)關(guān)系的建立和求解,以及根據(jù)優(yōu)化目標(biāo)函數(shù)(9)計算新的剛體變換兩個步驟。

圖1 本文方法的總體流程圖Fig.1 Overall flowchart of the method

2.3.2 對應(yīng)關(guān)系的建立和求解 根據(jù)點(diǎn)云的位置分布特征和顏色信息的引導(dǎo),能夠得到更加準(zhǔn)確的點(diǎn)云對應(yīng)關(guān)系。本文采用HSV顏色空間模型,其中H、S和V分別表示色相(hue)、飽和度(saturation)和色調(diào)(value)。由于色相值能夠更好地反映顏色的本質(zhì),并且其對光照度的變化不敏感,故本文只利用色相值進(jìn)行顏色輔助。通過引入色相值,建立點(diǎn)云之間的點(diǎn)對應(yīng)點(diǎn)關(guān)系,將式(2)改寫為

(10)

對于對應(yīng)點(diǎn)關(guān)系的求解,本文同樣采用Delaunay三角化方法。通過引入顏色輔助信息,能夠獲得比ICP算法更為精確的點(diǎn)云間對應(yīng)關(guān)系。兩組點(diǎn)云之間的對應(yīng)關(guān)系建立效果對比如圖2所示。

圖2 兩組點(diǎn)云對應(yīng)關(guān)系建立效果對比Fig.2 Comparison of the establishment effect of the two sets of point cloud correspondence

(11)

由于LM優(yōu)化算法對于初值選取較為敏感,本文采用SVD方法和LM算法相結(jié)合的優(yōu)化方法對模型(11)進(jìn)行求解,即利用傳統(tǒng)ICP算法中SVD方法求解優(yōu)化模型(11)的初值,再利用LM算法對優(yōu)化模型(11)進(jìn)行求解,以精確求解R*和t*。在此基礎(chǔ)上,更新第k步的剛體變換Rk和tk:

Rk=R*Rk-1,tk=R*tk-1+t*。

(12)

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

為了便于描述,將本文所提的基于改進(jìn)ICP算法的三維點(diǎn)云剛體配準(zhǔn)方法記為RPH-ICP方法。本節(jié)將對RPH-ICP方法和ICP算法進(jìn)行三維點(diǎn)云剛體配準(zhǔn)實(shí)驗(yàn),以驗(yàn)證本文算法的有效性。

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

實(shí)驗(yàn)中,采用標(biāo)準(zhǔn)數(shù)據(jù)庫和仿真數(shù)據(jù)對RPH-ICP方法和ICP算法進(jìn)行點(diǎn)云剛體配準(zhǔn)實(shí)驗(yàn)對比。標(biāo)準(zhǔn)數(shù)據(jù)選取于A Large-Scale Hierarchical Multi-View RGB-D Object Dataset[17]中的earth與foodbag數(shù)據(jù)。考慮數(shù)據(jù)采集中噪聲的影響,標(biāo)準(zhǔn)數(shù)據(jù)中本身帶有噪聲。此外,將模型點(diǎn)云沿z軸從0°到60°的隨機(jī)角進(jìn)行旋轉(zhuǎn),得到其目標(biāo)點(diǎn)云。再加入部分隨機(jī)噪聲和均值為8、方差為2的高斯噪聲得到待配準(zhǔn)形狀點(diǎn)云。采用模型點(diǎn)云與形狀點(diǎn)云的均方根誤差(root mean square,RMS)、迭代次數(shù),以及旋轉(zhuǎn)矩陣估計誤差εR=‖R估計-R真實(shí)‖2和平移向量估計誤差εt=‖t估計-t真實(shí)‖2作為點(diǎn)云配準(zhǔn)的性能指標(biāo),對RPH-ICP方法和ICP算法的點(diǎn)云配準(zhǔn)效果進(jìn)行比較。

3.2 三維點(diǎn)云剛體配準(zhǔn)的實(shí)驗(yàn)結(jié)果及分析

實(shí)驗(yàn)分別采用本文方法(RPH-ICP方法)和ICP算法進(jìn)行三維點(diǎn)云的剛體配準(zhǔn)。通過實(shí)驗(yàn)結(jié)果對比,以驗(yàn)證本文方法的收斂性與剛體配準(zhǔn)精度。兩種方法的配準(zhǔn)結(jié)果數(shù)據(jù)如表1所示。由表1可知，本文方法得到的RMS誤差值比ICP算法更小,并且旋轉(zhuǎn)矩陣R和平移向量t的估計誤差εR和εt均小于ICP算法,本文所提方法的點(diǎn)云配準(zhǔn)精度高,其原因是本文方法所引入的偽Huber函數(shù)能夠有效抑制噪聲和外點(diǎn)的影響。

表1 兩種三維點(diǎn)云剛體配準(zhǔn)方法的實(shí)驗(yàn)結(jié)果Tab.1 Experimental results of two rigid registration methods of 3D point cloud

圖3給出earth數(shù)據(jù)的收斂圖以及foodbag數(shù)據(jù)在不同外點(diǎn)比例下的收斂圖,由圖3可知,本文方法的收斂速度比ICP算法快,并且本文方法對于外點(diǎn)具有很好的魯棒性。

為了更直觀地顯示兩種方法的配準(zhǔn)結(jié)果,圖4和圖5給出了兩種方法對于earth數(shù)據(jù)和foodbag數(shù)據(jù)的三維點(diǎn)云剛體配準(zhǔn)結(jié)果。由圖4和圖5可知,本文方法配準(zhǔn)后在各視角下藍(lán)色點(diǎn)與紅色點(diǎn)的重合度均高于ICP算法,因此,本文方法能夠有效抑制噪聲和外點(diǎn)的影響，配準(zhǔn)精度高。

圖3 點(diǎn)云配準(zhǔn)收斂曲線Fig.3 Registration convergence curve of point cloud

圖4 本文方法和ICP算法配準(zhǔn)結(jié)果比較(earth數(shù)據(jù))Fig.4 The comparison of registration results between the proposed method and ICP algorithm (earth data)

圖5 本文方法和ICP算法配準(zhǔn)結(jié)果比較(foodbag數(shù)據(jù))Fig.5 The comparison of registration results between the proposed method and ICP algorithm (foodbag data)

4 結(jié)語

為了有效抑制噪聲和外點(diǎn)對于三維點(diǎn)云剛體配準(zhǔn)精度的影響,提高點(diǎn)云配準(zhǔn)方法的精度和魯棒性，本文提出了一種基于改進(jìn)ICP算法的三維點(diǎn)云剛體配準(zhǔn)方法。該方法通過引入偽Huber損失函數(shù)建立魯棒的三維點(diǎn)云剛體配準(zhǔn)優(yōu)化模型,利用HSV顏色空間的色相值輔助建立更為準(zhǔn)確的點(diǎn)云對應(yīng)關(guān)系,從而提高算法對于噪聲和外點(diǎn)的抗干擾能力。實(shí)驗(yàn)結(jié)果表明,本文所提出方法能夠有效抑制噪聲和外點(diǎn)的影響，點(diǎn)云剛體配準(zhǔn)精度高。