多特征擴(kuò)展信息濾波在RGB-D點(diǎn)云數(shù)據(jù)中的應(yīng)用

常明，康志忠，李敏，李方舟

(1.中國地震局第一監(jiān)測中心，天津 300180；2.中國地質(zhì)大學(xué)(北京)土地科學(xué)與技術(shù)學(xué)院，北京 100081；3.天津城建大學(xué) 地質(zhì)與測繪學(xué)院，天津 300384)

0 引言

最近幾十年，三維激光掃描儀已經(jīng)被廣泛應(yīng)用于室內(nèi)、室外三維場景的構(gòu)建[1]。由于其造價(jià)昂貴，掃描儀的使用受到了一定的限制。RGB-D相機(jī)是一款同時(shí)獲取場景內(nèi)深度影像和彩色影像的相機(jī)，能夠?qū)崿F(xiàn)對場景內(nèi)彩色點(diǎn)云數(shù)據(jù)的生成。Kinect作為RGB-D相機(jī)的一種，通過對場景內(nèi)點(diǎn)云數(shù)據(jù)的生成，構(gòu)建三維模型，在實(shí)際生活中發(fā)揮著越來越重要的作用。

Kinect2.0可同時(shí)獲取辨率為1 920像素×1 080像素的彩色影像和512像素×425像素的深度影像數(shù)據(jù)，通過相機(jī)間的相互標(biāo)定，可以獲取三維場景內(nèi)彩色點(diǎn)云數(shù)據(jù)，并且通過對點(diǎn)云數(shù)據(jù)格網(wǎng)化的處理可以實(shí)現(xiàn)實(shí)時(shí)生成高密度場景模型[2-4]。對于范圍較大的場景，單幀Kinect影像無法獲取全部數(shù)據(jù)，需要進(jìn)行多幀Kinect數(shù)據(jù)間的拼接。根據(jù)Kinect獲取的數(shù)據(jù)特點(diǎn)，主要通過2種方式完成數(shù)據(jù)拼接：針對深度數(shù)據(jù)生成的點(diǎn)云數(shù)據(jù)，通過ICP[5]算法解算多幀點(diǎn)云數(shù)據(jù)之間的同名點(diǎn)，獲得最優(yōu)變換矩陣，完成三維點(diǎn)云數(shù)據(jù)的精確配準(zhǔn)[6-8]；針對RGB-D相機(jī)的彩色影像數(shù)據(jù)，利用SIFT、SURF以及ORB[9-12]等方法對彩色影像中的像素同名點(diǎn)進(jìn)行提取，完成彩色影像配準(zhǔn)，依據(jù)深度相機(jī)與彩色相機(jī)的標(biāo)定關(guān)系，實(shí)現(xiàn)對三維點(diǎn)云數(shù)據(jù)的配準(zhǔn)[13-14]。Weber等[15]提出利用深度傳感器獲取的管道點(diǎn)云數(shù)據(jù)自動(dòng)拼接算法，將所有輸入的點(diǎn)云數(shù)據(jù)放在一個(gè)集合中，由相鄰測站的同名點(diǎn)匹配結(jié)果構(gòu)建全局坐標(biāo)系統(tǒng)。單站坐標(biāo)系的轉(zhuǎn)換關(guān)系按照順序自動(dòng)轉(zhuǎn)換為全局點(diǎn)云坐標(biāo)系，進(jìn)而實(shí)現(xiàn)全局坐標(biāo)系的拼接。Xie[16]提出一種新算法實(shí)現(xiàn)對ICP算法中同名點(diǎn)權(quán)重的動(dòng)態(tài)賦予，算法中還利用點(diǎn)云數(shù)據(jù)的結(jié)構(gòu)對象信息以及稀疏SIFT算法獲取的同名點(diǎn)的空間距離，實(shí)現(xiàn)對點(diǎn)云數(shù)據(jù)的離群消除，來實(shí)現(xiàn)點(diǎn)云數(shù)據(jù)的雙站拼接。Santos等[17]使用RGB-D數(shù)據(jù)，提出了一種自適應(yīng)的粗拼到精拼的室內(nèi)拼接模型。利用深度測量的隨機(jī)誤差理論對三維點(diǎn)進(jìn)行加權(quán)，并引入新的距離模型，進(jìn)行具有的精確性和魯棒性的由粗到細(xì)的拼接，并利用閉合檢測的方法，最大限度地減少誤差累積。關(guān)于室內(nèi)場景的創(chuàng)建，New combe等[18]提出了利用RGB-D數(shù)據(jù)解決同時(shí)定位與測圖(simultaneous localization and mapping，SLAM)，利用ICP算法模型解決了Kinect傳感器的實(shí)時(shí)配準(zhǔn)與地圖顯示；Endres等[19]提出了RGB-DSLAM系統(tǒng)模型，通過幀與幀之間的特征匹配完成粗配準(zhǔn)，再利用ICP算法實(shí)現(xiàn)了幀間匹配。Murartal等[20]提出了ORBSLAM2系統(tǒng)，來解決單目、雙目以及RGB-D的SLAM問題；Gao等[21]通過提出場景中的特征平面要素減弱誤差的累積。

