基于局部幾何特征的稠密點云配準方法

朱一帆，裴 凌，吳 奇，夏宋鵬程，李 濤，陳 雷,2，郁文賢

(1.上海交通大學上海市北斗導航與位置服務重點實驗室，上海 200240； 2. 北京跟蹤與通信技術研究所，北京 100094)

0 引言

近年來，隨著傳感器技術的進步，三維點云數(shù)據(jù)在精度和稠密度方面獲得了大幅提升[1]，三維點云數(shù)據(jù)作為重要的空間結構數(shù)據(jù)，被廣泛地應用于自主導航和三維場景重建等。對稀疏的三維激光點云配準問題已經進行了較為深入的研究，稠密的三維點云的配準問題成為了研究熱點。

迭代最鄰近點 (Iterative Closest Point，ICP)算法是解決點云配準問題的核心算法。該算法采用點點(點面)距離尋找鄰近點,再通過最小二乘法求取點云間的旋轉和平移矩陣，以達到點云配準的目的,其實質是一種迭代收斂的算法[2]。隨著三維點云數(shù)據(jù)稠密程度的提高，現(xiàn)有的基于ICP算法的三維點云配準方法由于計算量大、 依賴初始位置設定等因素，配準成功率不高。

在視覺同時定位與建圖(Visual Simultaneous Localization and Mapping，VSLAM)中[3-4]，通常對點云數(shù)據(jù)進行局部特征抽取，再基于特征進行配準，可以大幅減少點云數(shù)量，提高配準的魯棒性。由于三維點云的無序性和傳感器的噪聲，點云局部特征的描述工作相對圖像特征要困難很多，這也造成了現(xiàn)有的點云描述子的魯棒性不足，在進行點云配準的過程中位姿恢復精度較低[5]。隨著深度學習技術的興起，基于數(shù)據(jù)驅動的模型被用于三維點云局部特征的描述上，并且表現(xiàn)非常出色。實驗證明，基于數(shù)據(jù)驅動的模型對點云局部的幾何結構能夠進行有效的描述，并在特征空間上能夠很好地進行聚類，效果優(yōu)于現(xiàn)有的手動設計的描述子[6]。

基于上述原因，本文提出了一種基于點云局部幾何特征的稠密點云配準方法。該方法借鑒VSLAM算法中圖像匹配的思路[7]，對點云進行局部特征抽取和描述，然后進行位姿估算。在局部特征提取方面，使用深度卷積網絡對特征進行描述，以減少三維點云數(shù)據(jù)的噪聲、低分辨率和不完備性等帶來的影響。該方法的優(yōu)勢在于精度高、計算量低，且能夠匹配位姿差異大的點云。實驗結果驗證了該方法的有效性、實時性和魯棒性。

1 算法設計

點云的配準工作主要可以描述為：求解兩幀三維點云的旋轉平移矩陣(6個自由度)，使兩幀點云能夠對齊到同一個坐標系上。本文提出的點云配準方法技術路線如圖1所示。對于給定的兩幀點云，分別計算其特征點和特征描述之后，通過隨機采樣一致(Random Sample Consensus，RANSAC)算法對兩幀三維點云的旋轉平移矩陣進行魯棒的估計。

圖1 3D點云配準方法算法流程Fig.1 3D point cloud registration algorithm flow

1.1 特征點檢測

三維點云(P={p1,p2,…，pn})可以被描述為一些離散三維點(pi=[xi,yi,zi]T,xi,yi,zi∈R)的集合。其中特征點檢測算法的目的是尋找該集合的一個子集(K?P)，使其能夠盡可能地代表原來集合的信息。對于點云配準這一具體應用，所選取的關鍵點要滿足以下要求：1)特征點的提取算法要求效率高；2)在不同的視點下都能提取到大部分相同的特征點；3)提取出的特征點應位于表面穩(wěn)定的區(qū)域，以保證穩(wěn)定的特征提取。

法線對齊的徑向特征(Normal Aligned Radial Feature，NARF)是一種3D特征點檢測和描述算法[8]。其特征點選取算法能夠很好地滿足上述要求，因此，在本文的點云配準方法中選用NARF特征點。首先，將目標點云轉化為距離圖像便于快速檢索以保證要求1)；然后，對于每一個三維點pi，選擇其在距離圖像上的一個方形窗(窗寬為：s)內的鄰域點，并計算其與pi的三維距離；之后按照式(1)計算三維點pi的分數(shù)

(1)

其中，di是pi在距離圖像上與其上下左右4個方向上的鄰接像素點的平均三維距離；dM是方形窗內一系列點中距離pi第M遠的點與pi的三維距離，其中M=(0.5(s+1))2。當分數(shù)高于一定閾值時，認為三維點pi滿足要求2)，并將其列入候選關鍵點列表。對于候選關鍵點列表中的pi，在點云中找到其在半徑σ中的所有領域點：n0,…,nk。評分函數(shù)I(p)被用以評估三維點pi對要求3)的滿足情況，定義為

I(p)=I1(p)I2(p)

(2)

