基于特征點與改進ICP的點云配準方法

黃際瑋，陸安江

（貴州大學大數(shù)據(jù)與信息工程學院，貴陽 550025）

1 引言

隨著三維激光掃描技術在文物保護、三維重建、無損檢測等領域的快速發(fā)展，點云配準[1]在三維圖像領域得到廣泛應用。由于儀器視角與物體自身遮擋等因素的限制，需要從多個站點才能獲取物體的全部信息，通過點云配準技術可以尋找出最優(yōu)旋轉平移矩陣，使得不同視角下的物體能夠變換到同一坐標系下[2]，從而得到完整的點云數(shù)據(jù)。目前已有大量的學者對點云配準進行研究，其中Besl等人[3]提出的迭代最近點（Iterative Closest Point,ICP）算法應用最為廣泛，但要求點云的初始位置較好，且收斂速度較慢。針對這些問題，宋成航等人[4]利用向量夾角提取特征點后通過快速點特征直方圖（Fast Point Feature Histograms,FPFH）描述特征點，并在ICP算法中加入KD-tree完成最終配準；李慧慧等[5]在ICP算法中利用二次搜索求出最近距離，提高傳統(tǒng)ICP算法的效率；李為民等[6]在主成分分析法（Principal Component Analysis,PCA）的基礎上完成初始配準，但PCA算法要求待配準點云有較高的重疊度；荊路等人[7]則是利用SIFT算法提取特征點并結合SACIA算法完成初始配準，提高了配準精度，但SIFT算法耗時較長。針對上述仍然存在的問題，在此提出一種基于特征點與改進ICP的點云配準方法。

2 算法原理

本算法采用的方法是在粗配準階段使用ISS算法從下采樣點云中提取特征點，結合采樣一致性算法完成初始配準，在精配準階段加入法向量夾角約束剔除誤配點，代替?zhèn)鹘y(tǒng)ICP算法中將全部點云參與計算，以此提高算法速度與精度。

2.1 關鍵點提取算法

算法總體步驟如圖1所示。對原始點云進行下采樣后，通過ISS算法從采樣點中提取特征點并用FPFH進行特征描述；利用SAC-IA算法對原始點云進行初始配準，最終采用法向量約束的改進ICP算法完成精細配準。

圖1 算法總體框架

在點云空間中，一些點具有較為明顯的幾何特征且具有尺度不變性，這些點通常稱為特征點。內部形態(tài)描述子（Intrinsic Shape Signatures,ISS）算法[8]可以對局部信息進行有效提取，其原理如下：

1）對點云P中每個點pi建立一個局部坐標系，并設置搜索半徑ri；

2）以pi為中心，搜索其半徑ri內的k個鄰域點，根據(jù)中心點與鄰域點的距離計算出每個鄰域點的權值wij如下：

3）構建pi與pij的協(xié)方差矩陣cov(pi)：

4）由協(xié)方差矩陣求出對應特征值{λi1,λi2,λi2}，其中λi1＜λi2＜λi3；

5）分別設置兩個閾值ε1與ε2，若滿足λi2/λi1≤ε1且λi1/λi2≤ε2，則該點可視為特征點。

2.2 FPFH特征描述子

快速點特征直方圖（FPFH）描述子[9]是一種局部特征描述子，它以直方圖的形式表示局部鄰域內點云間的特征，如圖2所示。以查詢點pq為圓心，r為半徑搜索鄰域點，并分別計算其法向量，通過角度三元組(α,φ,θ)表示查詢點與鄰域點之間的關系，得到簡化的點特征直方圖（Simplified Point Feature Histgram,SPFH）特征，如圖3所示。

圖2 FPFH鄰域范圍示意圖

圖3 SPFH局部坐標系

圖中的nq、nk分別為pq、pk的法向量，d為pq、pk的距離向量，可得出：

以此對查詢點ps的鄰域進行更新，通過所有鄰域點的SPFH特征計算得到ps的FPFH特征，如下式：

其中權重wk表示查詢點ps與其鄰域點pk的歐氏距離，k為鄰域點數(shù)量。

2.3 點云粗配準

在點云配準中，粗配準結果好壞直接影響到后續(xù)精配準效果。采用采樣一致性（Sample Consensus Initial Aligment,SAC-IA）算法[10]是一種常用的粗配準算法，具體步驟如下：

1）在源點云P中選取x個不同的采樣點并設置距離閾值wd，以保證采樣點間具有不同的FPFH特征；