現(xiàn)階段對Kinect生成的點(diǎn)云數(shù)據(jù)的拼接處理，大部分是基于ICP算法通過搜索最近點(diǎn)進(jìn)行的。但Kinect點(diǎn)云數(shù)據(jù)單點(diǎn)的精度有限，隨著測站數(shù)的增加，誤差的累積會(huì)越來越大，影響最后拼接結(jié)果的精度，從而限制了Kinect點(diǎn)云數(shù)據(jù)的應(yīng)用。

針對這個(gè)情況，本文通過對場景內(nèi)點(diǎn)、面特征的提取構(gòu)建了多特征擴(kuò)展信息濾波模型，實(shí)現(xiàn)對Kinect點(diǎn)云數(shù)據(jù)的全局拼接，達(dá)到減小誤差累積提升點(diǎn)云拼接質(zhì)量的目的。

1 多特征擴(kuò)展信息濾波

康志忠[22]提出了一種利用擴(kuò)展卡爾曼濾波進(jìn)行全局拼接的方法。擴(kuò)展信息濾波是擴(kuò)展卡爾曼濾波的對稱形式。相比于卡爾曼濾波，信息濾波在實(shí)現(xiàn)的過程更加簡單清晰[23]。

卡爾曼濾波在t時(shí)刻狀態(tài)向量和狀態(tài)方程表示為μt和Σt，而信息濾波的信息向量和信息矩陣則表示為：

(1)

式中：ξt表示t時(shí)刻的信息向量；Ωt表示t時(shí)刻的信息矩陣。

在利用信息濾波進(jìn)行每個(gè)測站點(diǎn)的位置估計(jì)時(shí)，主要分為移動(dòng)更新和觀測更新。

1.1 系統(tǒng)信息空間和狀態(tài)模型

1)系統(tǒng)信息空間。在對點(diǎn)云坐標(biāo)系進(jìn)行轉(zhuǎn)換時(shí)可認(rèn)為是剛性轉(zhuǎn)換，其變換矩陣由平移矩陣T和旋轉(zhuǎn)矩陣R構(gòu)成。每一個(gè)測站位置都由6個(gè)自由度來表示，包括3個(gè)空間平移坐標(biāo)x，y，z以及3個(gè)繞著X，Y，Z軸旋轉(zhuǎn)的角度α，β，γ。

將多站坐標(biāo)系統(tǒng)一到同一坐標(biāo)系下實(shí)現(xiàn)全局拼接，每個(gè)站點(diǎn)的位姿用向量(x，y，z，α，β，γ)來表示，則第K時(shí)刻的系統(tǒng)狀態(tài)為：

(2)

式中：n表示到目前為止已拼接的站數(shù)；Xi(k)表示第i站點(diǎn)云的位姿。

則信息矩陣

信息向量為：

(4)

式中：Σij(k)是一個(gè)分塊矩陣，表示Xi(k)與Xj(k)之間的協(xié)方差。

2)系統(tǒng)狀態(tài)模型。系統(tǒng)信息向量包括每個(gè)測站點(diǎn)的位置姿態(tài)。在觀測過程中，每站的位置都是靜態(tài)的，所以系統(tǒng)狀態(tài)轉(zhuǎn)換方程為：

Xt+1(k)=f(Xt(k))=Xt(k)

(5)

式中：狀態(tài)轉(zhuǎn)換函數(shù)f(·)單位陣I，所以可以忽略。

1.2 移動(dòng)更新

