基于k-d tree的ICP三維點(diǎn)云拼接方法

李艷，屈仁飛，顧菘，謝燕梅

(1.成都航空職業(yè)技術(shù)學(xué)院無人機(jī)產(chǎn)業(yè)學(xué)院，四川成都 610199；2.成都西南交大研究院有限公司，四川成都 610031)

1 概述

三維點(diǎn)云在逆向工程[1]、文物保護(hù)[2]、口腔醫(yī)學(xué)[3]與無人駕駛[4]等行業(yè)中得到了廣泛的應(yīng)用。在獲取三維點(diǎn)云時(shí)，受被測物體的幾何形貌、測量設(shè)備等因素的限制，測量設(shè)備需要從多個(gè)角度對被測物體進(jìn)行數(shù)據(jù)采集，采集得到的各片點(diǎn)云均位于設(shè)備坐標(biāo)系下。因此，需要計(jì)算出旋轉(zhuǎn)平移矩陣，調(diào)整多視角測量的各片三維點(diǎn)云在空間中的位姿，并統(tǒng)一到同一坐標(biāo)系當(dāng)中，從而獲得完整的被測物體三維點(diǎn)云。計(jì)算旋轉(zhuǎn)平移矩陣的過程，即是求解三維點(diǎn)云拼接問題的過程。

為解決三維點(diǎn)云拼接問題，國內(nèi)外學(xué)者提出了眾多拼接算法[5-8]，一般可將拼接過程分解為粗拼接與精拼接兩個(gè)步驟?；谥鞒煞址治鯷5](Principal Component Analysis，PCA)的拼接算法是通過計(jì)算兩片點(diǎn)云的主成分與質(zhì)心坐標(biāo)值，然后，計(jì)算得到點(diǎn)云之間的旋轉(zhuǎn)平移矩陣，實(shí)現(xiàn)拼接，但是當(dāng)點(diǎn)云主成分不明確時(shí)，難以實(shí)現(xiàn)拼接，且易出現(xiàn)主軸反向問題[9]。采樣一致性初始配準(zhǔn)法[6](Sample Consensus Initial Aligment，SAC-IA)使用點(diǎn)云法線、快速點(diǎn)特征直方圖[10](Fast Point Feature Histograms，F(xiàn)PFH)等特征信息計(jì)算兩片點(diǎn)云之間的對應(yīng)關(guān)系進(jìn)行拼接，但其效率低、拼接精度較差。正態(tài)分布變換[7](Normal Distributions Transform，NDT)則是通過標(biāo)準(zhǔn)最優(yōu)化技術(shù)確定兩片點(diǎn)云之間的最優(yōu)對應(yīng)關(guān)系，并進(jìn)行拼接，但該算法的收斂域相對較差、對點(diǎn)云初始空間位姿有一定要求。上述算法均適用于粗拼接，但難以滿足精度要求，具有一定局限性，故三維點(diǎn)云拼接需在粗拼接的基礎(chǔ)上進(jìn)行精拼接。

目前最經(jīng)典的精拼接算法是迭代最近點(diǎn)[8](Iterative Closest Point，ICP)算法，其采用最小二乘估計(jì)計(jì)算點(diǎn)集之間的關(guān)系，通過迭代計(jì)算使最近點(diǎn)的誤差之和最小。兩片點(diǎn)云中的拼接點(diǎn)集決定了ICP 算法的收斂速度與拼接精度，若點(diǎn)對匹配錯(cuò)誤，則會導(dǎo)致算法陷入局部最優(yōu)解。

為此，本文提出了一種基于k-d tree 的ICP 三維點(diǎn)云拼接算法，確立了點(diǎn)云拼接的對應(yīng)點(diǎn)集，滿足高精度的三維拼接要求。

2 三維點(diǎn)云k-d tree

k-d樹是一種能夠存儲和檢索k維數(shù)據(jù)的數(shù)據(jù)結(jié)構(gòu)。它的根節(jié)點(diǎn)在di(i=1～k)維上形成一個(gè)超平面，將k維空間分成兩個(gè)子空間。在di維中小于超平面值的數(shù)據(jù)放在左子樹中，大于超平面值的數(shù)據(jù)放在右子樹中。

2.1 建立三維點(diǎn)云k-d tree

為達(dá)到在三維點(diǎn)云中快速檢索近鄰點(diǎn)的目的，需要對三維點(diǎn)云建立相應(yīng)的k-d tree，建立流程如圖1所示。

圖1 三維點(diǎn)云k-d tree建立流程