其中，α是pi和其所有領域點中不是邊界點的主方向；wn是一個權重，對于邊緣點取值為1，非邊緣點取值為1-(1-λ)3，λ代表非邊緣點的曲率大小。當候選關鍵點列表中的某個pi的分數(shù)I(pi)大于一定閾值時，該點被選為特征點。NARF特征點提取流程如圖2所示。

圖2 NARF特征點提取流程Fig.2 NARF feature point extraction process

1.2 特征描述

特征描述的作用在于對屬于特征點序列中的每一個三維點(pi∈K)進行編碼，該編碼便于在不同幀之間進行匹配。對于點云配準這一具體應用，選取的關鍵點要滿足以下要求：1)提取的描述子具有一定的抗噪聲性能，且能夠克服點云低分辨率和不完備性等帶來的影響；2)提取的點云具有一定的角度不變性，能夠在不同視角下完成匹配。對于要求1)，本文提出將特征點的鄰域信息以體素格的數(shù)據(jù)形式進行表征，以克服噪聲帶來的影響；對于要求2)，本文使用基于數(shù)據(jù)驅動的描述子，以保證描述子的旋轉不變性。

1.2.1 特征點體素化

對于一個特征點pi∈K，在點云中找到其在半徑σ(在本文中σ=0.15m)內的所有領域點：n0,…，nk。以pi為中心構造一個n×n×n的體素格(在本文中n=30)，每個體素格中存儲截斷距離函數(shù)(Truncated Distance Function，TDF)值。TDF值代表了每個體素格中心相對最近的三維點云表面的距離。同時，這些TDF值被截斷在1(在點云表面上)和0(遠離點云表面)之間。對于體素vi，其TDF值由式(3)進行計算

(3)

圖3 TDF體素格計算流程Fig.3 TDF voxel lattice calculation process

1.2.2 描述子計算

對于每個特征點生成大小為n×n×n的TDF體素格，包含了該特征點的局部幾何特征，描述子計算的目的在于給該特征進行編碼，以便于后續(xù)的特征匹配。受到3DMatch工作的啟發(fā)[9]，本文使用3D卷積網絡對TDF進行特征抽取，網絡結構如圖4所示。該網絡包含8個卷積層和1個池化層，能夠將一個303的TDF體素格轉化為512維的描述向量(卷積層與池化層的卷積核大小和通道數(shù)已標注于圖中)。

圖4 描述子計算網絡Fig.4 Descriptor extraction network

(4)

1.3 基于K-d Tree的特征關聯(lián)

特征關聯(lián)的目的是在待配準的點云P與Q的關鍵點子集KP和KQ中找到特征最為相近的點，以完成兩幀點云的特征匹配。其本質是一個通過距離函數(shù)(歐式距離)在高維矢量之間進行相似性檢索的問題， 使用K維樹搜索算法能夠高效地實現(xiàn)上述過程，該算法的具體流程敘述如下，流程圖如圖5所示。

圖5 基于K-d Tree的特征關聯(lián)算法流程圖Fig.5 Flow chart of feature association algorithm based on K-d Tree

首先，利用1.1節(jié)與1.2節(jié)提到的算法分別對點云P與Q提取關鍵點構成集合KP和KQ，并計算每個特征點的描述子分別構成集合DP和DQ。Kd-tree是每個節(jié)點都為k維點的二叉樹。由于本文的描述子維度為512維，因此在Kd-tree構建部分對應為根據(jù)DP和DQ分別構建512維空間的二叉樹，其構建過程可以參考Bentley提出方法[10]中的第3節(jié)。

在構建完成Kd-tree之后，分別在DP和DQ中進行最臨近搜索，以確定在特征空間中最靠近的特征點，并得到相互匹配的三維點的集合MP與MQ，其實現(xiàn)過程可以參考Bentley提出方法[10]中的第4節(jié)。

1.4 基于RANSAC算法的位姿恢復

在基于K-d Tree的特征關聯(lián)之后，可以得到兩幀點云中兩兩匹配的點云集合?；谏鲜鼋Y果，可以得到目標點云P與點云Q的相對位姿，進一步可以得到點云配準的結果。該算法具體敘述流程如下。

首先，利用1.3節(jié)提到的特征關聯(lián)方法得到兩幀點云中兩兩匹配的三維點集合MP與MQ；然后對于MP與MQ中的三維點，計算2組點的質心坐標p和q；再計算每個點的去質心坐標，如式(5)所示

qi←qi-qpi←pi-p

(5)

(6)

在現(xiàn)實世界中，由于場景和特征的多樣性，本來應該一一匹配的MP與MQ不可避免地存在誤匹配?？紤]到這一情況，本文使用RANSAC[11]機制對MP與MQ的點進行過濾,以獲得目標點云準確的位姿。RACSAC機制可以從一組包含局外點的觀測數(shù)據(jù)集中，通過迭代方式估計數(shù)學模型的參數(shù)。在本文的應用中，可以利用迭代和概率的方法，從有誤匹配的數(shù)據(jù)中估計出點云的相對位姿。引入RANSAC機制的目標點云的位姿恢復算法的偽代碼如表1所示。

