基于特征點(diǎn)與改進(jìn)ICP的點(diǎn)云配準(zhǔn)方法

黃際瑋，陸安江

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

1 引言

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

2 算法原理

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

2.1 關(guān)鍵點(diǎn)提取算法

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

圖1 算法總體框架

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

1）對(duì)點(diǎn)云P中每個(gè)點(diǎn)pi建立一個(gè)局部坐標(biāo)系，并設(shè)置搜索半徑ri；

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

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

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

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

2.2 FPFH特征描述子

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

圖2 FPFH鄰域范圍示意圖

圖3 SPFH局部坐標(biāo)系

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

以此對(duì)查詢點(diǎn)ps的鄰域進(jìn)行更新，通過(guò)所有鄰域點(diǎn)的SPFH特征計(jì)算得到ps的FPFH特征，如下式：

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

2.3 點(diǎn)云粗配準(zhǔn)

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

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

2）在目標(biāo)點(diǎn)云中尋找具有源點(diǎn)云采樣點(diǎn)相似FPFH特性的點(diǎn)作為對(duì)應(yīng)點(diǎn)；

3）通過(guò)對(duì)應(yīng)點(diǎn)之間的位置關(guān)系計(jì)算出旋轉(zhuǎn)平移矩陣，并用Huber函數(shù)判定是否為最佳變換，記為其中：

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

2.4 點(diǎn)云精配準(zhǔn)

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

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

1）粗配準(zhǔn)后源點(diǎn)云特征點(diǎn)集合為P'，目標(biāo)點(diǎn)云的特征點(diǎn)集和為Q'，分別計(jì)算出每個(gè)點(diǎn)的法向量；

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

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

3 實(shí)驗(yàn)結(jié)果與分析

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

其中pj與qj分別表示源點(diǎn)云與目標(biāo)點(diǎn)云中的對(duì)應(yīng)點(diǎn)，n表示對(duì)應(yīng)點(diǎn)數(shù)量。

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

圖4 原始點(diǎn)云位置

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

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

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

圖6 點(diǎn)云中關(guān)鍵點(diǎn)提取

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

圖7 本算法配準(zhǔn)結(jié)果

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

表1 本算法配準(zhǔn)誤差與耗時(shí)

本算法與ICP、4PCS+ICP、SAC-IA+ICP各算法的對(duì)比結(jié)果如表2所示。其中，本算法配準(zhǔn)誤差延用表1，配準(zhǔn)耗時(shí)則為表1粗配準(zhǔn)與精配準(zhǔn)耗時(shí)之和。

表2 不同算法對(duì)比

從對(duì)比結(jié)果可知，相較于其他現(xiàn)有算法，本研究提出的算法能夠更好地實(shí)現(xiàn)點(diǎn)云配準(zhǔn)。

4 結(jié)束語(yǔ)

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