改進(jìn)的迭代最近點(diǎn)快速點(diǎn)云拼接算法

崔曉云, 張二鋒

(1.西安郵電大學(xué) 數(shù)字藝術(shù)學(xué)院，陜西 西安 710121； 2.清華大學(xué) 教育研究院，北京 100084)

三維點(diǎn)云技術(shù)[1]是三維場(chǎng)景重建的關(guān)鍵技術(shù)之一，其實(shí)現(xiàn)原理主要是通過(guò)生成點(diǎn)云形態(tài)即通過(guò)對(duì)場(chǎng)景中目標(biāo)物體的點(diǎn)匹配重建，從而完成三維點(diǎn)云重建。現(xiàn)已廣泛應(yīng)用在軍事、工業(yè)和醫(yī)療等多個(gè)鄰域。

迭代最近點(diǎn)( iterative closest point，ICP) 算法[2]是一種點(diǎn)云匹配算法，通過(guò)旋轉(zhuǎn)矩陣及平移向量，根據(jù)有效點(diǎn)映射關(guān)系對(duì)不同坐標(biāo)系下的數(shù)據(jù)點(diǎn)云進(jìn)行坐標(biāo)變換。該算法能夠精準(zhǔn)地完成三維點(diǎn)云配準(zhǔn)，但是對(duì)于迭代輸入值要求極高，其源點(diǎn)云與目標(biāo)點(diǎn)云位置接近且兩片點(diǎn)云之間存在包含關(guān)系。若這些條件沒(méi)有達(dá)到，則會(huì)影響配準(zhǔn)精度及收斂速率。因此，國(guó)內(nèi)外眾多學(xué)者致力于ICP算法的研究及改進(jìn)[3-4]。點(diǎn)云初始配準(zhǔn)的優(yōu)化求解算法[5]基于序列圖像運(yùn)動(dòng)規(guī)律，能以較小的計(jì)算量獲取近似全局最優(yōu)的點(diǎn)云初始配準(zhǔn)結(jié)果，從而實(shí)現(xiàn)點(diǎn)云配準(zhǔn)；通過(guò)添加帶邊界的尺度矩陣，引入動(dòng)態(tài)因子，保持了算法的尺度不變性[6]；將正態(tài)分布變換算法與ICP算法相結(jié)合，先通過(guò)正態(tài)分布變換算法進(jìn)行初始配準(zhǔn)，然后采用兩次ICP進(jìn)行微調(diào)和細(xì)配準(zhǔn)，提高了算法的配準(zhǔn)精度[7]。但是，上述算法僅提高了點(diǎn)云拼接技術(shù)的配準(zhǔn)精度，未涉及點(diǎn)云彩色信息的匹配。

為了更好的提高配準(zhǔn)精度及收斂精度，本文提出一種改進(jìn)的迭代最近點(diǎn)云快速拼接算法。利用NCAM光學(xué)[8]追蹤彩色與深度信息，獲取原始點(diǎn)云數(shù)據(jù)點(diǎn)；將色彩特征點(diǎn)與隨機(jī)采樣相結(jié)合，進(jìn)行特征點(diǎn)采樣，獲取有效的初始點(diǎn)云數(shù)據(jù)；引入傘曲率體積積分不變量特征點(diǎn)描述算法[9]，避免因復(fù)雜光照環(huán)境影響而生成的大量不可靠信息點(diǎn)，實(shí)現(xiàn)誤匹配點(diǎn)剔除；同時(shí)構(gòu)建帶色彩約束的目標(biāo)函數(shù)，并通過(guò)四元數(shù)法[10]和特征值分解法[11-12]求解最優(yōu)值，實(shí)現(xiàn)坐標(biāo)變換求解。

1 NCAM光學(xué)追蹤系統(tǒng)架構(gòu)

以光學(xué)追蹤定位系統(tǒng)為基礎(chǔ)，利用圖形圖像識(shí)別[13]和光學(xué)技術(shù)，通過(guò)多傳感器攝像頭和定制服務(wù)器，大面積地獲取具備色彩和深度數(shù)據(jù)的三維坐標(biāo)空間的點(diǎn)云信息。系統(tǒng)包括數(shù)據(jù)采集、數(shù)據(jù)處理及視頻合成輸出渲染。NCAM光學(xué)追蹤系統(tǒng)架構(gòu)[8]如圖1所示。

圖1 NCAM光學(xué)追蹤系統(tǒng)架構(gòu)

