基于KDTree改進(jìn)的ICP算法在點(diǎn)云配準(zhǔn)中的應(yīng)用研究

郭俊輝

（上海海事大學(xué) 信息工程學(xué)院，上海 201306）

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é)果。

圖1 ScanStation C10掃描儀的作業(yè)流程

圖2 兩個(gè)坐標(biāo)系下采集到的點(diǎn)云數(shù)據(jù)

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)系下。

圖3 兩站掃描示意圖

對(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 ClosestPoint）算法[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ì)算的。

算法步驟如下：

（1）令Ω為點(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。

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

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

（5）根據(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ò)程如下：

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

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

（3）構(gòu)建矩陣 Q：

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

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

（6）解算平移向量 T：

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

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

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

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

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

（5）由得到的 R、T變換 Mm，得到最新的 Mm。

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

（7）當(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所示。

圖4 基于KDTree改進(jìn)的ICP算法實(shí)現(xiàn)

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

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

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

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é)果。

圖5 兔子模型點(diǎn)云的配準(zhǔn)效果

圖6 長(zhǎng)方體點(diǎn)云的配準(zhǔn)效果

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

[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.