實現激光點云高效配準的 ICP 優化及性能驗證

　　摘 要：激光點云常規匹配算法是迭代最近點(Iterative Closest Point, ICP)算法，但其收斂速度慢、魯棒性差，因此提出一種融合多種優化算法的激光點云高效 ICP 配準方法。首先對點云體素濾波降采樣，通過 ISS 算子提取關鍵點，采用快速點特征直方圖(Fast Point Feature Histograms, FPFH)提取關鍵點特征，嵌入 OpenMP (多核多線程并行處理模式)提高特征提取速度;然后基于提取的 FPFH 特征，使用采樣一致性初始配準算法(Sample Consensus Initial Alignment, SAC-IA)進行相似特征點粗配準，獲取點云集間的初始旋轉平移變換矩陣;最后采用 ICP 算法進行精配準，同時采用最優節點優先(Best Bin First, BBF)優化 K-D tree 近鄰搜索法來加速對應關系點對的搜索，并設定動態閾值消除錯誤對應點對，提高配準快速性和準確性。對兩個實例的配準點云進行了實驗驗證，結果表明，提出的優化配準算法具有明顯速度優勢和精度優勢。

　　本文源自紅外與激光工程 發表時間：2021-03-10《紅外與激光工程》系中國宇航學會光電技術專業委員會會刊，由中國航天科工集團公司主管，創刊于1972年，是國家科委和國家新聞出版署批準的國內外公開發行的部級學術刊物。 本刊是中國航天界光電子技術領域內學術性與工程應用性集于一體的綜合性刊物，主要刊登國內紅外與激光技術方面的學術論文和工程研究報告，集中反映了中國光電技術在宇航、衛星及導彈武器系統中的工程應用水平。

　　關鍵詞 激光點云;快速點特征直方圖;采樣一致性算法;迭代最近點算法;點云配準

　　1 引言

　　激光掃描三維重建作為一種新興空間數據獲取技術已廣泛應用于多種領域[1,2]。采用三維激光掃描儀對特定目標掃描采集點云時，受到目標尺寸、外界環境等因素干擾，致使點云無法被一次性獲取，需多站點多角度多次掃描才能實現，多站點云需進行配準，轉換到統一坐標系中形成完整點云。

　　點云配準主要采用 Besl[3]提出的傳統迭代最近點(ICP)算法，具有良好配準精度，但收斂速度慢，且當兩組點云存在較大差異時，算法會陷入局部最優解，魯棒性差[4]。為了提高配準效率，在進行 ICP 配準之前可以先進行粗配準[5]。粗配準是根據局部特征的旋轉平移不變性，采用特征匹配實現[6]。如李宇翔[7]提出基于內部形態描述子結合 K-D tree 的改進迭代最近點配準算法來提高配準速度;Rasu[8]使用點特征直方圖對點的幾何特征描述得到多維特征向量;孫文瀟[9]采用局部表面擬合進行法線估計并計算其快速點特征直方圖進行粗配準。

　　對于 ICP 算法優化中，盧純青等[10]基于深度信息動態尺度估計方法提升點云配準的時間穩定性和配準精度;Bae[11]計算點與其鄰域內點集組成的協方差矩陣，使用點的曲率變化率代替表面曲率，減少計算量;唐志榮[12]使用典型相關分析法對兩組轉動矩陣進行求解，實現點云配準;黃源[13]根據點云在不同半徑內的法向量變化度提取特征點，利用距離約束條件獲取準確匹配點;王帥等[14]使用譜聚類按幾何結構特征對各視角點云區域分割，采用改進 ICP 對點云精確配準。

　　現有文獻對粗配準、精配準 ICP 算法及其各種變種算法研究集中在優化算法本身上，而對加速搜索算法和錯誤對應點對配準算法精度和速度的影響研究較少。為進一步提高點云配準算法的速度和精度，本項目融合多種優化方法，重點提高搜索速度和特征值提取速度，包括優化點特征直方圖提取方法與效率、采樣一致性初始配準(SAC-IA)和采用最優節點優先 K-D tree 近鄰搜索法等，來實現高效、高精度的 ICP 點云優化配準算法。

