摘  要： 在三維激光點(diǎn)云數(shù)據(jù)配準(zhǔn)的過(guò)程中,，利用傳統(tǒng)Iterative Closest Point（ICP）算法搜索對(duì)應(yīng)點(diǎn)對(duì)時(shí)速度慢,，而且配準(zhǔn)精細(xì)化程度低，遠(yuǎn)達(dá)不到三維建模后期處理的要求,。針對(duì)這一問(wèn)題,，提出一種基于KDTree改進(jìn)的ICP算法以實(shí)現(xiàn)激光點(diǎn)云數(shù)據(jù)的快速精細(xì)化配準(zhǔn)。通過(guò)實(shí)驗(yàn)驗(yàn)證算法的有效性和合理性,，為后期模型重建過(guò)程中的三角網(wǎng)格化,、曲面化、紋理映射提供強(qiáng)有力的理論和實(shí)踐基礎(chǔ),。

　　關(guān)鍵詞： 激光點(diǎn)云,；ICP算法；KDTree,；曲面化

0 引言

　　在地面三維激光掃描過(guò)程中,，受物體尺寸、物體間的遮蔽以及掃描儀視場(chǎng)角等因素的影響,，每站掃描只能獲得本站掃描儀坐標(biāo)系下的點(diǎn)云數(shù)據(jù),。對(duì)于大型立體模型而言，大多數(shù)情況下不太可能只通過(guò)一次掃描就獲得全部的物體表面坐標(biāo)及屬性數(shù)據(jù)。因此,，為了獲得完整的物體表面坐標(biāo)及屬性數(shù)據(jù),，必須從不同的視角來(lái)掃描場(chǎng)景。在點(diǎn)云數(shù)據(jù)處理階段,，點(diǎn)云配準(zhǔn)是十分關(guān)鍵的問(wèn)題之一,，配準(zhǔn)的精細(xì)化程度直接影響后續(xù)操作。

1 掃描設(shè)備及作業(yè)流程

　　對(duì)物體進(jìn)行點(diǎn)云數(shù)據(jù)采集時(shí),，采用的三維激光掃描儀是ScanStation C10全站式三維激光掃描儀,。掃描參數(shù)設(shè)置情況如下：全景掃描，掃描視角為360°×270°,，掃描速度為  50 000點(diǎn)/s,，掃描距離為300 m，點(diǎn)位標(biāo)稱精度為±2 mm,。

　　設(shè)置好參數(shù)以后,，分別在兩位置坐標(biāo)系下對(duì)只有4個(gè)面的長(zhǎng)方體實(shí)物進(jìn)行掃描采集。然后,，依次進(jìn)行點(diǎn)云數(shù)據(jù)的去噪,、稀疏采樣，由此獲得能足夠表達(dá)物體模型的點(diǎn)云數(shù)據(jù),。

　　圖1為使用ScanStation C10掃描儀的作業(yè)流程,，圖2為經(jīng)過(guò)去噪采樣處理過(guò)的數(shù)據(jù)并可視化的結(jié)果。

2 配準(zhǔn)定義

　　點(diǎn)云配準(zhǔn)簡(jiǎn)單來(lái)說(shuō)就是將從多個(gè)站點(diǎn)獲得的點(diǎn)云數(shù)據(jù)進(jìn)行拼接,，得到一個(gè)統(tǒng)一坐標(biāo)系下的三維數(shù)據(jù)點(diǎn)集,。它類似于數(shù)學(xué)上的映射問(wèn)題，也就是說(shuō)要先找到兩個(gè)點(diǎn)云數(shù)據(jù)集間的對(duì)應(yīng)關(guān)系,，然后將一個(gè)坐標(biāo)系下的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到另一個(gè)坐標(biāo)系下,。

　　配準(zhǔn)過(guò)程主要有以下兩個(gè)步驟：（1）尋找對(duì)應(yīng)關(guān)系；（2）解算變換參數(shù),。即首先確定同名點(diǎn)對(duì),，然后解算旋轉(zhuǎn)矩陣R和平移矩陣T。

