基于多分辨率的快速迭代最近點(diǎn)配準(zhǔn)算法

王 碩 王亞飛 李學(xué)華

(北京信息科技大學(xué)信息與通信工程學(xué)院 北京 100101)

0 引 言

點(diǎn)云配準(zhǔn)是計(jì)算機(jī)視覺和圖像處理的一項(xiàng)重要研究課題。該技術(shù)廣泛應(yīng)用于醫(yī)學(xué)[1]、移動機(jī)器人導(dǎo)航[2]、計(jì)算機(jī)輔助設(shè)計(jì)[3]、三維重建[4]等領(lǐng)域中。通過特定的幾何變換，點(diǎn)云配準(zhǔn)旨在使兩組不同圖像的點(diǎn)達(dá)到空間一致性。換句話說，從參數(shù)未知的初始變換開始，它試圖找到一個(gè)最優(yōu)的變換，使兩個(gè)不同相機(jī)坐標(biāo)系下的點(diǎn)云轉(zhuǎn)換到同一個(gè)世界坐標(biāo)系下。

關(guān)于點(diǎn)云配準(zhǔn)的研究可以追溯到20世紀(jì)80年代早期，當(dāng)時(shí)專門針對二次曲面提出了全局剛性點(diǎn)集配準(zhǔn)算法。1992年Besl等[5]提出了經(jīng)典的ICP算法，用于點(diǎn)云的精確配準(zhǔn)，后續(xù)ICP算法的改進(jìn)主要圍繞提高配準(zhǔn)精度和加快配準(zhǔn)速度兩個(gè)方面進(jìn)行研究。為了改進(jìn)ICP算法的配準(zhǔn)精度問題，Rusu等[6]提出快速特征直方圖(FPFH)特征，目標(biāo)是通過點(diǎn)周圍的平均曲率來編碼其鄰域的幾何屬性，該算法能很好地應(yīng)對鄰域中存在不同采樣密度或不同噪聲水平的點(diǎn)，進(jìn)而提升配準(zhǔn)精度。Seal等[7]使用Hausdorff距離計(jì)算兩點(diǎn)云間誤差，降低收斂誤差。Ye等[8]將點(diǎn)的顏色和深度信息一起作為配準(zhǔn)因素，提高了配準(zhǔn)精度，并避免初始值的影響。文獻(xiàn)[9-12]提出了一種基于關(guān)鍵點(diǎn)的點(diǎn)云配準(zhǔn)方法，根據(jù)關(guān)鍵點(diǎn)不變的特性實(shí)現(xiàn)精確配準(zhǔn)，然而每個(gè)點(diǎn)的特征計(jì)算量很大，時(shí)間消耗較多。唐志榮等[13]，提出一種基于多維混合柯西分布的點(diǎn)云配準(zhǔn)算法，該算法將點(diǎn)云數(shù)據(jù)模型轉(zhuǎn)換為多維混合柯西分布模型，用數(shù)據(jù)中心點(diǎn)進(jìn)行配準(zhǔn)，該算法配準(zhǔn)效果較好，但配準(zhǔn)效率有待提升。

以上方法由于要計(jì)算點(diǎn)云更多維度的特征，增加了點(diǎn)云配準(zhǔn)時(shí)間。為加快配準(zhǔn)速度，鐘瑩等[14]使用歐氏距離閾值和法矢量夾角閾值來選取正確的匹配點(diǎn)對，這種方法可降低點(diǎn)云間點(diǎn)對的噪聲帶來的干擾，減少迭代次數(shù)，但該算法過于依賴于閾值的選定，閾值設(shè)置不佳會導(dǎo)致精度的下降。楊秋翔等[15]根據(jù)每一點(diǎn)的法矢量角計(jì)算該點(diǎn)的關(guān)鍵度，依據(jù)關(guān)鍵度對點(diǎn)云數(shù)據(jù)進(jìn)行精簡，提高了配準(zhǔn)效率，但當(dāng)點(diǎn)云中點(diǎn)數(shù)超過十萬時(shí)，關(guān)鍵度的計(jì)算成為主要的耗時(shí)部分。Ji等[16]利用遺傳算法將點(diǎn)云轉(zhuǎn)化為三角網(wǎng)搜尋最近點(diǎn)的方法實(shí)現(xiàn)點(diǎn)云配準(zhǔn)，具有較高的配準(zhǔn)效率，但配準(zhǔn)精度有待提升。王暢等[17]使用全局結(jié)構(gòu)特征做初始配準(zhǔn)，之后利用局部結(jié)構(gòu)特征做快速精確配準(zhǔn)算法。該算法提高了配準(zhǔn)速度，但當(dāng)點(diǎn)云數(shù)較少的情況下，可能會使算法配準(zhǔn)結(jié)果不理想。趙敏等[18]提出了一種基于lp空間力學(xué)模型的點(diǎn)云配準(zhǔn)算法，其中：l表示線性賦范空間，p表示空間維度。該算法收斂速度快，但當(dāng)物體形狀結(jié)構(gòu)接近對稱時(shí)，會降低配準(zhǔn)精度。陳旭等[19]提出一種基于校正點(diǎn)云主成分方向的線性計(jì)算方法，配合ICP算法實(shí)現(xiàn)高效精確的點(diǎn)云全局配準(zhǔn)，但此算法在點(diǎn)云分布不均勻或有缺陷時(shí)結(jié)果可能有所偏差。傳統(tǒng)ICP算法的時(shí)間復(fù)雜度為O(mdnd)，m、n分別為d維空間里點(diǎn)的數(shù)量，即ICP算法的計(jì)算效率受點(diǎn)集數(shù)量和空間維數(shù)的影響[20]。