2 實驗和分析

為了充分評估所提方法的性能，本文設計并進行了一系列實驗，主要驗證以下3個方面:1)所提點云配準方法的成功率；2)所提方法位姿估計的準確性；3)所提方法的配準效率。

表1 基于RANSAC 算法的位姿恢復算法

2.1 實驗設置

本文在公開數(shù)據(jù)集7-Scenes[12]與SUN3D[13]上對所提出的方法進行驗證和評估。實驗中的幾個場景樣例如圖6所示，每個場景都包含了不同的分辨率、使用不同的重構算法創(chuàng)建。這些場景具備不同的傳感器噪聲、視角和遮擋方式等情況，能夠很好地驗證算法。值得注意的是，圖中的顏色信息只被用于可視化，在實驗中沒有用到。

圖6 實驗數(shù)據(jù)集Fig.6 Data sets for experiment

本文同時給出了一些參與運算的關鍵參數(shù)和閾值，具體如表2所示。

表2 關鍵參數(shù)設計

2.2 成功率分析

本節(jié)對所提出的點云配準算法的穩(wěn)定性進行了評估，并與傳統(tǒng)NARF描述子和ICP算法進行了比較。本文從7-Scenes與SUN3D數(shù)據(jù)集中選取了8個場景，并從每個場景中選取5對不連續(xù)點云(平移和旋轉較大)進行配準的評估(一共有40對點云參與評估)。本文使用Choi提出的評判標準判斷點云配準是否成功。對于兩幀待匹配的點云(Pi，Pj)，如果滿足式(7)的條件，則認為算法所估計的Tij是正確的

(7)

將本文提出的算法與ICP算法、NARF算法進行對比，并將三種算法在數(shù)據(jù)集上的成功率列舉如表3所示。同時，圖7給出了數(shù)據(jù)集中一些具有挑戰(zhàn)性的配準案例的示意圖，其中第一列為本文提出的配準結果，第二列為NARF配準結果，第三列為ICP配準結果。

表3 點云配準成功率

圖7 配準結果樣例Fig.7 Samples of point cloud registration results

實驗結果表明：在給定的數(shù)據(jù)集上，本文提出的算法配準成功率最高。其中ICP算法表現(xiàn)最差，其原因在于數(shù)據(jù)集中待配準點云稠密且相對旋轉和平移較大，該條件不適應ICP算法的工作(點云稀疏且相對旋轉與平移較小)[14]。本文提出的算法能夠在給定數(shù)據(jù)集中魯棒性優(yōu)于NARF算法的原因在于描述方法的穩(wěn)定性。在實驗中可以觀察到，使用本文的描述子進行匹配特征點對數(shù)是使用NARF描述子的1.61倍。

2.3 配準精度的比較

本節(jié)對所提出的點云配準算法的精度進行了評估，并與傳統(tǒng)NARF描述子和ICP算法進行了比較。本文選取了2.2節(jié)中各個算法的成功案例進行精度的評估。同樣使用RMSE對精度進行評價，定義如式(8)所示

(8)

表4 點云配準精度

試驗結果表明：在配準精度上，本文提出算法相對使用NARF描述的方法略優(yōu)，這是由于關鍵點的穩(wěn)定匹配的原因。更多的正確匹配點對能夠促進更為準確的位姿估計。ICP算法在精度上差于經典算法，其原因在于：由于點云的稠密性，ICP算法在最優(yōu)結果的鄰近位置難以找到正確的對應點對，導致無法進一步迭代優(yōu)化。

2.4 配準時間的比較

本節(jié)對所提出的點云配準算法的效率進行了評估，并與傳統(tǒng)NARF描述子和ICP算法進行了比較。本文進行實驗的計算機平臺的軟件硬件環(huán)境列舉如表5所示。

表5 實驗條件

本文同時統(tǒng)計了三種算法在該環(huán)境下進行點云配準的平均時間，結果如表6所示。

表6 配準時間比較

試驗結果表明，在稠密點云上，基于特征匹配方法的配準時間優(yōu)于基于迭代算法。本文提出的算法在數(shù)據(jù)集上能夠基本做到10.7021s/幀的配準速度，與NARF基本在一個水平。

3 結論

隨著點云傳感器的發(fā)展，三維點云數(shù)據(jù)的稠密性和測量精度將會在未來得到顯著提高。本文提出了一種基于點云局部幾何特征的稠密點云配準方法，探討了將基于深度學習的點云描述子運用于點云配準的問題。實驗結果表明，針對稠密非連續(xù)點云的配準問題，基于特征匹配的方法在效率和精度上都優(yōu)于基于ICP的方法；同時基于數(shù)據(jù)驅動的點云描述子在不損失配準精度的情況下，魯棒性明顯優(yōu)于手動設計的點云描述子。

該算法目前存在的問題有：為保證實時性需要GPU的支持、描述子提取需要進行前期訓練等。在未來，我們將改進算法，使之可進一步應用于不同類型的點云(激光雷達)；同時提升算法的實時性，將其應用于重定位、軌跡推算、回環(huán)檢測等問題中。