將攝像機(jī)里具有“跟蹤功能”的深度攝像機(jī)NcamLive配合傳感器Camera Bar雙捕捉攝像頭，實(shí)時(shí)光學(xué)追蹤并獲取待采集的信息數(shù)據(jù)，從而確定此目標(biāo)圖像的確切位置以及轉(zhuǎn)動(dòng)信息。該攝像頭實(shí)時(shí)追蹤獲取環(huán)境信息、攝像機(jī)鏡頭參數(shù)Zoom及Focus等數(shù)據(jù)，統(tǒng)一通過(guò)數(shù)據(jù)接口盒，以Cat6網(wǎng)線的級(jí)聯(lián)方式輸送至虛擬跟蹤服務(wù)器，在服務(wù)器完成整個(gè)點(diǎn)云數(shù)據(jù)信息點(diǎn)的拼接，最后將虛擬跟蹤服務(wù)器計(jì)算得出的跟蹤數(shù)據(jù)按照Free D協(xié)議編碼，輸出至ORAD渲染服務(wù)器完成視頻合成顯示。

2 改進(jìn)的ICP點(diǎn)云拼接算法

ICP算法主要包括原始點(diǎn)云數(shù)據(jù)獲取、初始對(duì)應(yīng)點(diǎn)云集的確定、誤匹配點(diǎn)剔除和坐標(biāo)變換求解等4個(gè)部分。具體實(shí)現(xiàn)步驟如下。

步驟1 原始點(diǎn)云數(shù)據(jù)獲取。在保留物體表面關(guān)鍵信息點(diǎn)的基礎(chǔ)上，減少噪聲點(diǎn)，完成數(shù)據(jù)采樣。

步驟2 初始對(duì)應(yīng)點(diǎn)云集的確定。在源點(diǎn)云集P中選取第i個(gè)點(diǎn)的空間三維向量Pi，i=1,2,…,np，np為提取的源點(diǎn)云集P的點(diǎn)數(shù)。在目標(biāo)點(diǎn)云集Q中找到Pi最近鄰點(diǎn)Qj，j=1,2,…,nq，nq為提取的目標(biāo)點(diǎn)云集Q的點(diǎn)數(shù)。

步驟3 誤匹配點(diǎn)剔除。引入評(píng)價(jià)標(biāo)準(zhǔn)、約束條件去除不可靠數(shù)據(jù)點(diǎn)，完成誤匹配點(diǎn)剔除。

步驟4 坐標(biāo)變換求解。利用最小二乘法迭代求解，獲取坐標(biāo)變換。

在源點(diǎn)云數(shù)據(jù)與待配準(zhǔn)的點(diǎn)云數(shù)據(jù)之間尋找距離最近點(diǎn)是整個(gè)ICP算法的關(guān)鍵環(huán)節(jié)。利用最小二乘法，在點(diǎn)云重疊區(qū)域獲取最近點(diǎn)的時(shí)間復(fù)雜度O(nplognq)高，因此，按照配準(zhǔn)點(diǎn)色調(diào)h相似性原理，對(duì)ICP算法進(jìn)行改進(jìn)。

改進(jìn)的ICP點(diǎn)云拼接算法結(jié)構(gòu)如圖2示。

圖2 改進(jìn)的ICP點(diǎn)云拼接算法結(jié)構(gòu)

利用NCAM光學(xué)追蹤彩色與深度信息，并建立彩色點(diǎn)云模型；將色彩特征點(diǎn)與隨機(jī)采樣方法相結(jié)合，進(jìn)行特征點(diǎn)采樣對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行劃分；引入傘曲率體積積分不變量特征點(diǎn)描述算法，剔除誤配準(zhǔn)點(diǎn)，利用k-d樹(shù)鄰域搜索法[14]建立空間索引結(jié)構(gòu)，縮小最近點(diǎn)搜索范圍，以此構(gòu)建約束函數(shù)，依據(jù)均方根誤差[15]對(duì)配準(zhǔn)誤差進(jìn)行評(píng)價(jià)，保證對(duì)應(yīng)點(diǎn)對(duì)的查找準(zhǔn)確性，實(shí)現(xiàn)點(diǎn)云配準(zhǔn)。

2.1 數(shù)據(jù)采集

將深度攝像機(jī)的深度距離范圍限制在1 300～3 500 mm之間，此范圍內(nèi)的像素值設(shè)為1，除此之外的均設(shè)置為0。Camera Bar捕捉攝像頭的鏡頭曝光設(shè)置為8 500～9 000，gamma設(shè)置為0.5左右，以60幀/s頻的采樣率，利用NCAM光學(xué)追蹤系統(tǒng)實(shí)時(shí)采集待拼接的三維點(diǎn)云。