　　同名點(diǎn)對(duì)：同一個(gè)點(diǎn)在不同坐標(biāo)系下的表達(dá),。

　　圖3所示為兩站掃描示意圖，在A,、B兩處分別安放掃描儀對(duì)同一個(gè)物體進(jìn)行掃描,。在A處獲得坐標(biāo)O1-x1y1z1下的點(diǎn)云數(shù)據(jù)M，在B處獲得坐標(biāo)系O2-x1y1z1下的點(diǎn)云數(shù)據(jù)N,，配準(zhǔn)的目的就是將兩個(gè)坐標(biāo)系O1-x1y1z1,、O2-x1y1z1下的點(diǎn)云數(shù)據(jù)M和N轉(zhuǎn)換到同一個(gè)坐標(biāo)系下。

　　對(duì)于從兩站采集到的點(diǎn)云集合M和N，Mi（X,，Y,，Z），Ni（x,，y,，z），且Mi,、Ni為在不同坐標(biāo)系下的同一點(diǎn),，嚴(yán)格來(lái)說(shuō)，點(diǎn)云配準(zhǔn)就是將全部來(lái)自兩個(gè)不同坐標(biāo)系下的同名點(diǎn)對(duì)（Mi,，Ni）滿足剛體變換（R,，T），即：

　　其中,，R為旋轉(zhuǎn)矩陣,，T為平移矩陣，α,、β,、γ表示沿X、Y,、Z軸的旋轉(zhuǎn)角,，tx、ty,、tz表示位移量,。

　　式（1）稱作空間相似變換公式，它是點(diǎn)云配準(zhǔn)的基本公式,。由式（1）可解出同名點(diǎn)轉(zhuǎn)換參數(shù),，而后進(jìn)行點(diǎn)云數(shù)據(jù)配準(zhǔn)。

3 點(diǎn)云配準(zhǔn)算法

　　目前,，點(diǎn)云配準(zhǔn)算法依據(jù)其采用的配準(zhǔn)基元可將其分為無(wú)特征的配準(zhǔn)和基于特征的配準(zhǔn)[1]兩大類,。

　　基于特征的配準(zhǔn)是指利用角點(diǎn)、邊緣,、面等幾何特征[2]來(lái)解算變化參數(shù),。這類算法主要有以下幾種：基于控制點(diǎn)的配準(zhǔn)算法[3]、基于線特征的配準(zhǔn)算法[4]以及基于曲率[5]的點(diǎn)云配準(zhǔn)算法,。

　　無(wú)特征的配準(zhǔn)就是直接利用原始數(shù)據(jù)進(jìn)行配準(zhǔn),。此類算法中最為著名的是ICP（Iterative Closest Point）算法[6]，但該算法只適用于存在明確對(duì)應(yīng)關(guān)系的點(diǎn)集,，并且計(jì)算速度慢,。為此,，在其他傳統(tǒng)ICP算法[7]的基礎(chǔ)之上，提出基于KDTree[8]的改進(jìn)ICP算法,，包括基于KDTree搜索對(duì)應(yīng)點(diǎn)對(duì)和矩陣變換參數(shù)的計(jì)算兩方面的內(nèi)容,。

　　3.1 傳統(tǒng)ICP配準(zhǔn)算法

　　基本思路：在對(duì)應(yīng)點(diǎn)云中搜尋最鄰近點(diǎn)對(duì)，利用此最鄰近點(diǎn)對(duì)求解剛體變換參數(shù)R,、T,，在這個(gè)過(guò)程中點(diǎn)對(duì)的搜尋和變換參數(shù)的求解都是迭代計(jì)算的。

　　算法步驟如下：

　?。?）令Ω為點(diǎn)云M和N的重疊域,，設(shè)在Ω然數(shù)集N及其擴(kuò)展情況，如正整數(shù)集Z+,、n維實(shí)坐標(biāo)中的任一點(diǎn)對(duì)應(yīng)在M和N上的位置分別是Mi,、Ni，初始迭代時(shí)兩個(gè)點(diǎn)集的初始變換參數(shù)是R0,，T0,。

