改進的采樣一致性點云配準算法

王月海，莊志鵬，邢 娜

(北方工業大學 信息學院，北京 100144)

0 引 言

點云配準是三維重建行業[1-4]的核心技術和理論研究基礎，一直備受學者關注[5-8]。迭代最近點配準算法是目前運用最為廣泛的點云配準算法，該算法經過在兩組點云之間探尋距離間隔最近的點對，并迭代計算出點對間的最優旋轉平移矩陣。比較經典的點云配準算法是Besl等提出的ICP點云配準算法，該算法配準精度比較高，但是算法要求待配準數據具有較好的初值，不然容易得到局部最優的配準結果，為此國內外學者紛紛提出許多富有創造性的改進算法[9-13]，文獻[10]使用內部形態描述子算法來提取點云的特征，進而使用采樣一致性配準算法實現點云的配準，但其并未對點云數據進行濾波，因此配準時間較長。文獻[11]在點云配準中，利用中心重合法，使得目標點云Q與待配準點云P的中心重合，降低兩組點云數據的平移偏差。對于旋轉偏差小的點云，該方法能以較快的速度完成配準，對于旋轉偏差過大的點云，配準精度就會降低。文獻[12]提出基于曲率極值的配準算法，針對曲率變換明顯的點云數據表現出較好的配準效果，但是對于含有大量噪聲的點云數據，算法配準質量較差。文獻[13]提出了一種將SAC-IA與ICP算法相融合的點云配準方法，該方法對點云數據的快速點特征直方圖FPFH[14-17]特征進行特征點的表示，隨后采用點云配準算法(sample consensus initial alignment，SAC-IA)進行特征點的匹配，由此得到點云之間的對應關系，最終計算出點云的初始變換，得到一個較好的配準結果，但該方法對于初始點云數據沒有做優化處理，因此配準時間較長，原始點云中的噪點也會影響配準精度。本文針對原始點云數據做優化處理，與隨機采樣一致性算法[13]不一樣，使用自適應體素網格濾波法，降低數據量級的同時，保留了點云數據的原始分布特征，在快速點特征直方圖求解階段引入距離的二次函數，降低遠距離鄰域點的權值，同時提高近距離鄰域點的權值，本算法整體相較于傳統算法有較大提升。

1 采樣一致性算法

點云數據的初始配準結果對于整個算法的最終配準結果的精度影響很大，采樣一致性初始配準算法，是目前初始配準當中精度較高的算法之一，該算法是基于快速點云特征直方圖FPFH的配準模式。通過計算每站點云的點特征直方圖來描述點云局部特征，并用這個特征作為標準，進行兩站點云的配準。經過多次迭代，選取其中最符合整體特征匹配的變換矩陣。在SAC-IA算法執行之前，首先需要計算點云的局部幾何特征描述符，算法基本流程為：

(1)選取采樣點：從源點云P中選取m個點，且每個點之間需要能滿足距離大于提前給定的最小距離閾值d，這是為了盡量保證所采集的點云之間處在不同的曲面上，使得點之間能具有不同的FPFH，減少內存消耗的同時增大采樣點集合的信息熵。

(2)查找采樣點的對應點：對于源點云P上的m個采樣點在目標點云Q中尋找與該采樣點具有類似特征的點集合，再隨機的從這些類似點里選擇一個點作為源點云P的采樣點在目標點云Q中的對應點。

(3)計算變換矩陣：計算源點云P上的m個采樣點與目標點云Q的對應點之間的旋轉變換矩陣，然后分別計算經過該變換之后的距離誤差和函數，對當前配準的精確情況進行判別。本算法使用的誤差和函數是Huber

(1)

式中：ml是一個給定的誤差閾值，取值0.001。li表示經過變換后第i組對應點之間的距離差值。配準過程中尋找可以使得距離誤差和函數Huber值達到最小的旋轉平移矩陣，將其作為最終的配準變換矩陣，最終求得配準結果。

2 自適應采樣一致性算法

2.1 自適應體素網格濾波法

