基于組合特征點(diǎn)和主成分分析的點(diǎn)云配準(zhǔn)算法

張亞文，林文忠，韓曉東

（1.福建農(nóng)林大學(xué)機(jī)電工程學(xué)院，福建 福州 350100；2.閩江學(xué)院計(jì)算機(jī)與控制工程學(xué)院，福建 福州 350108）

0 引 言

點(diǎn)云數(shù)據(jù)是物體的一種三維表示形式，是物體表面海量點(diǎn)的集合，可精確描述三維物體的空間特征，具有極高的靈活性，已廣泛應(yīng)用于三維重建、自動(dòng)駕駛以及地圖繪制等領(lǐng)域［1］。點(diǎn)云配準(zhǔn)是點(diǎn)云應(yīng)用過(guò)程中的關(guān)鍵技術(shù)之一，在配準(zhǔn)過(guò)程中將2 幅點(diǎn)云作為輸入，通過(guò)計(jì)算輸出表示點(diǎn)云位姿變換的旋轉(zhuǎn)與平移矩陣，來(lái)盡可能地配準(zhǔn)點(diǎn)云。目前點(diǎn)云配準(zhǔn)算法主要分為基于局部特征和全局匹配的配準(zhǔn)方法。局部特征方法主要包含由二維圖像關(guān)鍵點(diǎn)檢測(cè)引入的3DHarris 和尺度不變特征變換（Scale-Invariant Feature Transform，SIFT），以及直接應(yīng)用于三維數(shù)據(jù)的法線對(duì)齊的徑向特征檢測(cè)（Normal Aligned Radial Feature，NARF）和ISS（Intrinsic Shape Signatures）關(guān)鍵點(diǎn)檢測(cè)，其次還包含點(diǎn)特征直方圖（Point Feature Histogram，PFH）、快速點(diǎn)特征直方圖等各種特征描述子［2］。基于點(diǎn)云全局匹配的方法目前應(yīng)用最廣泛的經(jīng)典算法是ICP 算法以和正態(tài)分布變換算法（Normal Distribution Transform，NDT），后續(xù)大多算法都由此演變而來(lái)［3-5］。李仁忠等［6］提出了一種基于ISS特征點(diǎn)結(jié)合改進(jìn)ICP 的配準(zhǔn)算法，但其提取的特征點(diǎn)數(shù)量較少，對(duì)點(diǎn)云形狀描述仍存在一定誤差。馮田等［7］提出了一種基于點(diǎn)云特征向量提取的配準(zhǔn)算法，根據(jù)點(diǎn)的曲率特征和特定鄰域內(nèi)點(diǎn)的數(shù)量來(lái)篩選特征點(diǎn)，并進(jìn)行主成分分析提取特征向量進(jìn)行配準(zhǔn)，但在提取特征向量的過(guò)程中容易忽略點(diǎn)云形狀特征。劉今越等［8］提出了一種基于曲率閾值的配準(zhǔn)方法，求得點(diǎn)曲率并設(shè)置閾值簡(jiǎn)化點(diǎn)云，進(jìn)而輸入點(diǎn)云特征點(diǎn)進(jìn)行配準(zhǔn)，但執(zhí)行過(guò)程中配準(zhǔn)時(shí)間大大增加。

針對(duì)點(diǎn)云配準(zhǔn)精度低、易誤匹配以及基于單一特征點(diǎn)對(duì)點(diǎn)云特征描述存在誤差等問(wèn)題，本文提出一種匹配精度較高的基于點(diǎn)云組合特征和主成分分析的點(diǎn)云配準(zhǔn)方法，提取點(diǎn)云ISS 關(guān)鍵點(diǎn)并結(jié)合AC 法提取輪廓點(diǎn)，組成能夠精確描述點(diǎn)云外部輪廓和內(nèi)部形態(tài)的特征點(diǎn)，在粗配準(zhǔn)階段采用結(jié)合PCA 改進(jìn)的SAC-IA 算法調(diào)整點(diǎn)云位姿，完成初始配準(zhǔn)。精配準(zhǔn)階段采用結(jié)合KD-Tree 改進(jìn)的ICP 算法。實(shí)驗(yàn)結(jié)果表明，與ICP、NDT、Super 4PCS、ISS+SAC-IA+ICP 等算法相比，本文算法配準(zhǔn)精度更高、誤差更小。