　　（2）點(diǎn)集M中的每個(gè)點(diǎn)Mi,，由初始變換參數(shù)最小為標(biāo)準(zhǔn),，求出新的變換參數(shù)R、T,。

　?。?）根據(jù)找到的全部最近點(diǎn)對(duì)（mi，ni）,，求出兩個(gè)點(diǎn)集的變換參數(shù)R,、T，并且以全部點(diǎn)對(duì)距離的平方和最小為標(biāo)準(zhǔn),，求出新的變換參數(shù)R,、T。

　?。?）在相鄰兩次計(jì)算所得的距離平方和的差值小于給定的閾值時(shí)結(jié)束迭代,，否則重復(fù)步驟（2）和（3）直至小于給定的閾值。

　?。?）根據(jù)最終得到的R,、T將點(diǎn)云M映射變換到點(diǎn)云N的坐標(biāo)系下，完成配準(zhǔn),。

　　3.2 改進(jìn)的基于KDTree的ICP算法

　　3.2.1 算法準(zhǔn)備工作

　　由KDTree的算法原理可知,，當(dāng)鄰域點(diǎn)集中點(diǎn)數(shù)k為1時(shí)，搜尋點(diǎn)與鄰域點(diǎn)間建立一一映射關(guān)系,。此時(shí)，搜索到的鄰域點(diǎn)是搜尋點(diǎn)與鄰域點(diǎn)集中距離最小的點(diǎn)。

　　該算法中要用到的變換矩陣?yán)盟脑胤╗9]求解,，過(guò)程如下：

　?。?）求解點(diǎn)集M、N的重心坐標(biāo)O1,、O2,。

　　（2）點(diǎn)集M,、N的重心化：

　?。?）構(gòu)建矩陣Q：

　　（4）求解Q的最大特征值以及最大特征值對(duì)應(yīng)的特征向量（w,，m,，n，p）,。

　?。?）構(gòu)造旋轉(zhuǎn)矩陣：

　　（6）解算平移向量T：

　　T=O2-R′O1(4)

　　3.2.2 算法實(shí)現(xiàn)步驟

　?。?）設(shè)點(diǎn)集M,、N的部分區(qū)域分別為目標(biāo)點(diǎn)集M′和參考點(diǎn)集N′。

　?。?）令k=1,，在N′中通過(guò)KDTree加速搜索為M′中的任意點(diǎn)搜索最近鄰域點(diǎn)，由此找出M′中任意一點(diǎn)的映射點(diǎn),，也就是找出M′中點(diǎn)集合Mm={M1m,，M2m，…,，Mnm}在N′上的映射點(diǎn)集Nm={N1m,，N2m，…,，Nnm},，m代表迭代次數(shù)，n代表點(diǎn)個(gè)數(shù),。

　?。?）利用設(shè)置好的最小閾值距離Di，刪除Mm,、Nm中錯(cuò)誤的點(diǎn)對(duì),，并完成Mm、Nm的更新,。

　?。?）利用四元素法計(jì)算Mm,、Nm的變換矩陣R和平移量T。

　?。?）由得到的R,、T變換Mm，得到最新的Mm,。

　?。?）重復(fù)步驟（2）~（5），求出Mm中每一點(diǎn)到Nm中的映射點(diǎn)對(duì),，以及相應(yīng)的R,、T。

　?。?）當(dāng)最后的R,、T滿足配準(zhǔn)后，對(duì)應(yīng)點(diǎn)對(duì)坐標(biāo)間差值的閾值收斂條件|xm-xn|or|ym-yn|or|zm-zn|<ε時(shí),，結(jié)束循環(huán),，匹配成功；如果不滿足收斂條件,，進(jìn)行第m+1次迭代計(jì)算,。

　　算法設(shè)計(jì)流程如圖4所示。

　　3.2.3 主要函數(shù)代碼介紹

　　最小閾值Di設(shè)定函數(shù)：