由于通過三維激光雷達采集到的三維點云數據量極其龐大，平均每一幀點云數據的數量能達到上萬個點且伴隨著大量噪聲，預處理階段的三維點云數據與點云配準耗費的時長成正比，而噪聲的存在會降低配準的準確度，因此在對點云配準處理前，需要先進行數據預處理。在對點云數據進行預處理時，應當盡可能地保持原始點云曲面法向量以及點云密度分布的特點。若使用隨機采樣法，采樣點云的密度分布與原點云相差較大，而點云的曲面法向量等輪廓信息也難以得到完整保留，使用這種算法進行特征提取，極易得到特征扭曲的FPFH，不利于后續配準，因此需要一種既能減少點云數量，又能保留點云分布特征的濾波算法。

自適應體素網格濾波算法的基本原理是：通過邊長大小為L的立方體網格，將原始輸入點云等間距地分成N份，計算每個體素網格內點云的重心，將該重心替代網格內的點。具體計算方法：

(1)首先確定體素網格的邊長L，因為每個體素網格將保留一個重心點，所以立方體大小直接影響著采樣后點云分辨率的大小。L越大，采樣后的點云分辨率越低，與之相反，分辨率則越高。采樣立方體的個數為A×B×C， 計算方法如下

(2)

式中：xmax,xmin,ymax,ymin,zmax,zmin分別為X、Y、Z這3個坐標軸上點云坐標值相對應的最大值和最小值。

(2)將所有求得的重心代替當前體素立方體的點云，然后將得到的重心合并成新的點云，該點云即為采樣之后得到的稀疏點云數據。

傳統的體素網格濾波法對原點云實現采樣處理，是通過調整體素立方體大小的方式，得到合適的采樣點集。該方法在減少三維點云數量的同時，保留了原始三維點云特征，保證新的三維點云與原始三維點云的點特征直方圖在全局上近似。但這種方法需要手動規定體素網格立方體的邊長大小，對于不同的點云數據，通過相同邊長的體素網格立方體過濾之后的點云數量級將會是不確定的，而進行配準計算的點云數量級大小直接影響著算法的運算時間和運行效率，為有效控制三維點云配準算法的配準時間，需要得到確定的點云數量級大小。

因此本文在傳統體素網格濾波法上進行改進，針對輸入數量級未知的三維點云數據，自適應地求解出準確的體素立方體大小

maxts.t.F(k*ε+l)

(3)

式中：F表示體素網格濾波器，t表示動態迭代系數，ε表示體素立方體邊長的變化步長，l表示初始立方體邊長大小，Pmax表示通過濾波器采樣后得到的點云數量最大值。

自適應體素網格濾波法算法流程[1]：

步驟1 設置ε用于規定變化步長；設置Pmax大小，用于獲取期望的點云數量級。

步驟2 輸入原始三維點云數據P。

步驟3 如果原始三維點云數據P的三維點數量小于Pmax，直接跳轉到步驟5。

步驟4 如果點云數據P的點集數量大于Pmax，根據P的點集數量與Pmax的差值調整t，直到P的點集數量不大于Pmax為止。

步驟5 輸出采樣后的結果Pf。

使用本文提出的自適應體素網格濾波算法，在保證原始點云特征的前提下，能減少點云數量和計算時間，提高整個配準算法的效率。同時，本算法通過設定Pmax來確定濾波后得到的允許最大點云數量，解決了傳統體素網格濾波器對于體素立方體邊長的局限性，以自適應的體素立方體過濾出期望的點云數量。

2.2 特征點的提取

PFH作為一種較為健壯的三維點云特征描述符，用于表示查詢點pq的局部幾何形狀特征。如圖1所示，以查詢點pq為圓心，r為半徑形成的圓為pq點PFH特征的影響區域，在這個影響區域內，查詢點與全部鄰域點之間兩兩相連構成一個統一的集合。

圖1 PFH鄰域運算

通過圖2的局部坐標關系，可以將相鄰兩個三維點Ps(xs,ys,zs) 和Pt(xt,yt,zt) 的六元組通過式(4)進行降維處理，轉換為一個四元組 (α,φ,θ,d)

(4)

圖2 局部坐標系

SPFH(simple point feature histograms)是在原有的PFH基礎上進行簡化而產生的，其根本目標是為了降低算法的運算時間復雜度。對于包含n個點的點云P的PFH理論時間復雜度為O(nk2)，其中k為近鄰點個數。SPFH的算法思維是：查詢點計算三維點特征直方圖時將不會包含鄰域點與領域點之間的關聯信息，僅僅只是利用查詢點與鄰域點之間的關聯信息，圖3表示SPFH算法k鄰域運算關系。