1 算法基本原理

1.1 組合特征點(diǎn)提取

1.1.1 ISS特征點(diǎn)檢測(cè)

ISS特征點(diǎn)是一種用于三維點(diǎn)云數(shù)據(jù)處理的特征點(diǎn)提取方法，通過(guò)點(diǎn)特定鄰域信息和特征值來(lái)提取點(diǎn)特征。通過(guò)ISS 提取的特征點(diǎn)更加精簡(jiǎn)，在后續(xù)處理過(guò)程中可以減少算法計(jì)算量和時(shí)間［9-13］。提取主要步驟如下：

1）記點(diǎn)云Q中n個(gè)點(diǎn)中一點(diǎn)為（xi，yi，zi），n=1，2，3，4，…，N，對(duì)其中的每個(gè)點(diǎn)都設(shè)置搜索半徑，記Q'為其鄰域內(nèi)點(diǎn)的集合，采用式（1）計(jì)算鄰域內(nèi)點(diǎn)的權(quán)值并記為wij：

其中，r為搜索半徑；Qi為點(diǎn)云Q中某一點(diǎn)的鄰域點(diǎn)；為Qi鄰域內(nèi)一點(diǎn)為2點(diǎn)間的歐氏距離。

2）計(jì)算點(diǎn)云Q中每個(gè)點(diǎn)與其對(duì)應(yīng)的Q'內(nèi)每個(gè)點(diǎn)的協(xié)方差矩陣，以及其對(duì)應(yīng)的特征值按照大小順序進(jìn)行排列，如果滿足式（3）即為提取的ISS特征點(diǎn)。

其中，Cov（Qi）為協(xié)方差矩陣；T 為矩陣的轉(zhuǎn)置為協(xié)方差矩陣的特征向量；μ1、μ2分別為設(shè)置的閾值。

本文實(shí)驗(yàn)采用Cat 和Michael 點(diǎn)云數(shù)據(jù)，圖1 和圖2 分別為Cat 和Michael 原始配準(zhǔn)點(diǎn)云圖像和ISS 關(guān)鍵點(diǎn)提取效果。

圖1 原始配準(zhǔn)點(diǎn)云圖

圖2 ISS關(guān)鍵點(diǎn)提取效果圖

1.1.2 AC法提取輪廓點(diǎn)

目前點(diǎn)云輪廓點(diǎn)［14］提取的2 種主流方法分別是AC（Angle Criterion）法 和HDC（Halfdisc Criterion）法，但在PCL（Point Cloud Library）點(diǎn)云庫(kù)中只集成了AC 法，且該方法基于點(diǎn)云法線特征實(shí)現(xiàn)輪廓點(diǎn)提取，相比于HDC 法簡(jiǎn)單有效，提取的邊界較為清晰。點(diǎn)云的法線是垂直于與點(diǎn)云表面相切的平面的向量，采用AC 法提取輪廓點(diǎn)主要是通過(guò)計(jì)算得出點(diǎn)云法線，再由得到的法線結(jié)合鄰域內(nèi)點(diǎn)云數(shù)據(jù)估計(jì)點(diǎn)云邊界。具體實(shí)驗(yàn)步驟如下：

1）記點(diǎn)云Q中n個(gè)點(diǎn)中一點(diǎn)為（xi，yi，zi），n=1，2，3，4，…，N，其中Qi=（xi，yi，zi），對(duì)點(diǎn)Qi設(shè)置搜索半徑r，并將半徑內(nèi)的鄰域點(diǎn)集記為。設(shè)表示點(diǎn)云曲面的方程為ax+by+cz=h（h≥0，a2+b2+c2=1），計(jì)算點(diǎn)集到該曲面方程的距離并記為，此時(shí)求解對(duì)應(yīng)的特征向量n即為當(dāng)前點(diǎn)Qi的法向量。根據(jù)當(dāng)前點(diǎn)Qi和其法向量n做切平面γ，并將其鄰域r內(nèi)的點(diǎn)集向切平面γ做投影，且投影點(diǎn)記為