首先，計(jì)算三維點(diǎn)云在X、Y、Z三個(gè)維度上的方差，按照方差大小輪流選取分割維度，在分割維度上選取中值作為樹的根節(jié)點(diǎn)，則中值所在處形成的超平面將空間分割左右子空間，如此反復(fù)直至子空間中無數(shù)據(jù)。

如圖2所示為一組三維點(diǎn)通過上述方法建立的k-d tree，圖中△數(shù)據(jù)位于分割維度。

圖2 三維數(shù)據(jù)生成的k-d tree

2.2 最近鄰搜索

完成三維點(diǎn)云k-d tree的建立之后，便可在k-d tree中對目標(biāo)點(diǎn)進(jìn)行檢索，搜尋k-d tree中距離目標(biāo)點(diǎn)最近的點(diǎn)，即是k-d tree的最近鄰搜索，其過程如下：

(1)從根節(jié)點(diǎn)開始，遞歸地往子節(jié)點(diǎn)搜索。搜索方向的決定方法與插入元素的方法相同，即如果目標(biāo)點(diǎn)在超平面的左邊則進(jìn)入左子節(jié)點(diǎn)，在右邊則進(jìn)入右子節(jié)點(diǎn)。

(2)一旦搜索到葉節(jié)點(diǎn)，該葉節(jié)點(diǎn)就被視為當(dāng)前最佳點(diǎn)，計(jì)算該葉節(jié)點(diǎn)與目標(biāo)點(diǎn)之間的距離。

(3)解開遞歸，并對每個(gè)經(jīng)過的節(jié)點(diǎn)進(jìn)行下列步驟：

①如果當(dāng)前點(diǎn)比當(dāng)前最佳點(diǎn)更接近目標(biāo)點(diǎn)，則將當(dāng)前最佳點(diǎn)更新為當(dāng)前點(diǎn)；

②檢查另一子樹是否存在更近的點(diǎn)，若存在，則從該節(jié)點(diǎn)往下搜索。

(4)當(dāng)根節(jié)點(diǎn)搜索完畢后，便完成了最近鄰搜索。

3 ICP算法

ICP 算法的基本原理是：在目標(biāo)點(diǎn)云P中選取點(diǎn)pi，在待匹配的源點(diǎn)云Q中搜索最近鄰點(diǎn)qi，并計(jì)算出pi與qi的最優(yōu)匹配旋轉(zhuǎn)矩陣R和平移向量T，從而使如式(1)所示的誤差函數(shù)最小化。

式(1)中pi為目標(biāo)點(diǎn)云P中的點(diǎn)，qi為源點(diǎn)云Q中與pi對應(yīng)的最近點(diǎn)，n為最近鄰點(diǎn)的對數(shù)，R為旋轉(zhuǎn)矩陣，t為平移向量。

3.1 傳統(tǒng)的ICP算法

傳統(tǒng)ICP算法的步驟如下：

(1)取目標(biāo)點(diǎn)云P中的點(diǎn)集pi∈P；

(2)在源點(diǎn)云Q中，找到pi的對應(yīng)點(diǎn)集qi∈Q，使得‖qi-pi‖=min；

(3)基于對應(yīng)點(diǎn)對，計(jì)算出相應(yīng)的旋轉(zhuǎn)平移矩陣[R|t]；

(4)利用步驟(3)得到的旋轉(zhuǎn)平移矩陣[R|t]對pi進(jìn)行旋轉(zhuǎn)平移變換，得到新的對應(yīng)點(diǎn)集pi'=Rpi+t,pi∈P；

(5)計(jì)算pi'和對應(yīng)點(diǎn)集qi之間的平均距離d，如式(2)所示；

(6)給定一個(gè)距離閾值ε，并預(yù)設(shè)迭代計(jì)算的最大迭代次數(shù)；

(7)當(dāng)式(2)中的d小于給定的距離閾值ε，或者迭代計(jì)算的次數(shù)大于預(yù)設(shè)的最大迭代次數(shù)時(shí)，則停止進(jìn)行迭代計(jì)算。否則，返回步驟(2)繼續(xù)計(jì)算，直到滿足收斂條件。

3.2 基于k-d tree的ICP算法