2.2 彩色點(diǎn)云模型構(gòu)建

通過(guò)NCAM獲取的彩色點(diǎn)云信息，不但包含空間位姿信息，還包含三維點(diǎn)云信息紅、黃、藍(lán)三通道的r、g、b像素值，取值范圍均為[0,255]。考慮到目標(biāo)信息點(diǎn)受實(shí)際復(fù)雜環(huán)境燈光的影響，r、g、b值誤差較大，而色調(diào)h受燈光影響較小，特用每個(gè)點(diǎn)的歸一化色調(diào)值[16]代表色彩信息，以此構(gòu)建彩色點(diǎn)云模型，從而實(shí)現(xiàn)對(duì)輸入值的優(yōu)化處理。色調(diào)h的計(jì)算表達(dá)式為

(1)

其中R、G、B分別為r、g、b值除以像素總和得到的歸一化色彩分量值，取值范圍均為[0，1]。點(diǎn)云信息RGB值歸一化如圖3所示。

圖3 點(diǎn)云信息RGB值歸一化

在實(shí)現(xiàn)源點(diǎn)云集P和目標(biāo)點(diǎn)云集Q的拼接過(guò)程中，對(duì)不同視角下的兩片點(diǎn)云RGB值根據(jù)式(1)進(jìn)行歸一化處理，得到源點(diǎn)云集合

(2)

目標(biāo)點(diǎn)云集合

(3)

2.3 特征點(diǎn)采樣

在覆蓋整個(gè)彩色點(diǎn)云模型的基礎(chǔ)上，為了避免因局部色調(diào)比重較大，導(dǎo)致隨機(jī)采樣過(guò)程中大部分的采樣點(diǎn)相同或者色調(diào)較為相近的問(wèn)題，利用色彩特征點(diǎn)與隨機(jī)采樣相結(jié)合的方法實(shí)現(xiàn)彩色點(diǎn)云特征點(diǎn)采樣，保證采樣空間及色彩的均勻分布同時(shí)能夠覆蓋整個(gè)模型。具體操作步驟如下。

步驟1 將色調(diào)h值從0至1均分為n份，將點(diǎn)云集P劃分為n個(gè)子點(diǎn)云集Pk(k=1,2,…,n)，則Pk僅包含滿(mǎn)足色調(diào)h的點(diǎn)，此時(shí)h取值范圍為{(k-1)/n}≤h<(k/n)。

步驟2 將點(diǎn)云集Q劃分為n個(gè)子點(diǎn)云集Qw(w=1,2,…,n)，則子點(diǎn)云集Qw僅包含滿(mǎn)足色調(diào)h的點(diǎn)，此時(shí)h取值范圍為{(w-1.5)/n}≤h≤{(w+0.5)/n}。

以n=4為例，子點(diǎn)云劃分如圖4所示。

圖4 子點(diǎn)云劃分

步驟3 特征點(diǎn)采樣。選擇Pk中的任一點(diǎn)作為特征點(diǎn)，通過(guò)迭代方式，獲取所有有效的特征點(diǎn)并儲(chǔ)存至服務(wù)器。

由圖4可知，源點(diǎn)云集Pk中所有的點(diǎn)僅需要在Qk中尋找，而Qk覆蓋的色調(diào)范圍大于Pk，因此，處于色調(diào)邊緣的所有Pk點(diǎn)均能夠找到與之相匹配的點(diǎn)，從而減少誤差。同時(shí)為保證采樣點(diǎn)空間分布均勻且色彩信息豐富，故而將n值設(shè)置為15，能夠有效避免色彩特征點(diǎn)局部擬合問(wèn)題[17]。

2.4 特征描述

引入傘曲率體積不變量特征點(diǎn)描述算法[18]，利用k-d樹(shù)鄰域搜索法確定待搜索點(diǎn)P的8個(gè)鄰域點(diǎn)，以此特征點(diǎn)求解特征值，完成特征點(diǎn)描述。源點(diǎn)云集P的特征值表達(dá)式為

(4)

其中wk為第k個(gè)鄰域點(diǎn)的權(quán)重，UPk為P的第k個(gè)鄰域點(diǎn)的特征值。通過(guò)各個(gè)鄰域點(diǎn)做出的貢獻(xiàn)設(shè)置權(quán)重，同時(shí)給定閾值，當(dāng)權(quán)重在閾值范圍內(nèi)，則可作為有效特征點(diǎn)，否則剔除該點(diǎn)。同理，可計(jì)算目標(biāo)點(diǎn)云集Q的特征值，完成特征點(diǎn)描述。