采用AC 法對(duì)Cat 和Michael 點(diǎn)云數(shù)據(jù)提取輪廓點(diǎn)，圖3為2幅點(diǎn)云輪廓點(diǎn)提取效果圖。

圖3 點(diǎn)云輪廓點(diǎn)提取效果圖

1.2 FPFH特征點(diǎn)描述

點(diǎn)特征直方圖PFH 和快速點(diǎn)特征直方圖（Fast Point Feature Histogram，F(xiàn)PFH）是PCL 庫(kù)中重要的特征描述子，主要用來(lái)對(duì)點(diǎn)云關(guān)鍵點(diǎn)進(jìn)行描述，實(shí)現(xiàn)后續(xù)點(diǎn)到點(diǎn)的配準(zhǔn)［15-16］。其中FPFH 是在PFH 基礎(chǔ)上改進(jìn)而來(lái)的，在實(shí)現(xiàn)了PFH 功能外大大增加了計(jì)算速度［17-19］，在點(diǎn)云三維重建、點(diǎn)云配準(zhǔn)、點(diǎn)云分割等一系列操作中發(fā)揮著至關(guān)重要的作用。FPFH點(diǎn)云特征具體描述步驟如下：

1）記點(diǎn)云P中n個(gè)點(diǎn)中一點(diǎn)為（xi，yi，zi），n=1，2，3，4，…，N，其中Pi=（xi，yi，zi），對(duì)點(diǎn)Pi設(shè)置搜索半徑r，記搜索半徑r內(nèi)鄰域點(diǎn)數(shù)目為k，并記為Pk，對(duì)于點(diǎn)云中每個(gè)點(diǎn)的鄰域點(diǎn)計(jì)算法線偏差，并用特征（α，?，θ）表示，該特征記為SPFH。設(shè)Pi有鄰域點(diǎn)Pα，ni、na分別為Pi，Pa的法線，在點(diǎn)Pi上建立局部坐標(biāo)系（u，v，w），令u=ni，則，w=u×v，得α=v·na，，θ=arctan（w·na，u·na），其中d為點(diǎn)Pi和點(diǎn)Pα的歐氏距離

2）對(duì)點(diǎn)Pi分別搜索其鄰域內(nèi)近鄰點(diǎn)Pk，通過(guò)計(jì)算其SPFH 來(lái)計(jì)算FPFH，其中wk為權(quán)重，表示查詢點(diǎn)Pi與鄰域點(diǎn)Pk之間的距離。計(jì)算FPFH如式（4）：

1.3 點(diǎn)云主成分分析

主成分分析法（Principal Component Analysis，PCA）可以在保留原始數(shù)據(jù)變異性的前提下通過(guò)線性投影將高維空間的數(shù)據(jù)映射到低維空間中［20-22］。在點(diǎn)云配準(zhǔn)過(guò)程中使用主成分分析法可以確定點(diǎn)云主軸方向，具體實(shí)現(xiàn)步驟如下：

1）設(shè)2幅點(diǎn)云P、Q分別含有M、N個(gè)點(diǎn)，首先利用式（5）計(jì)算2 幅點(diǎn)云的質(zhì)心并記為，利用式（6）計(jì)算點(diǎn)云協(xié)方差矩陣并記為CP、CQ，利用式（7）對(duì)CP、CQ進(jìn)行奇異值分解求解兩者特征向量并記為NP、NQ，兩者均為3×3矩陣，即為點(diǎn)云主方向。

其中，DP、DQ為CP、CQ的奇異值矩陣；VP、VQ為CP、CQ的右奇異值矩陣；T 為矩陣的轉(zhuǎn)置；RO、TQ為點(diǎn)云初始旋轉(zhuǎn)矩陣和平移矩陣。

2）由于使用主成分分析法可能存在主軸相反的情況，因此需要對(duì)點(diǎn)云初始旋轉(zhuǎn)平移矩陣進(jìn)行修正，將點(diǎn)云P經(jīng)過(guò)RO、TQ旋轉(zhuǎn)平移得到點(diǎn)云，利用式（9）計(jì)算P'在Q中對(duì)應(yīng)點(diǎn)集的配準(zhǔn)誤差，利用式（10）計(jì)算2 組點(diǎn)云的平均均方誤差，分別記為Erro'和Erro。提出點(diǎn)云P主方向NP的3 組列向量即為點(diǎn)云在3 個(gè)坐標(biāo)軸上的主方向，由于主軸存在相反的情況，因此點(diǎn)云初始旋轉(zhuǎn)平移矩陣存在8 種情況，根據(jù)式（9）、式（10）計(jì)算最小誤差，求得對(duì)應(yīng)正確的旋轉(zhuǎn)平移矩陣。

