基于改進ICP算法的三維點云剛體配準方法

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

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

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

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

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

1 ICP算法簡介

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

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

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

(1)

其中,c(i)為i的對應點。

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

步驟1基于第k-1次的剛體變換Rk-1和tk-1,建立點云之間的點對應點關系:

(2)

(R*,t*)=

(3)

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

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

(4)

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

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

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 基于改進ICP算法的三維點云剛體配準方法

2.1 算法描述

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

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

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

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

(8)

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

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

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

(9)

2.3 基于改進ICP的三維點云剛體配準方法

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

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

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

(10)

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

圖2 兩組點云對應關系建立效果對比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)進行求解,即利用傳統(tǒng)ICP算法中SVD方法求解優(yōu)化模型(11)的初值,再利用LM算法對優(yōu)化模型(11)進行求解,以精確求解R*和t*。在此基礎上,更新第k步的剛體變換Rk和tk:

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

(12)

3 實驗結(jié)果及分析

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

3.1 實驗環(huán)境設置

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

3.2 三維點云剛體配準的實驗結(jié)果及分析

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

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

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

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

圖3 點云配準收斂曲線Fig.3 Registration convergence curve of point cloud

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

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

4 結(jié)語

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