除此之外，還將對(duì)因攝像機(jī)硬件及抖動(dòng)而產(chǎn)生的噪聲點(diǎn)和離群點(diǎn)，采用改進(jìn)的Multi-Quadri濾波算法[18]去除點(diǎn)云數(shù)據(jù)中分布不規(guī)則，且在一些區(qū)域發(fā)生隨機(jī)抖動(dòng)的無(wú)效數(shù)據(jù)點(diǎn)來(lái)進(jìn)行噪聲處理。

2.5 目標(biāo)函數(shù)構(gòu)建

點(diǎn)云集P和Q之間滿(mǎn)足三維剛體變換關(guān)系，在第m次迭代過(guò)程中，剛體變化的旋轉(zhuǎn)矩陣記為R(m)，平移矩陣記為T(mén)(m)，以此構(gòu)建帶色彩約束的目標(biāo)函數(shù)

(5)

根據(jù)均方根公式[19]

(6)

假設(shè)色調(diào)分布均勻的情況下，目標(biāo)函數(shù)的計(jì)算復(fù)雜度為O(nplog(nq/n))，而實(shí)際計(jì)算過(guò)程中，色調(diào)不是均勻分布的，除主色調(diào)之外的其它色調(diào)占比較少，與之對(duì)應(yīng)的特征點(diǎn)也較少，故而實(shí)際計(jì)算過(guò)程中算法的復(fù)雜度會(huì)更低。

3 改進(jìn)的ICP算法實(shí)現(xiàn)

改進(jìn)的ICP算法的實(shí)現(xiàn)分為初始配準(zhǔn)和精確配準(zhǔn)兩部分，具體步驟如下。

初始配準(zhǔn)

步驟1 通過(guò)NCAM光學(xué)追蹤獲取彩色與深度點(diǎn)云信息。

步驟2 對(duì)獲取的點(diǎn)云信息，構(gòu)建彩色點(diǎn)云模型。

步驟3 依據(jù)式(1)對(duì)r、g、b像素值進(jìn)行歸一化處理，得到式(2)和式(3)，即源點(diǎn)云集合P和目標(biāo)點(diǎn)云集合Q。

步驟4 根據(jù)旋轉(zhuǎn)平移位姿關(guān)系，完成初始配準(zhǔn)。

精確配準(zhǔn)

步驟5 對(duì)P和Q利用色彩特征點(diǎn)與隨機(jī)采樣相結(jié)合的方法獲取有效子點(diǎn)云集Pk和Qw。

步驟6 為點(diǎn)云集Pk和Qw建立k-d數(shù)據(jù)結(jié)構(gòu)并進(jìn)行鄰近搜索，根據(jù)式(4)計(jì)算點(diǎn)云集Pk和Qw的特征值，完成特征點(diǎn)描述。

步驟7 在Qw中尋找與源點(diǎn)子云集Pk色調(diào)一致的子點(diǎn)云集Qk。

步驟8 根據(jù)式子(5)對(duì)點(diǎn)云集P和Q構(gòu)建帶色彩約束的目標(biāo)函數(shù)E(m)，并利用四元數(shù)法和特征值分解法獲取R(m)和T(m)的最優(yōu)值。

改進(jìn)的ICP算法拼接流程如圖5。

圖5 改進(jìn)的ICP算法拼接流程

4 實(shí)驗(yàn)設(shè)計(jì)及結(jié)果分析

為了驗(yàn)證本文算法的時(shí)效性及精確性，搭建此次實(shí)驗(yàn)場(chǎng)景。實(shí)驗(yàn)所需的服務(wù)器配置操作系統(tǒng)為Windows 10，主頻2.60 GHz，CPU為Intel Core i7，內(nèi)存為8GB；程序開(kāi)發(fā)引擎Unreal Engine 4.20.3，開(kāi)發(fā)軟件為Visual Studio 2008和Matlab，開(kāi)源庫(kù)包含PCL點(diǎn)云庫(kù)和OpenGL等；攝像機(jī)選取索尼FS4(含鏡頭)。

4.1 改進(jìn)的ICP算法拼接效果

利用Matlab將彩色圖像生成兩組點(diǎn)云數(shù)據(jù)，通過(guò)改進(jìn)的ICP算法實(shí)現(xiàn)拼接。待拼接的P與Q點(diǎn)云數(shù)據(jù)模型及初始位姿如圖6所示。