1.4 本文所提算法流程及步驟

基于組合特征點(diǎn)和主成分分析的點(diǎn)云配準(zhǔn)算法主要包含“粗配準(zhǔn)”和“精配準(zhǔn)”2 個(gè)階段，由“粗”到“細(xì)”進(jìn)行配準(zhǔn)，其中算法具體流程如圖4所示。

圖4 本文所提算法流程圖

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

點(diǎn)云粗配準(zhǔn)可以將具有重疊區(qū)域的點(diǎn)云數(shù)據(jù)進(jìn)行初步擬合，為點(diǎn)云精配準(zhǔn)提供良好的位姿，在很大程度上避免點(diǎn)云精配準(zhǔn)陷入局部最優(yōu)狀態(tài)。隨機(jī)采樣一致性算法為RANSAC，是用于從一組數(shù)據(jù)中估計(jì)模型參數(shù)的方法。采樣一致性初始配準(zhǔn)算法（SACIA）是基于RANSAC 算法的一種對(duì)齊算法，通過(guò)點(diǎn)云特征描述子和點(diǎn)云法線的計(jì)算最終可求得旋轉(zhuǎn)平移矩陣，完成點(diǎn)云初配準(zhǔn)［23-25］。在點(diǎn)云粗配準(zhǔn)階段通過(guò)主成分分析對(duì)點(diǎn)云數(shù)據(jù)進(jìn)行維度約束確定點(diǎn)云主軸方向，可以降低算法配準(zhǔn)過(guò)程的迭代次數(shù)。本文點(diǎn)云粗配準(zhǔn)使用改進(jìn)后的采樣一致性配準(zhǔn)算法，主要包含如下2個(gè)步驟：

1）首先對(duì)輸入的點(diǎn)云分別提取ISS_BDRY 特征點(diǎn)并計(jì)算法線和FPFH 特征，之后進(jìn)行主成分分析，計(jì)算點(diǎn)云誤差最小的主方向，計(jì)算求得點(diǎn)云的旋轉(zhuǎn)矩陣Ro和平移矩陣To，為采樣一致性初始配準(zhǔn)算法提供良好的初始配準(zhǔn)位姿。

2）將步驟1中求得的Ro和To應(yīng)用于點(diǎn)云采樣一致性初始配準(zhǔn)，從待配準(zhǔn)點(diǎn)云中選取一定數(shù)量滿足距離閾值要求的采樣點(diǎn)，以確保選取的采樣點(diǎn)具有不同的FPFH特征。從目標(biāo)點(diǎn)云中查找與源點(diǎn)云中具有相似特征的點(diǎn)，并從中選取一點(diǎn)作為源點(diǎn)云在目標(biāo)點(diǎn)云中的對(duì)應(yīng)點(diǎn)，針對(duì)對(duì)應(yīng)點(diǎn)對(duì)計(jì)算點(diǎn)云之間的旋轉(zhuǎn)矩陣R1和平移矩陣T1，并計(jì)算配準(zhǔn)誤差來(lái)判斷當(dāng)前點(diǎn)云匹配精度。

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

經(jīng)過(guò)改進(jìn)的采樣一致性初始配準(zhǔn)算法后，源點(diǎn)云和目標(biāo)點(diǎn)云已經(jīng)大致對(duì)齊，但點(diǎn)云之間仍然存在錯(cuò)位情況，在配準(zhǔn)精度上還有很大的上升空間。為了提升點(diǎn)云配準(zhǔn)精度和速度，采用添加KD-Tree 加速搜索點(diǎn)云的ICP（Iterative Closest Point）算法進(jìn)行精配準(zhǔn)［26-27］。KD-Tree 是一種在K維空間中進(jìn)行范圍搜索和最近鄰搜索的數(shù)據(jù)結(jié)構(gòu)。點(diǎn)云精配準(zhǔn)具體實(shí)現(xiàn)步驟如下：