　　2 優化原理

　　如圖 1 所示為融合了多種優化算法實現點云高效配準優化方案。其中，在點云預處理中對點云進行體素網格濾波;在關鍵點選擇中，采用內部形態描述子(Intrinsic Shape Signatures, ISS)算法提取關鍵點;在特征提取優化中，嵌入多核多線程 OpenMP 并行處理模式，加速關鍵點快速特征直方圖提取;在粗配準中，采用采樣一致性初始配準優化;精配準中，采用最優節點優先法優化 K-D Tree 近鄰搜索，加速對應關系點對搜索，同時設置動態閾值剔除錯誤對應關系點對，提高精匹配精度。

　　2.1 點云預處理

　　由于激光雷達掃描點云中空間點數目非常大，會影響點云的配準效率;另外點密度分布也不均勻，且掃描點存在粗大誤差和測量誤差，因此有必要進行體素柵格濾波。根據原始點云分布空間創建八叉樹三維體素柵格，細分深度 8。在每個小網格體素中，用所有點的重心近似代替體素中所有點，可得降采樣點云的同時還保持點云基本形狀特征。

　　2.2 關鍵點選擇

　　采用內部形態描述子(Intrinsic Shape Signatures, ISS)算法進行關鍵點提取：設點云 W 中含 m 個點， p x y z i m i i i i = = ( , , , 1,2, , ) ，對每個點 i p 建立一個局部坐標系，并設定一搜索半徑 iss r ，確定 i p 為球心、 iss r 為半徑球體內的包含點，計算權值?ij ： 1 , ij i j iss i j p p r p p ? = − ? − (1)計算每個點 i p 的協方差矩陣： ( ) ( )( ) cov i j iss i j iss T ij i j i j p r i ij p p r p p p p p ?? − ? − ? − − = ?? (2)獲得協方差矩陣的特征值? ? 1 2 3 , , ? ? ? i i i ，從大到小排序;設置閾值?1 和?2 ，滿足 2 1 1 i i ????和 3 2 2 i i ????的點即為關鍵點。

　　2.3 特征提取

　　如圖 2 所示，基于中心點與 k 鄰域之間的法向矢量關系計算點特征描述向量。

　　圖 2(a)以關鍵點 pq 為中心劃半徑為 r 圓球，包含 k 個近鄰點。如圖 2(b)所示，將 pq 與 k 個鄰域點擬合一小平面，其法向量即 q n ，對 q n 定義一個笛卡爾坐標系，三坐標軸為: 2 q k q k q x n p p y x p p z x y ? = ?? − ? = ? − ?? = ? ? (3)描述 q n 與某近鄰點 pk 法線 k n 之間的相對空間偏差關系，用三個角度特征值： cos( ) ? = ? arc y nk (4) 2 arccos( ) k q k q p p x p p ? − = ? − (5)? = ? ? arctan , (z n x n k k ) (6)

　　計算出 pq 與半徑 r 鄰域內所有點組成的點對之間的三個特征值后，用直方圖統計三個特征值的區間分布數目，記為簡化點特征直方圖 (Simplified Point Feature Histogram，SPFH)。再依次查詢每個鄰域點 ki p 的 SPFH。綜合關鍵點 pq SPFH 特征和 k 個近鄰點 SPFH 特征，計算關鍵點 pq 的 FPFH 直方圖特征序列為： ( ) ( ) ( ) 1 1 1 + k q q i i i FPFH p SPFH p SPFH p k d = = ? ? (7)其中， di 為對應點對的歐氏距離。

　　FPFH 將三個特征 ( , , ) ? ? ?的參數范圍均劃分為 11 個統計子區間，統計每個子區間上點數目，合并得到一個 33 維特征向量。圖 3(a)所示，取建筑物激光點云中三點 A、B、C，分別為平面點、曲面點和角點，三點的 FPFH 序列如圖 3(b)、(c)、(d)。可見，點的幾何位置不同，FPFH 值也各有特點。

　　2.4 基于采樣一致性的粗配準

