基于RANSAC的點云配準優化算法

謝德芳，陳叢桂，周 聰，馬亮華，黎鑫澤

（廣州大學機械與電氣工程學院，廣州 510006）

0 引言

圖1 算法流程圖

1 濾波去噪

點云在特征提取、配準、曲面重建之前要進行預處理，預處理對點云處理有著重要意義，主要采用濾波去噪。

VoxelGrid[9]（體素網格）濾波有著很好的濾波效果，使用此濾波不僅能達到減少點云點集數目，也能保持點云的幾何形狀，維持原始形狀特征，對于點云配準是一種理想的濾波方式。

VoxelGrid 濾波工作原理：三維點云中的體素是三維空間中的最小分割單位，即相當于二維圖像中的像素。在輸入點云數據上創建一個個體素網格（將體素網格視為一組空間中的微小三維空間）。然后，在每個體素中，所有存在的點將用它們的質心近似。因此VoxelGrid濾波可以保持三維點云的宏觀幾何形狀。

2 RANSAC擬合

RANSAC 算法從一組數據集中，通過反復隨機選擇點集中的子集，達到擬合目標數學模型的效果。RANSAC 算法基本思想如下：假定目標數學模型，隨機選擇n個點，通過這n個點確定數學模型方程；在數據集選取點代入此數學模型方程，并計算誤差；找出所有在誤差范圍內的點——局內點，剔除局外點。在設定的迭代次數命令下重復上述過程，局內點最多的模型為最優數學模型。

迭代次數推導如下：

式中：n為假定模型需要選取點的數目；p為迭代過程中從數據集內隨機選取的點都為局內點的概率；ω為每次從數據集中選取一個局內點的概率，ω= 局內點數目/ 數據集數目，1-ωn是n個點中至少有1個點為局外點的概率；k為迭代次數。

形容詞的意動用法，是指主語主觀上斷定賓語擁有某種狀況，可以按照“認為賓語謂語”的格式來解釋。如:“不恥下問”的意思是一個人不認為請教比自己地位低下的人是可恥的。“恥”是形容詞的意動用法，解釋為:認為……是恥辱的事情。

對式（1）左右兩邊取對數得：

考慮到迭代過程中每個點的選取都是獨立的，某個點被選取之后，也可能會被選定，因此修正式（2），得修正后的迭代次數

3 粗配準

3.1 PFH和FPFH特征描述子

點云粗配準通過點的幾何特征進行，如法向量、曲率等。但點周圍的幾何特征數量多且相似度高，無法得到點云的全局特征信息，因此有了點特征直方圖PFH[10]（point feature histogram）。PFH 通過點和臨近點的空間差異作出幾何描述，PFH 提供的信息具有旋轉不變性，對于點云而言十分穩健。

如圖2 所示，Pq的PFH計算的影響區域，Pq用深色標注并放在圓球的中間位置，半徑為r，(Pq)的所有k鄰元素（即與點Pq的距離小于半徑r 的所有點）全部互相連接在一個網絡中。

FPFH（fast point feature histograms）在保持了PFH大部分特性的前提下，降低了算法的時間復雜度，提高了計算效率，本質上是PFH 的快速簡化模型。只需要計算Pq（查詢點）和緊鄰點（圖3）之間的特征元素。可知復雜度有所降低，稱之為SPFH（simple point feature histograms）。

圖2 查詢點Pq的計算PFH的影響區域

圖3 查詢點Pq的計算FPFH的影響區域

確定點的k領域，得出最終Pq直方圖公式如下：

3.2 SAC-IA配準

SAC-IA 配準（采樣一致性初始配準：Sample Consensus Initial Aligment,SAC-IA），通過FPFH特征配準點云可得到一個大致的位姿，達到粗配準的效果。

配準算法步驟如下。

（1）源點云B中選取n個點，為了保證選取的點具備不同的FPFH特征，選取點的距離必須小于給定的最小閾值。

（2）目標點云A查找與源點云B滿足相似條件的點，并保持一一對應關系。

（3）計算對應點的旋轉矩陣和平移矩陣，并根據Huber函數進行評判：

式中：m為給定閾值；li為第i組對應點變換后的距離差。

重復上述步驟直至結果最優，即誤差函數取最小值，得到最終的平移矩陣和旋轉矩陣。

4 精配準

粗配準后僅僅得到一個較好的位姿，為了使兩期點云盡可能重合，需要進行精配準。ICP算法的基本原理如下：兩期點云 A 和 B，點集為 A={a1, a2, a3, …, an}、B={b1, b2, b3…, bm}，通過旋轉平移變換后，點云A、B中的點一一對應。

式中：R為旋轉矩陣；T為平移矩陣。

ICP配準的步驟如下。

（1）目標點云A中取點集ai，并在源點云B中找到對應點bi，使。

（2）計算旋轉矩陣和平移矩陣，使目標函數取最小值。

（3）對目標點云A 進行旋轉平移變換，更新得到新點云數據集A′。

（4）計算已更新點云A′和源點云B 中所有對應點的距離，做歸一化處理，得。

（5）給定閾值，若平均距離d 小于給定的閾值，重復以上步驟，否則視為收斂。

通過上述步驟得到的旋轉矩陣和平移矩陣，用于原點云坐標轉換，完成配準過程。

5 實驗結果和分析

本文實驗在cpu 主頻2.4 GHz， 內 存 為 4 G 的windows10系統下進行實驗平臺的搭建，使用C++進行編程，實驗中選用的是PCL 開源庫中的milk_cartoon_all_small_clorox 數據文件，點云數據中大約有240 000個點，實驗結果如圖4所示。

圖4 配準結果

由實驗結果可知，本文提出的配準優化算法可以滿足點云配準的重合精度。此外，本文提出的配準優化算法和傳統的配準算法相比較，配準速度有明顯的提升，在保證配準精度的前提下，算法效率提高了29.5%。實驗使用改進的配準算法和傳統配準算法所消耗的時間如表1所示。

表1 傳統算法和改進算法的比較

6 結束語

針對傳統配準算法耗時不足的問題，本文提出了一種基于RANSAC算法的點云配準算法。首先，對點云使用RANSAC算法提取可以代替原點云的關鍵面，接著，使用FPFH特征進行粗配準，在粗配準的基礎上使用ICP算法得到旋轉矩陣和平移矩陣，達到精配準的效果。經實驗證明，該方法可應用于點云的配準過程。與傳統配準算法相比，收斂穩定，速度快，具有很好的實用價值。