本文主要針對ICP算法配準(zhǔn)效率問題進(jìn)行研究，提出一種易于實(shí)現(xiàn)且魯棒性高的多分辨率ICP配準(zhǔn)算法。首先使用本文提出的自適應(yīng)體素網(wǎng)格濾波器對點(diǎn)云進(jìn)行多分辨率采樣，然后使用提出的多分辨率思想，從低分辨率到高分辨率進(jìn)行配準(zhǔn)，利用低分辨率點(diǎn)云進(jìn)行快速初始配準(zhǔn)，再利用高分辨率點(diǎn)云提高配準(zhǔn)精度，完成粗略到精細(xì)的匹配。實(shí)驗(yàn)結(jié)果表明，本文算法簡單可行，改善了ICP算法計(jì)算效率低的問題。

1 傳統(tǒng)ICP算法

由Besl和McKay所提出的傳統(tǒng)ICP算法，是一種高效處理兩個(gè)點(diǎn)云間配準(zhǔn)的方法。通常，剛性配準(zhǔn)是確定兩個(gè)點(diǎn)云的點(diǎn)集之間的剛性變換的過程，可以匹配對應(yīng)點(diǎn)。給定兩組3D點(diǎn)云：待配準(zhǔn)點(diǎn)云P={p(1),p(2),…,p(n)}和目標(biāo)點(diǎn)云Q={q(1),q(2),…,q(m)}，其中n和m分別表示兩組點(diǎn)云中點(diǎn)的數(shù)量。ICP算法的目標(biāo)就是找到旋轉(zhuǎn)變換矩陣R和平移變換矩陣t，使得?i,q(i)=Rp(i)+t。然而由于實(shí)際配準(zhǔn)過程中，存在噪聲、誤差、拍攝角度等問題的影響導(dǎo)致上式不會完美成立，即不存在一個(gè)R和t可以使所有點(diǎn)滿足上式。因此ICP算法的目標(biāo)轉(zhuǎn)換為求旋轉(zhuǎn)R和平移t，使兩點(diǎn)云間對應(yīng)點(diǎn)距離之和最小，目標(biāo)函數(shù)f(R,t)表示為：

(1)

ICP算法以迭代的方式尋找最優(yōu)值，其基本步驟如下：

1) 尋找待配準(zhǔn)點(diǎn)云在目標(biāo)點(diǎn)云中的對應(yīng)點(diǎn)。根據(jù)待配準(zhǔn)點(diǎn)云P的點(diǎn)坐標(biāo)，在目標(biāo)點(diǎn)云Q中搜索相應(yīng)的最近點(diǎn)點(diǎn)集。

2) 根據(jù)點(diǎn)云匹配點(diǎn)對，求使對應(yīng)點(diǎn)間平均距離最小的剛體變換參數(shù)。k對點(diǎn)可以得到k個(gè)方程組，使用最小二乘法求解出方程中的參數(shù)R和t。

3) 應(yīng)用變換并計(jì)算新的對應(yīng)點(diǎn)間距離。對點(diǎn)云P中每一個(gè)點(diǎn)應(yīng)用轉(zhuǎn)換公式Pj=RPj+t，其中下標(biāo)j表示第j輪迭代。之后重新計(jì)算新的對應(yīng)點(diǎn)間距離Dj：

(2)

4) 判斷是否進(jìn)行下一輪迭代。設(shè)定精度閾值σ和迭代最大次數(shù)N，若步驟3中求出的新的對應(yīng)點(diǎn)間距離Dj小于閾值σ或迭代次數(shù)超過N，則停止迭代，否則重復(fù)步驟1-步驟3繼續(xù)迭代。