1）設(shè)源點(diǎn)云與目標(biāo)點(diǎn)云分別為P、Q，分別含有m、n個(gè)點(diǎn)，將經(jīng)過(guò)粗配準(zhǔn)變換得到的旋轉(zhuǎn)矩陣R1和平移矩陣T1應(yīng)用于源點(diǎn)云，并進(jìn)行比較，滿足距離小于閾值的點(diǎn)，則為對(duì)應(yīng)點(diǎn)。

2）根據(jù)求得的對(duì)應(yīng)點(diǎn)計(jì)算旋轉(zhuǎn)矩陣R2和平移矩陣T2，并將所求矩陣重新應(yīng)用于點(diǎn)云，此時(shí)源點(diǎn)云與目標(biāo)點(diǎn)云位置發(fā)生新的變化，再根據(jù)步驟1 計(jì)算新的對(duì)應(yīng)點(diǎn)對(duì)和旋轉(zhuǎn)平移矩陣，不斷迭代求解，直至點(diǎn)云旋轉(zhuǎn)平移矩陣滿足目標(biāo)函數(shù)式（11），或者點(diǎn)云對(duì)應(yīng)點(diǎn)對(duì)不再變化，算法迭代結(jié)束。

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

2.1 評(píng)價(jià)指標(biāo)

本文算法配準(zhǔn)精度評(píng)價(jià)指標(biāo)采用的是點(diǎn)云配準(zhǔn)過(guò)程中最常用的指標(biāo)RMSE（均方根誤差），定義如式（12）：

其中，n為2 組點(diǎn)云對(duì)應(yīng)點(diǎn)對(duì)數(shù)量，Xi為對(duì)應(yīng)點(diǎn)對(duì)之間的歐氏距離為對(duì)應(yīng)點(diǎn)對(duì)歐氏距離的真值。RMSE反應(yīng)了2 組點(diǎn)云的匹配程度，RMSE 值越接近于0 說(shuō)明2組點(diǎn)云匹配精度越高。

2.2 特征點(diǎn)提取實(shí)驗(yàn)與結(jié)果分析

單一特征點(diǎn)在描述點(diǎn)云形狀中往往存在一定的誤差，因而本文提出的ISS-BDRY 組合特征點(diǎn)引入BDRY 特征點(diǎn)充分描述點(diǎn)云空間結(jié)構(gòu)信息。為驗(yàn)證本文提出的ISS-BDRY 特征點(diǎn)在點(diǎn)云配準(zhǔn)方面的優(yōu)越性，設(shè)置如下實(shí)驗(yàn)：

1）在提取組合特征點(diǎn)過(guò)程中，分別設(shè)置不同閾值，并進(jìn)行點(diǎn)云初始配準(zhǔn)實(shí)驗(yàn)，根據(jù)配準(zhǔn)精度選擇合適的提取閾值，實(shí)驗(yàn)結(jié)果如圖5所示。

圖5 不同閾值輪廓點(diǎn)配準(zhǔn)精度

2）選取合適的閾值后分別提取點(diǎn)云ISS、BDRY、HARRIS、SIFT、ISS_BDRY 特征點(diǎn)進(jìn)行對(duì)比實(shí)驗(yàn)，并結(jié)合SAC-IA 進(jìn)行初始配準(zhǔn)，從配準(zhǔn)精度和配準(zhǔn)時(shí)間2個(gè)指標(biāo)進(jìn)行特征點(diǎn)評(píng)價(jià)，實(shí)驗(yàn)結(jié)果如表1所示，點(diǎn)云ISS_BDRY特征提取結(jié)果如圖6所示。

表1 Cat和Michael點(diǎn)云不同特征點(diǎn)配準(zhǔn)精度比較

圖6 點(diǎn)云組合特征點(diǎn)圖

根據(jù)表1 的實(shí)驗(yàn)結(jié)果可見(jiàn)本文提出的組合特征點(diǎn)相比于其他單點(diǎn)特征在Cat 和Michael 點(diǎn)云配準(zhǔn)精度上達(dá)到了10-8數(shù)量級(jí)，在配準(zhǔn)時(shí)間消耗上相比于單點(diǎn)特征增加了一個(gè)數(shù)量級(jí)。