圖3 SPFH鄰域運算

本文改進的快速點特征直方圖FPFH是PFH的簡化模式，它的思想是分別計算查詢點的k鄰域內每個點的簡化點特征直方圖SPFH，并用距離的二次方作為衡量鄰域點權重的系數。改進的FPFH為

(5)

式中：S(pq) 代表鄰域內每個點的簡單點特征直方圖，k代表鄰居點的數量，bi代表查詢點和第i個鄰居點之間的歐氏距離。相較于距離的一次函數，本算法使用的距離二次函數能進一步提升距離較近點的權值，同時弱化距離較遠點的權值，對于查詢點的特征表示更合理，使得對應點對的配對更加準確。

2.3 ICP算法

在經過粗配準之后得到的兩片點云已經基本配準，但仍然存在偏差，配準精度也較低，為了進一步提高配準精度，還需要使用精配準算法，本文使用ICP配準算法。

算法概述：傳統的迭代最近點ICP算法是一種十分經典的點云配準方法，將要拼接的兩對點集按照最近點配對原則確立對應點對qi與pi，對應點對的個數為N。最后再使用最小二乘法來最小化原始三維點云和目標三維點云數據之間的距離，計算出最優的坐標變換矩陣，得到其旋轉矩陣R和平移向量t，從而令誤差函數式(6)能夠取得最小值

(6)

式中： (qi,pi) 是目標點云和源點云的第i對點。

ICP算法步驟：

步驟1 初始輸入數據量大小為n的三維點云數據P。

步驟2 經過最近鄰搜索算法在三維目標點云Q之中找到與三維特征點pi對應的三維特征點qi, 使得pi與qi之間的歐幾里得距離最小。如果某一點對之間的歐幾里得距離滿足給定的閾值，則將這對點作為對應點對進行保存。

步驟3 由三維特征點qi和三維特征點pi得到旋轉矩陣R與平移向量t。

步驟4 將三維特征點云pi通過步驟3求得的R|t進行變換得到新的三維特征點云p′i。

步驟5 計算三維特征點p′i和三維特征點pi之間距離的均方誤差值:dis。

步驟6 最后依據dis是否為小于原始給定的閾值，或者其配準的迭代次數是否已經達到最初給定的最大迭代次數。符合其中一項，則就會終止迭代，否則將繼續反復迭代以上步驟。

2.4 本文算法描述

步驟1 輸入原始三維點云數據集P，和目標三維點云數據集Q；

步驟2 預設一個通過濾波器采樣之后得到的三維點云數量最大值Pmax，通過自適應體素網格濾波器濾波之后，得到一個輕量級且特征完整的三維點云數據Pf以及Qf；

步驟3 通過式(4)可以計算得到三維點云數據Pf和三維點云數據Qf內各鄰域點對之間的α,φ,θ, 并存入點特征直方圖所對應的子區間里；

步驟4 通過式(5)計算得到改進的快速點特征描述符P_fpfh(Pf的描述符)，Q_fpfh(Qf的描述符)；

步驟5 將含有特征描述符的點云進行采樣一致性配準得到旋轉平移矩陣sac_tran；

步驟6 導入待配準的三維點云數據，使用旋轉矩陣sac_tran作為初始旋轉矩陣，設置配準的最大迭代次數為10，兩次迭代的歐幾里得距離的最小變化量為1e-10， 進行迭代最近點配準；

步驟7 一旦達到最大迭代次數或最小變化值時，終止配準，輸出最終配準完成的點云。

3 實驗結果與分析

本文在CPU主頻為2.6 GHz,win10系統筆記本進行實驗開發平臺的搭建，在Visual studio 2017軟件上搭建了PCL點云庫的環境進行實驗。該實驗采用的驗證數據是來自斯坦福大學提供的公開三維點云數據模型。其中Bunny點云數據如圖4所示，Bunny點云數據約有4萬個點，序號分別為0和45，0號Bunny數據表示的是在規定零度位置采集到的點云數據，45號則表示在45度角位置采集到的點云數據模型。使用的衡量指標是點云數據之間的ESM均方誤差以及該算法運算所需的配準時間，并與對比文獻[13]進行比較。

圖4 Bunny原始數據