在ICP算法配準(zhǔn)過程中，兩個(gè)點(diǎn)云里點(diǎn)的數(shù)量和迭代次數(shù)對計(jì)算時(shí)間有很大影響。傳統(tǒng)ICP算法中將所有點(diǎn)都納入到計(jì)算過程中，嚴(yán)重影響了配準(zhǔn)效率。本文提出一種基于多分辨率配準(zhǔn)的ICP算法，以改善配準(zhǔn)效率低的問題。

2 多分辨率配準(zhǔn)的ICP算法

2.1 體素網(wǎng)格濾波器

對點(diǎn)云進(jìn)行多分辨率采樣時(shí)，應(yīng)盡量保持原點(diǎn)云的曲面特征以及點(diǎn)的密度分布。若使用隨機(jī)采樣法，點(diǎn)云的輪廓信息將被削弱，并且會造成點(diǎn)分布不均勻。使用體素網(wǎng)格濾波器對原點(diǎn)云進(jìn)行采樣處理，該方法可在減少點(diǎn)云里點(diǎn)的數(shù)量的同時(shí)，保持點(diǎn)云原來形態(tài)特征，但是采樣時(shí)體素立方體的邊長需要人為指定。因此，本文提出自適應(yīng)體素網(wǎng)格濾波器，根據(jù)點(diǎn)云中包含的點(diǎn)數(shù)自動計(jì)算體素網(wǎng)格的邊長，可對不同規(guī)模的點(diǎn)云自動做出適合的多分辨率采樣。

2.1.1傳統(tǒng)體素網(wǎng)格濾波器

體素網(wǎng)格濾波器的基本原理是：將原始輸入點(diǎn)云分成多個(gè)相同大小的立方體，立方體的大小可根據(jù)實(shí)際需求進(jìn)行調(diào)整。然后計(jì)算每個(gè)立方體中所有點(diǎn)的重心，以重心替代這個(gè)立方體中的所有點(diǎn)。具體計(jì)算方法如下所示：

1) 確定立方體邊長K，將點(diǎn)云按順序分成多個(gè)大小為K×K×K的立方體。K值越大，采樣后的點(diǎn)云的分辨率越低，反之，則分辨率越大。采樣立方體個(gè)數(shù)為A×B×C，計(jì)算方法如下：

(3)

式中：xmax、xmin、ymax、ymin、zmax、zmin分別為X、Y、Z三個(gè)坐標(biāo)軸上點(diǎn)坐標(biāo)的最大值、最小值。

2) 計(jì)算每個(gè)立方體包含點(diǎn)的重心(x′,y′,z′)，計(jì)算方法如下：

(4)

式中：m為該立方體中點(diǎn)的數(shù)目。

3) 以重心代替該立方體中所有點(diǎn)，并將所有立方體的重心重新合成為新的點(diǎn)云。

2.1.2自適應(yīng)體素網(wǎng)格濾波器

針對不同點(diǎn)數(shù)的點(diǎn)云，本文提出的體素網(wǎng)格濾波函數(shù)中立方體的邊長自適應(yīng)更新策略。

立方體邊長K的計(jì)算如下：

(5)

式中：l表示第l層濾波，l=0時(shí)為原始分辨率；Dori為原始分辨率點(diǎn)間平均距離。其計(jì)算如下：

(6)

式中：N為點(diǎn)云里點(diǎn)的總數(shù)；Hi為點(diǎn)云中第i個(gè)點(diǎn)；Hdi為距離Hi最近的點(diǎn)。

式(5)中β(s)為自適應(yīng)比例因子，控制體素網(wǎng)格立方體邊長增長率，定義為：

(7)

2.2 改進(jìn)的ICP算法

2.2.1整體思路

本文在ICP配準(zhǔn)算法中引入多分辨率的思想。在ICP算法前幾輪迭代的初始配準(zhǔn)中，首先使用較低分辨率的點(diǎn)云，快速獲得兩個(gè)點(diǎn)云之間的初始變換，提高匹配效率。然后使用初始變換的結(jié)果，以更高的分辨率實(shí)現(xiàn)更精確的配準(zhǔn)。不同分辨率中點(diǎn)云的形狀基本保持一致，只有點(diǎn)云中點(diǎn)的數(shù)量不同，因此較低分辨率的配準(zhǔn)結(jié)果，可作為高分辨率配準(zhǔn)的初值。點(diǎn)云里點(diǎn)的數(shù)量越少，算法的計(jì)算速度越快，同時(shí)精度會有所下降；點(diǎn)的數(shù)量越多，匹配的精度會提升，但計(jì)算耗時(shí)會大幅增加。使用較低分辨率的兩個(gè)點(diǎn)云獲得的配準(zhǔn)結(jié)果，比用原始分辨率獲得相同精度的配準(zhǔn)結(jié)果的耗時(shí)顯著減少。該算法的優(yōu)點(diǎn)是使用快速低分辨率配準(zhǔn)代替高分辨率配準(zhǔn)的初始迭代過程，減少了高分辨率下的配準(zhǔn)迭代次數(shù)，從而提高計(jì)算效率。所提出的快速多分辨率ICP算法流程圖如圖1所示。