2.3 本文配準(zhǔn)算法實(shí)驗(yàn)結(jié)果分析

為驗(yàn)證本文所提出算法的有效性，通過(guò)對(duì)比算法精度和運(yùn)行時(shí)間來(lái)評(píng)價(jià)不同算法的配準(zhǔn)效果，主要設(shè)置如下實(shí)驗(yàn)：

1）對(duì)點(diǎn)云數(shù)據(jù)提取組合特征并分別采用SAC-IA和改進(jìn)后的配準(zhǔn)算法PCA+SAC-IA 進(jìn)行點(diǎn)云初配準(zhǔn)對(duì)比實(shí)驗(yàn)，將Cat和Michael源點(diǎn)云與目標(biāo)點(diǎn)云特征點(diǎn)經(jīng)過(guò)主成分分析后確定的最小誤差的主軸初始位姿對(duì)應(yīng)的變換矩陣代入原始點(diǎn)云進(jìn)行初始配準(zhǔn)，其配準(zhǔn)結(jié)果如表2所示。

表2 Cat和Michael初始配準(zhǔn)精度比較

由表2 中點(diǎn)云配準(zhǔn)精度可得，結(jié)合主成分分析的改進(jìn)后的采樣一致性初始配準(zhǔn)算法在Cat 點(diǎn)云上相比于原始采樣一致性初始配準(zhǔn)算法在配準(zhǔn)精度上提升了65.19%，在Micheal 點(diǎn)云上提升了44.77%，在配準(zhǔn)時(shí)間上有所增加但變化較小。

2）將本文算法同傳統(tǒng)的ICP、NDT、Super 4PCS以及改進(jìn)后的ISS+SAC-IA+ICP、ISS_BDRY+SACIA+ICP、ISS_BDRY+PCA+SAC-IA+NDT 等算法進(jìn)行對(duì)比分析，算法對(duì)比結(jié)果如表3所示。

表3 Cat和Michael不同算法配準(zhǔn)比較

由表3 不同算法配準(zhǔn)精度可見(jiàn)，相比于ICP、NDT、Super 4PCS、ISS+SAC-IA+ICP、ISS_BDRY+SAC-IA+ICP、ISS_BDRY+PCA+SAC-IA+NDT 算 法，本文所提算法在Cat 和Michael 點(diǎn)云配準(zhǔn)精度上達(dá)到了10-16數(shù)量級(jí)，源點(diǎn)云和目標(biāo)點(diǎn)云幾乎完全重合，時(shí)間上相比于ICP、NDT、Super 4PCS、ISS+SAC-IA+ICP算法增加較多，但同其他改進(jìn)算法相比所差無(wú)幾。

使用本文算法對(duì)點(diǎn)云進(jìn)行配準(zhǔn)后的效果如圖7所示。

圖7 點(diǎn)云精配準(zhǔn)效果圖

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

由于點(diǎn)云數(shù)據(jù)密集且特征信息豐富，本文設(shè)計(jì)了一種基于點(diǎn)云組合特征點(diǎn)和主成分分析的點(diǎn)云配準(zhǔn)算法，將點(diǎn)云ISS 特征同輪廓特征相結(jié)合組成ISS_BDRY 組合特征點(diǎn)，充分提取點(diǎn)云空間特征信息，并利用主成分分析法改進(jìn)SAC-IA 算法，進(jìn)行點(diǎn)云數(shù)據(jù)降維確定點(diǎn)云主軸方向，為點(diǎn)云配準(zhǔn)提供空間信息和良好的位姿。相比于傳統(tǒng)ICP、NDT、Super 4PCS 以及其他改進(jìn)算法，本文算法在Cat 和Michael點(diǎn)云數(shù)據(jù)上配準(zhǔn)精度上達(dá)到了10-16數(shù)量級(jí)，可實(shí)現(xiàn)幾乎完全重合的點(diǎn)云配準(zhǔn)。該算法可以為點(diǎn)云三維重建提供一定參考，但是在時(shí)間消耗上還需要進(jìn)一步優(yōu)化。