圖6 兩組數(shù)據(jù)及初始位姿

從圖6可以看出，兩組數(shù)據(jù)的初始位姿無(wú)明顯對(duì)應(yīng)關(guān)系，此時(shí)，無(wú)法直接利用ICP算法進(jìn)行精確配準(zhǔn)。為了確保精準(zhǔn)匹配，依據(jù)圖6所表示的旋轉(zhuǎn)平移映射關(guān)系，對(duì)兩組數(shù)據(jù)進(jìn)行初始配準(zhǔn)，如圖7所示。

圖7 初始配準(zhǔn)后的位姿

依據(jù)改進(jìn)的ICP算法，對(duì)每個(gè)點(diǎn)的歸一化色調(diào)值處理，建立點(diǎn)云k維數(shù)據(jù)空間的數(shù)據(jù)結(jié)構(gòu)，搜過(guò)最近鄰點(diǎn)，構(gòu)建約束函數(shù)，從而求解最優(yōu)值，獲取得到精確配準(zhǔn)的輸入初始位姿如圖8所示。

圖8 初始位姿

在精確配準(zhǔn)的過(guò)程中，將迭代次數(shù)設(shè)置為80 s，同時(shí)考慮到不同的追蹤目標(biāo)其構(gòu)建的函數(shù)不同，故在每運(yùn)行10 s時(shí)，記錄下當(dāng)前迭代次數(shù)及函數(shù)的最優(yōu)解。配準(zhǔn)結(jié)果如圖9所示。

圖9 配準(zhǔn)后的位姿

從圖9可以看出，改進(jìn)算法的拼接效果較好，鼻子、身體、足部輪廓顯著，但是尾部輪廓效果有所偏差。這是因?yàn)楸亲?、身體、足部特征點(diǎn)分布集中，無(wú)明顯差異；而尾部特征點(diǎn)分布較少，因而差異較大。

在精確配準(zhǔn)的過(guò)程中，仍然會(huì)有大部分噪聲會(huì)顯現(xiàn)出來(lái)，但是，改進(jìn)的ICP算法加入了色彩約束條件可有效的剔除了大部分噪聲，并且提高收斂速率及拼接精度。

4.2 拼接精度及收斂速率對(duì)比分析

依據(jù)均方根誤差分別對(duì)比ICP算法和改進(jìn)的ICP算法的配準(zhǔn)誤差，利用迭代時(shí)間對(duì)配準(zhǔn)收斂速率進(jìn)行評(píng)價(jià)，實(shí)驗(yàn)結(jié)果分別如圖10和圖11所示。

圖10 迭代精度曲線

圖11 迭代時(shí)間曲線

從圖10和圖11分別可見(jiàn)，ICP算法在38次的迭代，耗時(shí)28.98 s的情況下，RMS值為0.039 1；改進(jìn)算法在18次迭代、耗時(shí)5.78 s的情況下，RMS可達(dá)到0.033 9，表明改進(jìn)算法受周?chē)h(huán)境及人為因素影響較小，程序可自動(dòng)進(jìn)行計(jì)算，故而在初始配準(zhǔn)之后，仍然能夠達(dá)到較高的配準(zhǔn)精度；在完成初始配準(zhǔn)之后將結(jié)果作為輸入值進(jìn)行細(xì)配準(zhǔn)，加入約束條件快速的去除色調(diào)不一致的點(diǎn)對(duì)，能夠進(jìn)一步加快其迭代收斂速率，提高運(yùn)算速率及匹配精度。

5 結(jié)語(yǔ)

改進(jìn)的ICP點(diǎn)云快速拼接算法，基于NCAM光學(xué)追蹤彩色與深度信息，根據(jù)彩色與深度點(diǎn)云信息匹配點(diǎn)色調(diào)相近性，利用k-d樹(shù)鄰域搜索法加速對(duì)應(yīng)點(diǎn)對(duì)的匹配，以點(diǎn)到切平面距離作為迭代的目標(biāo)，建立目標(biāo)約束函數(shù)，最后利用特征值分解法最優(yōu)化算法對(duì)目標(biāo)函數(shù)進(jìn)行求解。實(shí)驗(yàn)結(jié)果表明，與ICP算法相比，改進(jìn)算法的拼接匹配率高，同時(shí)迭代精度明顯提升，迭代時(shí)間大幅縮短，可較好地提高拼接質(zhì)量。