基于GICP算法的深度相機點云融合方法研究

任家豪 ，岳云霄

（長安大學工程機械學院，陜西 西安 710064）

視覺應用是農用機器人自動化中的關鍵技術[1-4]。深度相機是指能測量目標到相機之間距離（深度）的相機。一般的相機僅僅能完成影像的拍攝及2D圖像的采集，而深度相機增加了深度測量的功能。將多深度相機點云利用GICP算法進行配準融合，該算法的誤差比標準ICP算法的誤差小一個數量級[5]，可擴展相機的范圍，提高點云密度，這在機器人自主運動領域有著十分重要的作用。

1 點云數據采集

1.1 硬件介紹

本研究使用英特爾公司的RealSense D435i深度相機。2014年，英特爾公司推出了RealSense深度攝像頭，幀率可達Kinect的兩倍[6]。該深度相機集成了兩個紅外傳感器（IR Stereo Camera）、一個紅外激光發射器（IR Projector）和一個彩色相機（Color Camera）[7]。立體深度相機系統主要包括視覺處理器D4和深度模塊兩部分[8]。D435i相機中每一幀原始點云的點數量為307 200，可以滿足配準融合要求并且有所冗余。RealSense D435i深度相機如圖1所示。

圖1 RealSense D435i深度相機

1.2 點云采集

啟動相機節點roslaunch realsense2_camera rs_rgbd.launch，保存/camera/depth-registered/points話題中點云數據為pcd格式，在不同位置對同一目標位置進行點云采集作為點云融合的數據集。圖2為點云掃描的目標位置，圖3為采集到的點云數據。其中，圖3（a）的相機位置沿x軸平移30 cm后得到圖3（b）的相機位置，由于地面反光，因此會存在部分無效點。

圖2 目標位置

圖3 點云數據

2 GICP算法及優化

2.1 GICP算法原理

ICP算法最早由Arun等[9]于1987年提出，這種點集與點集坐標系匹配的算法被證明是解決復雜配準問題的關鍵方法[10]。GICP點云融合算法與ICP算法目標一致，但實現有所區別。ICP的理論推導嚴謹，但對點云要求比較嚴格，在實驗中可能無法做到兩個點集一一對應；GICP統一了各種點云的對應情況，故ICP算法可以被視為GICP算法的一種情況。當然，在特殊情況下，GICP也會“退化”為ICP。

GICP算法使用高斯分布概率模型對點云各個點的位置進行描述，目標點云T={t1，t2，…，tn}與源點云S={S1，S2，…，Sn}中，各個點均服從高斯分布，則：

式中，^t、^s表示各個點云的理想位置。若點云中各個點均處于理想位置，理論上就存在Tr，使得源點云和目標點云完全匹配。事實上，由于測量或傳感器自身的原因，點對之間存在著協方差。

定義殘差di=ti-TrSi，則殘差也應服從高斯分布，即：

在上述問題中，已知數個殘差di，則變為已知點云結果的情況下求使得這種結果出現的最大概率的轉換矩陣T*的問題，將其表示如下：

取其負對數，將最大值問題轉化為最小值，即：

2.2 GICP算法優化

針對RealSense D435i深度相機的點云采集特點，本研究對GICP算法進行部分改進，以提高算法計算效率及配準的準確性。

在進行點云采集時，深度相機每幀可采集307 200個點云數據，點云之中不乏部分冗余數據及部分無效點數據，因此對源點云和目標點云使用體素下采樣（體素濾波）方法。在不改變目標點云特征的前提下，大大減少點云數量，提高GICP算法計算效率。與此同時，在使用RealSense D435i深度相機進行點云采集時，所得到的點云數據可能存在測量誤差，造成部分點云坐標存在NaN值，故本研究在配準數據前對源點云與目標點云中的無效點都進行了去除。體素下采樣及去除無效點如圖4所示。

圖4 體素下采樣及去除無效點

在對深度點云進行處理后，開始進行算法優化后的相機點云融合，深度相機點云融合流程如圖5所示。

圖5 深度相機點云融合流程

3 實驗驗證

本實驗基于開源點云庫 PCL1.9.1，使用C++語言進行驗證。可利用PCL庫中的Vtk模塊顯示結果。本研究使用試湊法，對濾波器處理時采用的體素大小的參數進行實驗，當使用（0.05, 0.05, 0.05）作為網格大小時，既可保證目標特征又可提高融合速率。點云融合結果如圖6所示。

圖6 點云融合結果

4 小結

由圖6可知，使用RealSense D435i深度相機從不同方位拍攝點云圖，通過基于去除無效點及體素下采樣改進的GICP算法成功進行了融合，融合效果良好，且擴展了單深度相機的視角范圍。該方法可應用于機器人導航等多個方向。