當(dāng)?shù)玫叫乱徽镜臄?shù)據(jù)時(shí)，根據(jù)之前更新的系統(tǒng)狀態(tài)，估計(jì)其粗略的同一坐標(biāo)系下的位置與姿態(tài)，增加到系統(tǒng)狀態(tài)向量中。首先把新的點(diǎn)云數(shù)據(jù)和已更新的相鄰數(shù)據(jù)的原始數(shù)據(jù)(作為參考數(shù)據(jù))進(jìn)行粗略拼接。相鄰數(shù)據(jù)當(dāng)前狀態(tài)估計(jì)即為：

Xt=[xt，yt，zt，αt，βt，γt]

把當(dāng)前數(shù)據(jù)和參考數(shù)據(jù)的拼接結(jié)果記作為：

ΔXt=[Δx，Δy，Δz，Δα，Δβ，Δγ]

基于Xt和ΔXt來計(jì)算當(dāng)前數(shù)據(jù)在全局坐標(biāo)系下的坐標(biāo)：

(6)

1.3 觀測更新

觀測更新主要是針對設(shè)備移動(dòng)獲取到的新數(shù)據(jù)，對場景數(shù)據(jù)的特征進(jìn)行提取，構(gòu)建相應(yīng)的觀測模型，對信息向量以及信息矩陣進(jìn)行更新。通過獲取點(diǎn)云數(shù)據(jù)的點(diǎn)特征和面特征信息，構(gòu)建多特征觀測模型，從而削減誤差累積對全局拼接的影響，避免由于單點(diǎn)精度的有限，影響最后的點(diǎn)云數(shù)據(jù)拼接的質(zhì)量。

1)點(diǎn)特征信息的獲取。通過ICP算法，對相鄰雙幀Kinect點(diǎn)云數(shù)據(jù)中的近鄰點(diǎn)進(jìn)行提取，創(chuàng)建匹配特征點(diǎn)集，計(jì)算2幀點(diǎn)云數(shù)據(jù)的旋轉(zhuǎn)、平移矩陣。為了獲取最優(yōu)的旋轉(zhuǎn)、平移矩陣，需要不斷的迭代上述過程。設(shè)當(dāng)前時(shí)刻求解的旋轉(zhuǎn)、平移矩陣為Rt、Tt，上一時(shí)刻求解的旋轉(zhuǎn)、平移矩陣為Rt-1、Tt-1，則迭代終止條件為相鄰兩次迭代結(jié)果計(jì)算的旋轉(zhuǎn)、平移矩陣差小于設(shè)定的閾值，即

(7)

本實(shí)驗(yàn)中Rlim、Tlim分別設(shè)置為[0.05，0.05，0.05]，[0.01，0.01，0.01]，此時(shí)得到的最近點(diǎn)作為點(diǎn)特征信息。相鄰兩幀數(shù)據(jù)的原始影像、深度點(diǎn)云數(shù)據(jù)及點(diǎn)特征提取結(jié)果如圖1所示。

圖1 相鄰測幀點(diǎn)特征提取結(jié)果

2)面特征信息的獲取。針對場景內(nèi)的點(diǎn)云數(shù)據(jù)，利用區(qū)域面生長法[24]對其進(jìn)行分類、擬合，獲取主要平面的點(diǎn)云數(shù)據(jù)及其在各自坐標(biāo)系下的空間平面方程a*x+by+c*z+d=0。以圖1中2幀圖像的深度點(diǎn)云數(shù)據(jù)為例，對場景內(nèi)主要面特征進(jìn)行提取。面特征的提取結(jié)果如圖2所示，為了區(qū)別不同的平面，面特征提取結(jié)果采用不同的灰度值進(jìn)行顯示。由于Kinect獲取數(shù)據(jù)時(shí)相鄰幀場景相似度很高，可認(rèn)為面特征提取結(jié)果中相似空間平面方程所代表的2個(gè)平面為場景內(nèi)的同一平面，以其的單位法向量作為面特征信息，構(gòu)建多特征觀測模型。

圖2 相鄰兩幀面特征的提取結(jié)果

3)多特征觀測模型。常用Kinect點(diǎn)云數(shù)據(jù)的觀測模型是利用點(diǎn)特征進(jìn)行構(gòu)建的，但是由于單點(diǎn)的精度有限，存在一定的誤差。鑒于此本文提出利用點(diǎn)特征信息與面特征信息構(gòu)建多特征觀測模型。

