基于k-d tree的ICP三維點云拼接方法

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

(1.成都航空職業技術學院無人機產業學院，四川成都 610199；2.成都西南交大研究院有限公司，四川成都 610031)

1 概述

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

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

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

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

2 三維點云k-d tree

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

2.1 建立三維點云k-d tree

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

圖1 三維點云k-d tree建立流程

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

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

圖2 三維數據生成的k-d tree

2.2 最近鄰搜索

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

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

(2)一旦搜索到葉節點，該葉節點就被視為當前最佳點，計算該葉節點與目標點之間的距離。

(3)解開遞歸，并對每個經過的節點進行下列步驟：

①如果當前點比當前最佳點更接近目標點，則將當前最佳點更新為當前點；

②檢查另一子樹是否存在更近的點，若存在，則從該節點往下搜索。

(4)當根節點搜索完畢后，便完成了最近鄰搜索。

3 ICP算法

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

式(1)中pi為目標點云P中的點，qi為源點云Q中與pi對應的最近點，n為最近鄰點的對數，R為旋轉矩陣，t為平移向量。

3.1 傳統的ICP算法

傳統ICP算法的步驟如下：

(1)取目標點云P中的點集pi∈P；

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

(3)基于對應點對，計算出相應的旋轉平移矩陣[R|t]；

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

(5)計算pi'和對應點集qi之間的平均距離d，如式(2)所示；

(6)給定一個距離閾值ε，并預設迭代計算的最大迭代次數；

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

3.2 基于k-d tree的ICP算法

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

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

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

4 試驗與結果分析

為了驗證上述算法對三維點云拼接的正確性與有效性，使用無人機搭載激光雷達對現場房屋進行掃描試驗，如圖4 所示。掃描得到的部分三維激光點云數據如圖5所示，可見點云在三維空間中存在明顯錯位。

圖4 現場試驗

圖5 現場房屋點云

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

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

圖6 拼接效果

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

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

5 結論

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

2)本文提出的基于k-d tree 的ICP 三維點云拼接算法，可避免ICP算法陷入局部最優，有效改善了點云拼接效果。相對于傳統的ICP算法，該算法可有效減少目標點云與源點云中的錯誤匹配點對，同時提升算法效率。