　　inline void setTransformationEpsilon（double epsilon）{transformation_epsilon_=epsilon,；}

　　坐標(biāo)差閾值設(shè)定函數(shù)：

　　inline void setEuclideanFitnessEpsilon（double epsilon）{euclidean_fitness_epsilon_=epsilon,；}

4 實(shí)驗(yàn)結(jié)果與結(jié)論

　　根據(jù)以上提出的算法，利用斯坦福大學(xué)實(shí)驗(yàn)室在不同坐標(biāo)系下獲得的兔子點(diǎn)云數(shù)據(jù)和實(shí)測(cè)的只有4個(gè)面數(shù)據(jù)的長(zhǎng)方體的點(diǎn)云數(shù)據(jù)進(jìn)行實(shí)驗(yàn),。

　　實(shí)驗(yàn)平臺(tái)為Windows 8.1 64位操作系統(tǒng),，VS2010 32位，PCL點(diǎn)云庫(kù)1.7.1,。

　　如圖5,、圖6所示，左上角和右上角為兩個(gè)不同坐標(biāo)系下的點(diǎn)云數(shù)據(jù),；圖5左下角的右上方為利用傳統(tǒng)ICP算法獲得的實(shí)驗(yàn)結(jié)果,，右下角的右上方為基于KDTree改進(jìn)的ICP算法的實(shí)驗(yàn)結(jié)果；圖6左下角的上方圖為利用傳統(tǒng)ICP算法獲得的實(shí)驗(yàn)結(jié)果,，右下角的上方圖為基于KDTree改進(jìn)的ICP算法的實(shí)驗(yàn)結(jié)果,。

　　由以上比對(duì)可以明顯看出，傳統(tǒng)ICP算法獲得的結(jié)果有著明顯的匹配不到的地方,，而利用改進(jìn)的ICP算法獲得的精細(xì)化匹配結(jié)果趨于完美,，能夠?qū)崿F(xiàn)兩坐標(biāo)系下點(diǎn)云數(shù)據(jù)的精細(xì)化匹配。

　　參考文獻(xiàn)

　　[1] 王蕊,，李俊山,，劉玲霞,，等.基于幾何特征的點(diǎn)云配準(zhǔn)算法[J].華東理工大學(xué)學(xué)報(bào)（自然科學(xué)版），2009,，35（5）：768-773.

　　[2] 鄭德華,，岳東杰，岳建平.基于幾何特征約束的建筑物點(diǎn)云配準(zhǔn)算法[J].測(cè)繪學(xué)報(bào),，2008，37（4）：464-468.

　　[3] 張政.點(diǎn)云數(shù)據(jù)配準(zhǔn)算法研究[D].濟(jì)南：山東大學(xué),，2008.

　　[4] YANG R,， ALLEN P K. Registering， integrating,， and building CAD models from range data[C]. 1998 IEEE International Conference on Robotics and Automation IEEE,， 1998，4：3115-3120.

　　[5] 路銀北,，張蕾,，普杰信，等.基于曲率的點(diǎn)云數(shù)據(jù)配準(zhǔn)算法[J].計(jì)算機(jī)應(yīng)用,，2008,，27（11）：2766-2769.

　　[6] BESL P J， MCKAY N D. Method for registration of 3-D shapes[C]. Robotics-DL Tentative,， International Society for Optics and Photonics,， 1992： 586-606.

　　[7] ZINBER T， SCHMIDT J,， NIEMANN H. A refined ICP algorithm for robust 3-D correspondence estimation[C]. 2003 International Conference on Image Processing,， ICIP 2003， IEEE,， 2003,，3（2）：695-698.

　　[8] Zhang Zhengyou. Iterative point matching for registration of free-form curves and surfaces[J]. International Journal of Computer Vision，1994,，13（2）：119-152.

　　[9] HORN B K P,， HILDEN H M， NEGAHDARIPOUR S. Closed-form solution of absolute orientation using orthonormal matrices[J]. Journal of the Optical Society of America A,， 1988,， 5（7）： 1127-1135.