圖1 本文算法流程圖

2.2.2詳細(xì)流程

本文提出的多分辨率配準(zhǔn)詳細(xì)流程如下：

1) 使用提出的自適應(yīng)體素網(wǎng)格濾波器對兩點(diǎn)集P、Q進(jìn)行多次不同分辨率采樣。

Hl=(Hl-1)l=1,2,…,L

(8)

(9)

(10)

3) 重復(fù)步驟2)直到dl不再減小為止。

4) 令l=l-1提升點(diǎn)云分辨率，得到Pl-1和Ql-1。

5) 重復(fù)步驟2)-步驟4)，直到l=0或均方根誤差(RMS)達(dá)到設(shè)定閾值為止。RMS計(jì)算方法如下：

(11)

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

為了驗(yàn)證所提出的方法的魯棒性和計(jì)算效率，分別與傳統(tǒng)ICP算法、文獻(xiàn)[14]、文獻(xiàn)[15]、文獻(xiàn)[18]和文獻(xiàn)[19]改進(jìn)的ICP算法的配準(zhǔn)時(shí)間和精度進(jìn)行了仿真和比較。實(shí)驗(yàn)使用斯坦福大學(xué)開放點(diǎn)云數(shù)據(jù)庫的兔和龍模型進(jìn)行對比試驗(yàn)。實(shí)驗(yàn)平臺為Ubuntu 16.04系統(tǒng)，CLion 2016，運(yùn)行在配置為Core i7-8700K CPU 3.8 GHz，16 GB RAM的臺式計(jì)算機(jī)上。

實(shí)驗(yàn)中待配準(zhǔn)點(diǎn)云模型初始位置如圖2所示，白色為目標(biāo)點(diǎn)云、淺灰色為待配準(zhǔn)點(diǎn)云。其中兔模型點(diǎn)云中點(diǎn)的數(shù)量為35 947，龍模型點(diǎn)的數(shù)量為437 645。

圖3和圖4分別為兔和龍模型在不同分辨率下的點(diǎn)云模型。采樣后點(diǎn)云最低包含點(diǎn)數(shù)設(shè)為1 000。由式⑸計(jì)算可得，兔模型點(diǎn)云下采樣計(jì)算1次，體素網(wǎng)格邊長K=0.005；龍模型點(diǎn)云下采樣計(jì)算2次，體素網(wǎng)格邊長K分別為0.001和0.005。表1為兔和龍模型在不同分辨率下的采樣點(diǎn)數(shù)和采樣耗時(shí)。可以看出使用體素網(wǎng)格濾波器采樣得到的不同分辨率的點(diǎn)云模型在降低點(diǎn)云點(diǎn)數(shù)后，仍保留了原始點(diǎn)云的形態(tài)特征，且由多分辨率采樣帶來的計(jì)算耗時(shí)極短。

(a) 兔模型 (b) 龍模型圖2 待配準(zhǔn)點(diǎn)云

(a) K=0.005 (b) 原始分辨率圖3 兔模型多分辨率采樣結(jié)果

(a) K=0.005 (b) K=0.001

(c) 原始分辨率圖4 龍模型多分辨率采樣結(jié)果

表1 多分辨率采樣計(jì)算耗時(shí)

圖5為兔和龍模型配準(zhǔn)實(shí)驗(yàn)結(jié)果。可以看出傳統(tǒng)ICP、文獻(xiàn)[15]、文獻(xiàn)[18]、文獻(xiàn)[19]和本文算法都可以實(shí)現(xiàn)點(diǎn)云的精確配準(zhǔn)，配準(zhǔn)結(jié)果基本一致，從視覺上都可以認(rèn)為點(diǎn)云正確配準(zhǔn)；文獻(xiàn)[14]算法效果略差，出現(xiàn)了輕微重影的現(xiàn)象。

(a) 原始ICP配準(zhǔn)結(jié)果

(b) 文獻(xiàn)[14]配準(zhǔn)結(jié)果

(c) 文獻(xiàn)[15]配準(zhǔn)結(jié)果