(1)點(diǎn)特征觀測模型。利用ICP算法獲取的鄰近點(diǎn)作為點(diǎn)特征信息，構(gòu)建點(diǎn)特征觀測模型。假設(shè)相鄰兩站的點(diǎn)特征集，在第i站的坐標(biāo)系下表示為(xL1，yL1，zL1)，在第j站的坐標(biāo)系下表示為(xL2，yL2，zL2)，則點(diǎn)特征信息的觀測模型表示如下：

(8)

式中：h1(·)是系統(tǒng)點(diǎn)特征信息觀測函數(shù)；v(k)表示傳感器測量和坐標(biāo)轉(zhuǎn)換的不確定性，假設(shè)為高斯分布，表示為白噪聲向量N(0，S)。

(2)面特征觀測模型。對相鄰兩測站中同一平面的法向量進(jìn)行提取，并進(jìn)行單位化，構(gòu)建面特征觀測模型。假設(shè)同一個(gè)平面在第i站坐標(biāo)系的空間方程為：

ai*x+biy+ci*z+di=0

(9)

該平面在第i站下的法向量為(ai，bi，ci)；該平面在第j站坐標(biāo)系下的空間方程為：

aj*x+bjy+cj*z+dj=0

(10)

該平面在第j站下的法向量為(aj，bj，cj)。則面特征觀測模型表示為：

(11)

式中：h2(·)是系統(tǒng)面特征信息觀測函數(shù)；v(k)表示傳感器測量和坐標(biāo)轉(zhuǎn)換的不確定性，假設(shè)為高斯分布，表示為白噪聲向量N(0，S)。

(3)多特征觀測模型。將點(diǎn)特征信息與面特征信息當(dāng)做觀測值，構(gòu)建多特征觀測模型。則觀測模型為：

(12)

式中：Z1表示點(diǎn)特征的觀測模型；Z2表示面特征的觀測模型。由于2種特征信息的精度不同，所以對2種信息賦予不同的權(quán)值，來提高精度。

(13)

式中：λ1為點(diǎn)特征信息的權(quán)值；λ2為面特征信息的權(quán)值；Q1表示點(diǎn)特征信息的協(xié)方差矩陣；Q2表示面特征信息的協(xié)方差矩陣。

4)狀態(tài)增廣。當(dāng)獲取新一站的數(shù)據(jù)n時(shí)，根據(jù)系統(tǒng)狀態(tài)模型獲得該站的位置姿態(tài)Xn，然后增加到系統(tǒng)狀態(tài)向量中，則系統(tǒng)狀態(tài)向量和系統(tǒng)解方程矩陣分別增廣為：

(14)

則系統(tǒng)信息向量及信息矩陣分布為：

(15)

(16)

5)狀態(tài)更新。利用多特征觀測模型的觀測值，對預(yù)測信息矩陣及信息向量更新，按式(16)計(jì)算：

(17)

2 實(shí)驗(yàn)分析

本文對2個(gè)實(shí)驗(yàn)場景進(jìn)行實(shí)驗(yàn)，場景一為室內(nèi)房屋閉合場景，場景二為走廊不閉合場景。針對Kinect獲取的數(shù)據(jù)，分別利用ICP算法和多特征擴(kuò)展信息濾波算法進(jìn)行處理。

ICP算法對場景內(nèi)點(diǎn)云數(shù)據(jù)的最近點(diǎn)進(jìn)行獲取，構(gòu)建點(diǎn)集，實(shí)現(xiàn)旋轉(zhuǎn)平移矩陣的求解。并對場景內(nèi)整體的點(diǎn)云數(shù)據(jù)進(jìn)行旋轉(zhuǎn)平移，逐步將所有幀數(shù)據(jù)統(tǒng)一到全局坐標(biāo)系下實(shí)現(xiàn)場景內(nèi)的全局拼接；MEIF根據(jù)場景內(nèi)提取的點(diǎn)、面特征信息構(gòu)建多特征擴(kuò)展信息濾波模型實(shí)現(xiàn)全局拼接。通過比較2種算法在全局坐標(biāo)系下配準(zhǔn)點(diǎn)殘差的標(biāo)準(zhǔn)差(rootmean square error，RMSE)來驗(yàn)證本文模型的正確性。

2.1 場景一全局拼接結(jié)果比較

