基于GICP算法的深度相機(jī)點(diǎn)云融合方法研究

任家豪 ，岳云霄

（長(zhǎng)安大學(xué)工程機(jī)械學(xué)院，陜西 西安 710064）

視覺應(yīng)用是農(nóng)用機(jī)器人自動(dòng)化中的關(guān)鍵技術(shù)[1-4]。深度相機(jī)是指能測(cè)量目標(biāo)到相機(jī)之間距離（深度）的相機(jī)。一般的相機(jī)僅僅能完成影像的拍攝及2D圖像的采集，而深度相機(jī)增加了深度測(cè)量的功能。將多深度相機(jī)點(diǎn)云利用GICP算法進(jìn)行配準(zhǔn)融合，該算法的誤差比標(biāo)準(zhǔn)ICP算法的誤差小一個(gè)數(shù)量級(jí)[5]，可擴(kuò)展相機(jī)的范圍，提高點(diǎn)云密度，這在機(jī)器人自主運(yùn)動(dòng)領(lǐng)域有著十分重要的作用。

1 點(diǎn)云數(shù)據(jù)采集

1.1 硬件介紹

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

圖1 RealSense D435i深度相機(jī)

1.2 點(diǎn)云采集

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

圖2 目標(biāo)位置

圖3 點(diǎn)云數(shù)據(jù)

2 GICP算法及優(yōu)化

2.1 GICP算法原理

ICP算法最早由Arun等[9]于1987年提出，這種點(diǎn)集與點(diǎn)集坐標(biāo)系匹配的算法被證明是解決復(fù)雜配準(zhǔn)問題的關(guān)鍵方法[10]。GICP點(diǎn)云融合算法與ICP算法目標(biāo)一致，但實(shí)現(xiàn)有所區(qū)別。ICP的理論推導(dǎo)嚴(yán)謹(jǐn)，但對(duì)點(diǎn)云要求比較嚴(yán)格，在實(shí)驗(yàn)中可能無法做到兩個(gè)點(diǎn)集一一對(duì)應(yīng)；GICP統(tǒng)一了各種點(diǎn)云的對(duì)應(yīng)情況，故ICP算法可以被視為GICP算法的一種情況。當(dāng)然，在特殊情況下，GICP也會(huì)“退化”為ICP。

GICP算法使用高斯分布概率模型對(duì)點(diǎn)云各個(gè)點(diǎn)的位置進(jìn)行描述，目標(biāo)點(diǎn)云T={t1，t2，…，tn}與源點(diǎn)云S={S1，S2，…，Sn}中，各個(gè)點(diǎn)均服從高斯分布，則：

式中，^t、^s表示各個(gè)點(diǎn)云的理想位置。若點(diǎn)云中各個(gè)點(diǎn)均處于理想位置，理論上就存在Tr，使得源點(diǎn)云和目標(biāo)點(diǎn)云完全匹配。事實(shí)上，由于測(cè)量或傳感器自身的原因，點(diǎn)對(duì)之間存在著協(xié)方差。

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

在上述問題中，已知數(shù)個(gè)殘差di，則變?yōu)橐阎c(diǎn)云結(jié)果的情況下求使得這種結(jié)果出現(xiàn)的最大概率的轉(zhuǎn)換矩陣T*的問題，將其表示如下：

取其負(fù)對(duì)數(shù)，將最大值問題轉(zhuǎn)化為最小值，即：

2.2 GICP算法優(yōu)化

針對(duì)RealSense D435i深度相機(jī)的點(diǎn)云采集特點(diǎn)，本研究對(duì)GICP算法進(jìn)行部分改進(jìn)，以提高算法計(jì)算效率及配準(zhǔn)的準(zhǔn)確性。

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

圖4 體素下采樣及去除無效點(diǎn)

在對(duì)深度點(diǎn)云進(jìn)行處理后，開始進(jìn)行算法優(yōu)化后的相機(jī)點(diǎn)云融合，深度相機(jī)點(diǎn)云融合流程如圖5所示。

圖5 深度相機(jī)點(diǎn)云融合流程

3 實(shí)驗(yàn)驗(yàn)證

本實(shí)驗(yàn)基于開源點(diǎn)云庫(kù) PCL1.9.1，使用C++語言進(jìn)行驗(yàn)證?？衫肞CL庫(kù)中的Vtk模塊顯示結(jié)果。本研究使用試湊法，對(duì)濾波器處理時(shí)采用的體素大小的參數(shù)進(jìn)行實(shí)驗(yàn)，當(dāng)使用（0.05, 0.05, 0.05）作為網(wǎng)格大小時(shí)，既可保證目標(biāo)特征又可提高融合速率。點(diǎn)云融合結(jié)果如圖6所示。

圖6 點(diǎn)云融合結(jié)果

4 小結(jié)

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