　　FPFH 特征向量提取后，通過采樣一致性算法對目標點云和模板點云粗配準：(1)模板點云中，獲取 m 個待配準特征點;(2)目標點云中，搜索與模板點云中特征點 FPFH 值相似的所有點，隨機選一個相似點，作為從模板點云到目標點云的對應關系點;(3)計算對應關系點對的旋轉平移矩陣，并計算對應關系點旋轉平移轉換后的距離誤差總和函數，即 Huber 損失函數： ( ) ( ) 1 2 , 2 1 2 , 2 i i h i i i e e r L e r e r e r ?? ?? = ?? − ? ?? (8)其中， r 為距離閾值， i e 為對應點對間的歐氏距離。Huber 損失函數最小值對應的旋轉平移變換矩陣就是粗配準所求的最優變換矩陣。

　　2.5 優化 ICP 精配準

　　2.5.1 進行搜索加速優化的 ICP 精配準

　　采用最優節點優先 (Best Bin First, BBF)優化 K-D Tree 近鄰搜索，加速對應點對搜索：

　　(1)分割結點確定：統計所有特征點在(x, y, z)各維度方差，方差最大的維度為主維度，將特征點按主維度排序，正中間特征點為分割結點。

　　(2)搜索對應關系點對：待查詢點值與分割結點值比較，若小于等于，則進左子樹，否則進右子樹，沿 K-D Tree 搜索，直至葉子結點，在此子空間內查找與待查詢點距離最近點，同時建立優先級，點的優先級 Tpri 定義為： , ( , , ) T s r i x y z pri i i = − ? (9) i s 為待查詢點第 i 維值， i r 為特征點第 i 維值， i 為分割維度。 Tpri 越小，優先級越大。

　　(3)“回環”搜索：按優先級大小查找是否存在與該點距離更近對應點，若在其他分割子空間中存在，則進入此子空間繼續搜索。

　　2.5.2 動態閾值設置

　　將每次迭代完的配準誤差(即歐式適合度評分)設置為下次迭代的距離閾值，進行自適應動態設置，在第?次配準誤差為[15]： ( ( )) 2 , , 1 1 1 n MS a i b i i R p R p t n ? ? = = − ? + − ? (10) ( p p a i b i , , − ? ) ? (11) R?和 t?分別為第?次迭代的旋轉和平移矩陣，?為迭代的距離閾值。點云偏差一般服從正態分布，則取? =3RMS ，剔除不滿足式(11)的對應點對，防止該點對參與下次迭代。

　　3 實驗過程與結果分析

　　3.1 實驗過程

　　采用兩組點云進行算法驗證實驗。第一組采用 PCL 官網下載建筑物點云;第二組為采用激光雷達掃描建筑物獲得。實驗采用計算機硬件環境：處理器 Intel(R) Core(TM) i5-8300H CPU @ 2.30GHz，內存 16.0GB;操作系統 ubuntu 16.04，64-bit，編程語言 VC++。

　　3.2 實驗結果分析

　　3.2.1 實驗 1

　　圖 4(a)建筑物點云，目標點云(綠色)共 112586 個點，模板點云(紅色)共 112624 個點，兩原始點云經體素濾波后如圖 4(b)所示，濾波后分別含 17677、17640 個點。

　　從目標點云和模板點云中分別隨機選取兩個關鍵點進行特征提取并顯示點特征直方圖，如圖 5 所示，A、C 是模板點云關鍵點，B、D 是目標點云關鍵點。其中， A 點和 D 點的 FPFH 特征基本一致，因此作為一組對應關系點對。

　　當 ISS 算子加入后，對點云的配準效果進行了實驗驗證比較。如表 1 所示，其中 SAC-IA 是進行傳統 FPFH 特征提取進行的配準實驗結果，SAC-IA+ISS 是先提取 ISS 關鍵點、然后再進行 FPFH 特征提取進行的配準實驗結果。表 1 的結果表明，采用 ISS 算子提取關鍵點后，配準歐拉評分近似相同，而配準時間卻減少近一半，由 12.061s 降為 6.797s，因此 ISS 算子的引入有效提高了點云的配準效率。