針對場景一，利用Kinect相機(jī)共獲取45幀數(shù)據(jù)，分別用2種方法進(jìn)行全局拼接，生成的數(shù)據(jù)如下圖3所示。為了能夠直觀、定性地比較2種方法全局拼接的誤差累積情況，在生成的場景中選擇A、B、C 3個(gè)位置點(diǎn)云數(shù)據(jù)的拼接結(jié)果進(jìn)行放大，比較拼接誤差。

圖3 場景一ICP與MEIF全局拼接結(jié)果比較

其中A處2種方法拼接結(jié)果的放大圖中，不存在明顯的誤差累積，點(diǎn)云數(shù)據(jù)拼接結(jié)果較好；在B處ICP算法拼接結(jié)果放大圖中，存在一定的誤差累積，導(dǎo)致點(diǎn)云數(shù)據(jù)拼接結(jié)果存在明顯分層現(xiàn)象；在C處放大圖中，ICP算法的誤差累積比較大，點(diǎn)云數(shù)據(jù)拼接結(jié)果錯(cuò)位明顯；而MEIF算法在全局拼接過程中誤差累積較小，在B、C 2處點(diǎn)云數(shù)據(jù)拼接結(jié)果的放大圖中，不存在明顯的分層、錯(cuò)位現(xiàn)象。

為了定量地分析2種算法的全局拼接精度，計(jì)算全局坐標(biāo)系下配準(zhǔn)點(diǎn)殘差的標(biāo)準(zhǔn)差，對2種算法的拼接精度進(jìn)行分析。計(jì)算結(jié)果繪制標(biāo)準(zhǔn)差過程線如圖4所示。ICP算法和MEIF算法針對場景一數(shù)據(jù)全局坐標(biāo)系下配準(zhǔn)點(diǎn)殘差的標(biāo)準(zhǔn)差期望分別是0.227 m和0.038 m，MEIF算法相比于ICP算法能夠有效削弱多站拼接中誤差的累積。

圖4 拼接結(jié)果標(biāo)準(zhǔn)差

2.2 場景二全局拼接結(jié)果比較

相對于場景一，場景二為不閉合的走廊。RGB-D相機(jī)在進(jìn)行室內(nèi)點(diǎn)云場景拼接的應(yīng)用中，不閉合的場景可以說是這個(gè)研究方向的一大難點(diǎn)。Kinect作為RGB-D相機(jī)的一種，本文利用其對走廊數(shù)據(jù)進(jìn)行采集，共獲取73幀數(shù)據(jù)。利用ICP算法和MEIF算法分別進(jìn)行全局拼接，結(jié)果如圖5所示。

圖5 場景二 全局拼接結(jié)果

從圖5可以直觀看出，利用ICP算法進(jìn)行全局拼接的結(jié)果存在一定的誤差，走廊存在著明顯的彎曲變形；利用MEIF算法進(jìn)行全局拼接的結(jié)果相對于ICP算法質(zhì)量有了一定的提高，拼接結(jié)果中不存在明顯的扭曲現(xiàn)象。為了能夠定量地比較2種方法針對不閉合場景的拼接精度，統(tǒng)計(jì)全局坐標(biāo)系下配準(zhǔn)點(diǎn)殘差的標(biāo)準(zhǔn)差，生成圖6。

其中ICP算法全局拼接結(jié)果的標(biāo)準(zhǔn)差的期望為2.64 m；利用MEIF算法實(shí)現(xiàn)全局拼接的標(biāo)準(zhǔn)差的期望為0.039 m。

圖6 拼接結(jié)果標(biāo)準(zhǔn)差

3 結(jié)束語

利用ICP算法處理Kinect點(diǎn)云數(shù)據(jù)，只有單獨(dú)2站的情況下，精度可以滿足需求。但是當(dāng)需要對多站的點(diǎn)云數(shù)據(jù)進(jìn)行全局拼接時(shí)，隨著測站數(shù)的增加，誤差累積也越來越嚴(yán)重，影響最終的拼接結(jié)果。通過對實(shí)驗(yàn)場景一、二全局拼接結(jié)果定性、定量地分析，相比于ICP算法，利用本文提出的多特征擴(kuò)展信息濾波模型，能夠在全局拼接的過程中，有效地減小誤差累積的影響，提高全局拼接的精度，對RGB-D相機(jī)在室內(nèi)三維點(diǎn)云場景的應(yīng)用有一定的參考價(jià)值。