(d) 文獻(xiàn)[18]配準(zhǔn)結(jié)果

(e) 文獻(xiàn)[19]配準(zhǔn)結(jié)果

(f) 本文改進(jìn)的ICP配準(zhǔn)結(jié)果圖5 兔子及龍點(diǎn)云模型配準(zhǔn)結(jié)果

表2為四種算法的配準(zhǔn)誤差、迭代次數(shù)和運(yùn)行時(shí)間的比較。配準(zhǔn)誤差(RMS)停止閾值為1×10-4m。該結(jié)果表明在配準(zhǔn)精度基本相同的情況下，本文算法相對于傳統(tǒng)ICP、文獻(xiàn)[14]、文獻(xiàn)[15]和文獻(xiàn)[18]算法大大縮短了配準(zhǔn)時(shí)間。尤其是在龍模型配準(zhǔn)實(shí)驗(yàn)中，由于點(diǎn)數(shù)過多導(dǎo)致傳統(tǒng)ICP算法耗時(shí)成指數(shù)級增長，遠(yuǎn)超配準(zhǔn)時(shí)長所能接受的范圍，文獻(xiàn)[14]、文獻(xiàn)[15]的算法雖比原始ICP算法耗時(shí)降低一半左右，但時(shí)間仍很長。文獻(xiàn)[19]的算法已將耗時(shí)降低至可接受的范圍內(nèi)，但本文改進(jìn)的ICP算法將配準(zhǔn)時(shí)間進(jìn)一步縮短。

表2 算法性能比較

六種算法在兔模型上的配準(zhǔn)過程如圖6所示。可以看出，本文算法配準(zhǔn)耗時(shí)遠(yuǎn)低于傳統(tǒng)ICP算法。在基本相同的配準(zhǔn)精度下，傳統(tǒng)ICP需要近2.4 s；文獻(xiàn)[14]、文獻(xiàn)[15]的算法需近1.5 s；文獻(xiàn)[19]顯著降低配準(zhǔn)時(shí)間；而本文算法可在前幾輪迭代中完成快速初始配準(zhǔn)，在后幾輪迭代中對配準(zhǔn)精度進(jìn)一步提高，總時(shí)長控制在0.5 s內(nèi)，耗時(shí)降低為原始ICP算法的20%左右，且比文獻(xiàn)[19]算法耗時(shí)更少。

圖6 兔模型配準(zhǔn)時(shí)間比較

為進(jìn)一步驗(yàn)證本文算法在效率上的提升，選取了另外5組不同點(diǎn)數(shù)的點(diǎn)云進(jìn)行配準(zhǔn)實(shí)驗(yàn)。配準(zhǔn)誤差(RMS)停止閾值仍為1×10-4m，在各算法配準(zhǔn)精度均到達(dá)這個(gè)標(biāo)準(zhǔn)后，將本文算法的配準(zhǔn)耗時(shí)與傳統(tǒng)ICP、文獻(xiàn)[15]和文獻(xiàn)[19]的耗時(shí)進(jìn)行對比，如表3所示。可以看出，本文算法相比傳統(tǒng)ICP算法，配準(zhǔn)耗時(shí)僅為原來的15%及以下；相比文獻(xiàn)[19]的算法，在各個(gè)模型中，本文算法耗時(shí)均低于文獻(xiàn)[19]算法的耗時(shí)，且隨著點(diǎn)云規(guī)模的增大，配準(zhǔn)所用時(shí)間降低的越多。實(shí)驗(yàn)結(jié)果表明，本文提出的改進(jìn)的ICP配準(zhǔn)算法可以快速準(zhǔn)確收斂。

表3 不同規(guī)模點(diǎn)云配準(zhǔn)耗時(shí) s

4 結(jié) 語

本文提出了一種多分辨率改進(jìn)的ICP快速匹配算法用于兩點(diǎn)云間配準(zhǔn)。該算法構(gòu)造了一種用于點(diǎn)云匹配的多分辨率模型，并使用提出的自適應(yīng)體素網(wǎng)格濾波器對點(diǎn)云進(jìn)行多分辨率采樣。可以在低分辨率下快速獲得兩個(gè)點(diǎn)云之間的初始變換，然后使用先前變換以更高分辨率進(jìn)行更精確的配準(zhǔn)。通過這種方法降低了ICP算法在初始配準(zhǔn)過程中需要處理的點(diǎn)數(shù)，從而降低配準(zhǔn)時(shí)間。實(shí)驗(yàn)表明，本文算法提高了ICP算法的配準(zhǔn)效率，且隨點(diǎn)云規(guī)模增加，速度提升幅度遞增趨勢。