　　在特征提取過程中，嵌入了 OpenMP 多核多線程并行計算模式，加快特征提取速度，采用單線程 pcl::FPFH Estimation 特征提取耗時 13.522s，而采用 OpenMP 模式耗時 3.058s，僅為單線程的 22.6%。基于提取的 FPFH 特征值進行粗配準，共耗時 6.091s，粗配準的點云效果如圖 6(a)所示，同時獲得了兩組點云的粗配準變換矩陣。兩組點云基本重合，但仍有較明顯錯位，需進一步精配準。

　　完成粗配準后，再進行優化 ICP 配準，耗時 0.895s，共迭代 5 次，歐式適合度評分為 0.0040。其中，歐式適合度評分是目標點云到模板點云對應關系點對的距離平方和。ICP 精配準效果如圖 6(b)所示，同時獲得了兩組點云的精配準變換矩陣。

　　為比較，對兩組點云進行常規 ICP 配準，共耗時 8.297s，迭代 45 次，歐式適合度評 0.0041。

　　3.2.2 實驗 2

　　將激光雷達固定在三角支架上，獲得不同站點實驗室走廊的掃描點云。取兩站點掃描的激光點云進行配準，第一組為紅色點云，含 290545 點;第二組為綠色點，含 290608 點，如圖 7(a)所示。對兩個原始點云進行體素網格濾波后分別含 5938、5942 個點，如圖 7(b)所示。

　　對走廊點云篩選關鍵點，提取 FPFH 特征，并進行 SAC-IA 粗配準，求取兩組點云的變換矩陣，粗配準效果如圖 7(c)所示，耗時 1.080s，同時獲得了兩組點云的粗配準變換矩陣。

　　對粗配準獲得的兩組點云，再進行優化 ICP 配準，耗時 0.416s，迭代 7 次，歐式適合度評分 0.0057，如圖 7(d)所示，同時獲得了兩組點云的精配準變換矩陣。

　　為比較，對走廊點云進行常規 ICP 配準，共耗時 2.042s，迭代 33 次，歐式適合度評分 0.0058。

　　3.2.3 實驗結果分析

　　點云配準結果的主要評估標準是迭代次數、歐式適合度評分和配準耗時。上述兩個配準驗證實驗中，常規 ICP 算法和優化算法的評估參數如表 2 所示。另外，為了進一步說明所提出的優化組合方式的優越性，與文獻[8]所采用的點云配準方法進行了實驗比較，此方法記作 SAC-IA+ICP，即采用采樣一致性的粗配準和精配準的組合，實驗結果也列入表 2 中。由表 2 可見，在歐拉評分近似相同的配準精度效果下，組合優化方法 Optimized SAC-IA+ICP，在迭代次數和配準時間上均有顯著改善。

　　表 2 可見，優化 SAC-IA+ICP 在兩組實驗中的迭代次數均遠少于常規 ICP，僅為其 1/9~1/4。另外，優化 SAC-IA+ICP 在兩組實驗中計算速度均快于常規 ICP，在第一組實驗中，優化 SAC-IA+ICP 為 6.986s，約占常規 ICP 的 84.19%;在第二組實驗中，優化 SAC-IA+ICP 為 1.496s，約占常規 ICP 的 73.26%。優化 SAC-IA+ICP 在兩組實驗中得歐式適合度評分均略優于常規 ICP。

　　4 結論

　　提出了一種融合多種優化算法的激光點云高效 ICP 配準方法，采用多核多線程 OpenMP 模式加快 FPFH 特征提取，根據提取的點云 FPFH 特征采用 SAC-IA 粗配準計算初始旋轉平移變換矩陣，采用最優節點優先法優化 K-D tree 近鄰搜索法加速搜索，及設置動態閾值消除錯誤點對等優化措施。通過對建筑物點云和走廊點云進行算法驗證，比較了優化 SAC-IA+ICP 算法和常規 ICP 配準算法的性能，結果可知，優化 SAC-IA+ICP 算法的迭代次數減小為 1/9~1/4，耗時減小為 73.26%~84.19%，歐式適合度評分有所降低，因此具有高效和高精度的優勢。