2）在目標點云中尋找具有源點云采樣點相似FPFH特性的點作為對應點；

3）通過對應點之間的位置關系計算出旋轉平移矩陣，并用Huber函數(shù)判定是否為最佳變換，記為其中：

式中ml為設定值，ld為第d組對應點變換后的誤差。重復上述步驟，使得誤差函數(shù)最小的矩陣為最佳變換矩陣。

2.4 點云精配準

經粗配準后，兩片點云已經大致對齊。為進一步減小誤差，需要對點云進行精細配準。ICP算法是一種經典的點云配準方法，其原理[11]為：對于源點P云中的點pi，以歐氏距離為約束尋找目標點云Q中的對應點qi，通過對應點計算旋轉平移矩陣R和T使得誤差函數(shù)最小。誤差函數(shù)定義為：

由此方式讓所有的點都參與了計算，但其中的一些冗余點會導致計算速度拖慢，因此，在ICP配準過程中，設計加入法向量約束，設置法向量夾角閾值剔除誤配點，以提高算法效率。具體步驟如下：

1）粗配準后源點云特征點集合為P'，目標點云的特征點集和為Q'，分別計算出每個點的法向量；

2）對P'中的每個點在Q'中查找對應的歐氏距離最近的點，記為點對N，然后設置法向量閾值fθ，若N的法向量夾角小于閾值，保留該點對，否則視為誤配點，將其剔除，剔除誤配點后的點對記為N'；

3）根據(jù)N'使用SVD分解法計算變換矩陣(R,T)，并根據(jù)誤差函數(shù)E(R,T)求出最優(yōu)的變換矩陣。

3 實驗結果與分析

實驗選用斯坦福數(shù)據(jù)集中不同角度的“兔”（Bunny）與“龍”（Dragon）點云作為配準對象。實驗運行環(huán)境選取Visul Studio 2017+PCL（點云庫）1.8.1，Windows操作系統(tǒng)，CPU為AMD Ryzen 5-4600H，16.00G內存。采用均方根誤差（RMSE）來描述最終配準結果，定義為：

其中pj與qj分別表示源點云與目標點云中的對應點，n表示對應點數(shù)量。

原始點云中Bunny模型的點云的數(shù)量為40256與40097，Dragon模型的點云數(shù)量為41841與34836，如圖4所示。

圖4 原始點云位置

首先使用體素柵格對原始點云下采樣，體素柵格邊長設置為0.001m，然后進行特征點提取。其中各階段的點云數(shù)量如圖5所示。

圖5 不同階段點云數(shù)量

由圖中可知，相對于原始點云，提取出的關鍵點數(shù)量減少了90%以上，通過這種方法可以在保留原始點云大部分特征的同時有效地提高后續(xù)的配準速度。最終提取出的特征點如圖6所示。

圖6 點云中關鍵點提取

特征點分布于點云中凹凸性較強的位置，這些點更能代表點云的特征。提取出特征點之后，對特征點進行FPFH描述，并用SAC-IA算法對點云進行初始配準，最后結合法向量約束的ICP算法完成精細配準。配準效果如圖7所示。

圖7 本算法配準結果

可以看到，經過粗配準后，兩點云的位置已經大致重合，但在一些細節(jié)處，如Bunny點云的背部和Dragon點云的尾部依然還存在誤差；經過精細配準之后，兩片點云的位置得到更好地校對。在配準過程中的配準誤差與配準耗時如表1所示。

表1 本算法配準誤差與耗時

本算法與ICP、4PCS+ICP、SAC-IA+ICP各算法的對比結果如表2所示。其中，本算法配準誤差延用表1，配準耗時則為表1粗配準與精配準耗時之和。

表2 不同算法對比

從對比結果可知，相較于其他現(xiàn)有算法，本研究提出的算法能夠更好地實現(xiàn)點云配準。

4 結束語

基于特征點與改進ICP的點云配準方法，對于解決傳統(tǒng)ICP算法中存在的問題，發(fā)揮出了良好的效果。初始配準階段的ISS算法提取特征點與FPFH特征描述，后續(xù)的SAC-IA算法對原始點云的粗配準，以及最終在ICP算法中引入法向量約束剔除誤配點對，都成功實現(xiàn)了對配準效果的優(yōu)化。實驗結果符合理論預期。相較于原始ICP、SAC-IA+ICP與4PCS+ICP各算法，本算法能夠實現(xiàn)更高的配準精度與速度，具有很高的實際應用價值。