ICP 算法中的旋轉(zhuǎn)平移矩陣采用最小二乘估計(jì)計(jì)算，原理簡單，精度高。然而，由于采用迭代的方法進(jìn)行計(jì)算，該算法效率低。再者，ICP 算法對初始位姿有著較高要求，若拼接時(shí)，目標(biāo)點(diǎn)云與源點(diǎn)云存在大量非對應(yīng)點(diǎn)集，將導(dǎo)致算法陷入局部最優(yōu)解。鑒于此，使用基于k-d tree的ICP算法，對拼接的目標(biāo)點(diǎn)云與源點(diǎn)云進(jìn)行優(yōu)化，使得拼接的對應(yīng)點(diǎn)集更加精準(zhǔn)，從而提升拼接效率，避免算法陷入局部最優(yōu)。

圖3 為基于k-d tree 的ICP 算法流程，在迭代計(jì)算之前，使用k-d tree 對目標(biāo)點(diǎn)云中的對應(yīng)點(diǎn)集進(jìn)行初步選取，避免過多的非對應(yīng)點(diǎn)集參與到點(diǎn)云拼接中，從而避免算法陷入局部最優(yōu)解。同時(shí)，在迭代計(jì)算中，使用k-d tree 快速搜索最近點(diǎn)，獲取對應(yīng)點(diǎn)集，提高點(diǎn)云的拼接效率。

圖3 基于k-d tree的ICP算法流程

4 試驗(yàn)與結(jié)果分析

為了驗(yàn)證上述算法對三維點(diǎn)云拼接的正確性與有效性，使用無人機(jī)搭載激光雷達(dá)對現(xiàn)場房屋進(jìn)行掃描試驗(yàn)，如圖4 所示。掃描得到的部分三維激光點(diǎn)云數(shù)據(jù)如圖5所示，可見點(diǎn)云在三維空間中存在明顯錯(cuò)位。

圖4 現(xiàn)場試驗(yàn)

圖5 現(xiàn)場房屋點(diǎn)云

設(shè)圖5 中左側(cè)(綠色)點(diǎn)云為目標(biāo)點(diǎn)云，右側(cè)(紅色)點(diǎn)云為待匹配的源點(diǎn)云，使用兩種不同的算法分別對目標(biāo)點(diǎn)云和源點(diǎn)云進(jìn)行拼接：(a)傳統(tǒng)的ICP算法；(b)本文提出的基于k-d tree的ICP拼接算法。

在處理器為Intel(R) Core(TM) i7-8650U CPU @ 1.90GHz 2.11 GHz，RAM 為16.0G 的計(jì)算機(jī)上，在Windows 10 系統(tǒng)下編寫C++程序進(jìn)行拼接測試，算法(a)的拼接時(shí)間為128.879s，算法(b)的拼接時(shí)間為3.462s，拼接效果如圖6所示。

圖6 拼接效果

傳統(tǒng)的ICP算法計(jì)算出的旋轉(zhuǎn)平移矩陣[R|t]a如式(3)所示，該矩陣使得源點(diǎn)云中的地面點(diǎn)云與目標(biāo)點(diǎn)云中的地面點(diǎn)云之間形成了一定夾角，算法陷入了局部最優(yōu)，無法達(dá)到所需的拼接目的；本文提出的基于k-d tree 的ICP 拼接算法計(jì)算出的旋轉(zhuǎn)平移矩陣[R|t]b如式(4)所示，該矩陣使得源點(diǎn)云在屋頂處與目標(biāo)點(diǎn)云實(shí)現(xiàn)了較好的重合拼接。

從圖5中可以看出，源點(diǎn)云只需通過一定的平移便可使兩點(diǎn)云拼接在一起，即算法計(jì)算出的旋轉(zhuǎn)矩陣R越接近單位矩陣，該算法的準(zhǔn)確度越高、有效性越好。對比式(3)和式(4)，可見式(4)相對于式(3)更接近單位矩陣，即本文提出的基于k-d tree的ICP拼接算法對傳統(tǒng)的ICP算法有著明顯的改善。

5 結(jié)論

1)建立了三維點(diǎn)云k-d tree，確定了三維點(diǎn)云k-d tree的最近鄰搜索方法。在傳統(tǒng)ICP算法的基礎(chǔ)上，提出了一種基于kd tree的ICP三維點(diǎn)云拼接算法，并且確立了該算法的流程。

2)本文提出的基于k-d tree 的ICP 三維點(diǎn)云拼接算法，可避免ICP算法陷入局部最優(yōu)，有效改善了點(diǎn)云拼接效果。相對于傳統(tǒng)的ICP算法，該算法可有效減少目標(biāo)點(diǎn)云與源點(diǎn)云中的錯(cuò)誤匹配點(diǎn)對，同時(shí)提升算法效率。