首先將原始三維點云數據0號Bunny數據和45號Bunny數據通過自適應體素網格濾波器進行數據的預處理，得到的三維點云采樣之后的結果如圖5和圖6所示，該方法采樣之后，將會得到一個數據量級較小的采樣數據組，再將該數據組進行改進的采樣一致性點云配準，配準效果如圖7所示。

圖5 目標點云采樣結果

圖6 源點云采樣結果

圖7 本文改進算法配準結果

圖5和圖6中所示的結果是在設置Pmax=1000的條件下得到的采樣結果，得到目標點云采樣數據的點數為986個點，源點云采樣數據的點數為948個。從采樣結果不難看出，Bunny數據在經過自適應體素網格濾波器后，得到的點云數據量被大幅度的縮減，與此同時仍然保留了原始點云數據的基本特征。

通過圖7可以看出采用本文提出的三維點云配準算法能夠將兩片Bunny三維點云數據進行準確配準，配準效果較好。由表1可知曉三維點云配準之后的點云之間的均方誤差將會縮小到3.5625e-5。該實驗的結果表明，本文所提出的改進的三維點云配準算法配準精確度高，表1中展示的是本文改進算法在精配準和粗配準之后的表現，與參考文獻[13]進行比較，可以得出本文提出的三維點云配準改進算法無論是在進行配準消耗的時間上，還是配準的精度上都有較高的提升，在配準時間上減少了39.39%，三維點云配準的整體均方誤差降低了54.65%。

為了驗證本文提出的改進算法在不同三維點云數據集上的有效性，本文也將使用斯坦福大學所提供的3組公開數據集Happy(開心佛陀)、Dragon(龍)以及Armadillo(犰狳)進行三維點云配準實驗，通過分析配準的均方誤差以及配準所需要的時間來判定該算法的性能優劣。其中點云數據開心佛陀約有8萬個點，使用的是第0號和第24號點云數據；點云數據龍約有4萬個點，使用的是第0號和第24號點云數據；點云數據犰狳約有3萬個點，使用的是第270號和第300號點云數據；實驗結果如下：

通過結合圖8和表2進行分析，評價指標為點云間距離的均方誤差以及配準所需要的時間，可以看出本文提出的改進算法在針對不同數據集進行配準所需要的時間基本能控制在10 s左右，得到的誤差均在1e-4 m內，配準精度高。

表1 本文改進算法配準結果與傳統配準算法比較

圖8 本文算法配準結果

為驗證二次距離權值對本算法精度的提升效果，使用不同的Pmax使得Bunny點云數據量級變化，分別使用一次、二次、三次距離的FPFH特征直方圖，進行特征表示，并完成配準，得到配準后的結果如圖9所示。分析圖9可以看出使用二次距離的FPFH特征直方圖得到的配準精度更高。相較于原始的一次距離的FPFH，在其它條件相同的情況下，二次距離的FPFH在精度上最高能提升35.33%。

表2 本文算法在不同數據上的配準效果

圖9 距離權值與Pmax關系

4 結束語

點云配準技術作為三維重建十分重要的一個環節，已經漸漸地成為三維重建行業研究的熱點問題。本文針對傳統算法收斂慢,算法復雜度高等問題，提出了自適應體素網格濾波算法，可以根據原始點云量級不同的大小自動修改體素立方體大小，有效剔除偏差較大噪點的同時還降低了點云數據量級，并能保留點云數據的根本特性，為后續配準階段提供了一個可靠且輕量級的點云數據。除此之外，在快速點特征直方圖求解階段，引入距離的二次函數，優化鄰域點的權值，提升距離較近點的權值，同時弱化距離較遠點的權值，對于查詢點的特征表示更合理，使得對應點對的配對更加準確，直接提高了配準的精度。實驗結果表明，該改進算法相較于傳統的點云配準算法在配準時間上減少了39.39%，整體配準均方誤差提升了54.65%。

本算法與前人去年提出的點云算法相比，在配準時間和配準精度上都有所提升，但依然有待解決的問題。對于本文所提出的自適應體素網格濾波法，雖然能降低點云數據的數量級，但在使用網格點集的重心作為網格內其余所有點的替代點，難免會引進新的誤差，這是在配準精度與配準效率上進行權衡的結果。本文算法在采樣一致性點云配準算法上做了改進，配準精度以及配準時間都有所提升，但總體運行時間仍然為秒級，無法實現實時點云配準，將來會持續致力于提升配準算法的效率。