陀螺儀在基于Hash結(jié)構(gòu)的三維重建中的應(yīng)用

張國(guó)山，尚紅霞，王欣博

?

陀螺儀在基于Hash結(jié)構(gòu)的三維重建中的應(yīng)用

張國(guó)山，尚紅霞，王欣博

(天津大學(xué)電氣與自動(dòng)化工程學(xué)院，天津300072)

針對(duì)3D重建過(guò)程中出現(xiàn)深度相機(jī)突然運(yùn)動(dòng)，導(dǎo)致相機(jī)無(wú)法正確跟蹤重建場(chǎng)景，提出了一種結(jié)合陀螺儀與基于Hash結(jié)構(gòu)的KinectFusion 3D重建的方法. 將陀螺儀得到的位姿與ICP算法得到的位姿通過(guò)擴(kuò)展卡爾曼濾波器(EKF)數(shù)據(jù)融合，獲得更加精確的位姿，改進(jìn)了原始算法的3D重建效果. 實(shí)驗(yàn)測(cè)試結(jié)果表明，該算法可以得到更加精確的系統(tǒng)位姿預(yù)測(cè)模型，在相機(jī)突然運(yùn)動(dòng)時(shí)仍然能夠?qū)崿F(xiàn)有效跟蹤.

3D重建；陀螺儀；Hash結(jié)構(gòu)；擴(kuò)展卡爾曼濾波器；ICP算法

從密集范圍數(shù)據(jù)進(jìn)行表面重建的技術(shù)，是一種3D重建技術(shù)，在虛擬現(xiàn)實(shí)、動(dòng)漫制作、醫(yī)學(xué)影像、教育、CAD、工業(yè)設(shè)計(jì)等方面有著重要的應(yīng)用．該技術(shù)最早出現(xiàn)于文獻(xiàn)[1]，主要沿著兩個(gè)方向發(fā)展：從無(wú)組織的點(diǎn)重建和從隱藏著一定結(jié)構(gòu)的數(shù)據(jù)中重建．所提出的利用符號(hào)距離函數(shù)的方法被稱為霍普(Hoppe)方法．然后Bajaj等[2]使用-形狀來(lái)構(gòu)造一個(gè)符號(hào)距離函數(shù)．Curless等[3]提出的一種基于累加權(quán)符號(hào)距離函數(shù)的體積表示法是這種3D重建技術(shù)的代表，使用激光條紋光學(xué)三角掃描儀和CCD照相機(jī)進(jìn)行三維重建，但是這種掃描技術(shù)只能提供外表面的數(shù)據(jù)，內(nèi)部腔體不可見，此外，非常復(fù)雜的對(duì)象可能需要掃描大量的覆蓋表面，但是有光澤的表面會(huì)使光打偏，黑色的物體會(huì)吸收光，光亮的表面會(huì)互相干擾，而且這種設(shè)備昂貴且不便于攜帶，這些技術(shù)和設(shè)備上的缺陷限制了3D重建技術(shù)的發(fā)展.

上述的這些問(wèn)題隨著2010年11月，在北美正式發(fā)布的一種新型的體感設(shè)備Kinect而得到有效解決．Kinect相機(jī)使用一種結(jié)構(gòu)光技術(shù)[4]來(lái)產(chǎn)生包含物理場(chǎng)景的離散范圍測(cè)量的實(shí)時(shí)深度圖．這些深度圖數(shù)據(jù)可以被映射成一個(gè)離散3D點(diǎn)的集合，即3D點(diǎn)云．Kinect相機(jī)重建物體時(shí)，深度測(cè)量經(jīng)常出現(xiàn)波動(dòng)，且在沒(méi)有數(shù)據(jù)讀入的地方深度圖像中包含大量的“黑洞”．針對(duì)這個(gè)問(wèn)題，文獻(xiàn)[5-6]提出KinectFusion算法，該算法連續(xù)跟蹤相機(jī)的6自由度位姿，并將得到的當(dāng)前圖像融入到之前時(shí)間內(nèi)構(gòu)造的模型中．然而KinectFusion使用文獻(xiàn)[3]中的密度體積來(lái)存儲(chǔ)3D場(chǎng)景信息，由于內(nèi)存容量等的限制使得KinectFusion只能用于小場(chǎng)景物體的重建，這一缺點(diǎn)限制了3D重建技術(shù)的發(fā)展. 因此文獻(xiàn)[7]提出一種使用壓縮空間中的簡(jiǎn)單的空間散列，即Hash結(jié)構(gòu)的方案，允許進(jìn)行實(shí)時(shí)訪問(wèn)和隱面數(shù)據(jù)的更新，并且不需要定型或分層網(wǎng)格數(shù)據(jù)結(jié)構(gòu)．基于Hash結(jié)構(gòu)的KinectFusion算法通過(guò)實(shí)現(xiàn)GPU和CPU數(shù)據(jù)交換，能夠?qū)⑿碌膱?chǎng)景信息拼接到已有場(chǎng)景模型中，從而實(shí)現(xiàn)大范圍場(chǎng)景信息的3D重建，但是由于KinectFusion算法中使用的ICP定位算法對(duì)初值敏感，在進(jìn)行跟蹤定位時(shí)存在累積誤差，而且容易陷入局部最小，相機(jī)突然運(yùn)動(dòng)時(shí)不能有效跟蹤．

針對(duì)上述問(wèn)題，本文采用陀螺儀實(shí)現(xiàn)對(duì)相機(jī)的位姿測(cè)量，一方面作為ICP匹配算法的估計(jì)值，另一方面通過(guò)擴(kuò)展卡爾曼濾波器(EKF)數(shù)據(jù)融合算法與ICP計(jì)算出的位姿進(jìn)行融合，從而得到更加精確的位姿，增加算法的魯棒性，提高系統(tǒng)的適用性．

1?KinectFusion算法分析

1.1?使用工具及3D重建流程

圖1為本文中用到的以色列PrimeSense公司的PrimeSense1.09深度相機(jī)，它由一個(gè)RGB攝像頭、一個(gè)紅外攝像頭和一個(gè)紅外發(fā)射器組成．它工作時(shí)首先通過(guò)RGB攝像頭獲取彩色信息，利用紅外攝像頭感知紅外線，然后由PrimeSense1.09內(nèi)部硬件分析處理得到深度信息．PrimeSense1.09每秒可以處理30幀的深度信息，每幀640×480個(gè)像素，深度探測(cè)范圍為0.5～3.0,m.

圖1?深度相機(jī)PrimeSense1.09

KinectFusion算法使用點(diǎn)-面的ICP算法使深度相機(jī)的當(dāng)前深度幀跟蹤場(chǎng)景的全局模型，這個(gè)全局模型是使用所有前面的幀得到的．這種跟蹤方法相對(duì)于幀-幀的跟蹤有很多優(yōu)點(diǎn)，對(duì)于3D重建技術(shù)是一個(gè)很大的進(jìn)步．它通過(guò)將深度相機(jī)采集到的深度數(shù)據(jù)進(jìn)行匹配定位與融合，實(shí)現(xiàn)3D場(chǎng)景重建，圖2為算法流程[5].

KinectFusion算法主要由4部分組成[6]：①深度數(shù)據(jù)處理是將相機(jī)得到的原始深度數(shù)據(jù)轉(zhuǎn)換成3D點(diǎn)云，通過(guò)計(jì)算得到點(diǎn)云中頂點(diǎn)的3D坐標(biāo)和法向量；②相機(jī)跟蹤是使用ICP匹配算法，將當(dāng)前幀的3D點(diǎn)云與由現(xiàn)有模型生成的3D點(diǎn)云，即預(yù)測(cè)的3D點(diǎn)云進(jìn)行ICP匹配，從而計(jì)算出相機(jī)當(dāng)前幀的位姿；③點(diǎn)云融合是使用TSDF點(diǎn)云融合算法結(jié)合當(dāng)前相機(jī)的位姿，將當(dāng)前幀的3D點(diǎn)云融合到現(xiàn)有模型中；④場(chǎng)景渲染是對(duì)現(xiàn)有模型與通過(guò)當(dāng)前相機(jī)位姿預(yù)測(cè)出當(dāng)前相機(jī)觀察到的點(diǎn)云，進(jìn)行光線跟蹤投射，計(jì)算結(jié)果一方面給用戶提供可視化效果，另一方面用于②中的ICP匹配.

圖2?KinectFusion算法流程

1.2?ICP定位方法

ICP定位環(huán)節(jié)將當(dāng)前幀3D點(diǎn)云和預(yù)測(cè)得到的3D點(diǎn)云進(jìn)行匹配．KinectFusion算法中使用的ICP匹配算法是點(diǎn)-面的最小二乘最優(yōu)化的ICP算法[8-9]，ICP算法的輸出是一個(gè)3D剛體轉(zhuǎn)換矩陣，它將當(dāng)前幀的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到3D模型的點(diǎn)云中，使兩者之間的誤差度量最?。?dāng)使用點(diǎn)-面的誤差度量時(shí)，最小化的目的是使每個(gè)當(dāng)前幀中的點(diǎn)和它對(duì)應(yīng)的3D模型中的點(diǎn)的切平面之間的平方距離最小，如圖3所示[9].

圖3?兩個(gè)表面之間的點(diǎn)-面的誤差

???(1)

其中

因此式(1)可以改寫成

???(2)

???(3)

因此整個(gè)問(wèn)題就轉(zhuǎn)化為

???(4)

的最優(yōu)解問(wèn)題，可以通過(guò)奇異值分解求得.

1.3?TSDF點(diǎn)云融合算法

TSDF算法[3]用一個(gè)立方體柵格來(lái)表示3D空間(圖2步驟3)，物體模型表面與對(duì)應(yīng)的柵格之間的距離存儲(chǔ)在該立方體對(duì)應(yīng)的柵格中，而且表面可見一側(cè)和被遮擋一側(cè)是用所存儲(chǔ)的值的正負(fù)來(lái)表示的，物體表面上的點(diǎn)用零值表示，即過(guò)零點(diǎn).當(dāng)模型中有新的數(shù)據(jù)加入時(shí)，按

???(5)

???(6)

2?3D重建算法的改進(jìn)

2.1?Hash結(jié)構(gòu)

3D重建中隱式表示3D幾何的TSDF存儲(chǔ)在不同的數(shù)據(jù)結(jié)構(gòu)中，3D場(chǎng)景模型可以由具有不同的內(nèi)存效率和迭代計(jì)算效率數(shù)據(jù)結(jié)構(gòu)表示．KinectFusion算法中使用的數(shù)據(jù)結(jié)構(gòu)是固定大小的密度體積，這對(duì)于大場(chǎng)景的3D重建有一定的限制，因此有研究者提出不同的改進(jìn)算法，如八叉樹[10]和Hash結(jié)構(gòu)[7]的數(shù)據(jù)結(jié)構(gòu)等都可以對(duì)固定大小的密度體積方法呈現(xiàn)的局限性進(jìn)行改善. 體元分組在預(yù)定義大小的塊中，所有的體元存儲(chǔ)在一個(gè)連續(xù)數(shù)組(即體素塊陣列)中. 使用Hash表(如圖4所示)能夠在體元塊數(shù)組中找到體元塊.

圖4?Hash表的邏輯結(jié)構(gòu)

實(shí)際3D重建中顯卡存儲(chǔ)容量是有限的，盡管使用Hash結(jié)構(gòu)，這個(gè)問(wèn)題仍然沒(méi)有得到顯著改善，根據(jù)Hash結(jié)構(gòu)能夠快速查找、刪除等特點(diǎn)，將重建過(guò)程中活動(dòng)部分存儲(chǔ)在顯卡內(nèi)存中，其余部分存儲(chǔ)在主機(jī)中，當(dāng)有需要時(shí)再交換到顯卡內(nèi)存中，這樣就可以有效解決顯卡內(nèi)存不足的問(wèn)題．根據(jù)文獻(xiàn)[7]的描述，數(shù)據(jù)交換過(guò)程如圖5所示.

（a）數(shù)據(jù)從Host(CPU)換入到GPU

（b）數(shù)據(jù)從GPU換入到Host(CPU)

圖5?數(shù)據(jù)交換過(guò)程

Fig.5?Process of data swapping

2.2?陀螺儀

陀螺儀可以精確實(shí)時(shí)地測(cè)量出與之相連的物體的位姿變化，使用陀螺儀傳感器SDK中的getCurrent-Data函數(shù)可以直接得到相機(jī)歐拉角和線性加速度的值，對(duì)線性加速度的值進(jìn)行積分，并用ICP算法計(jì)算出的速度進(jìn)行校正即可得到相機(jī)的平移向量．將陀螺儀得到的歐拉角和平移向量組合起來(lái)即是深度相機(jī)的位姿變化.

本文中使用LPMS-CU(OEM)型號(hào)的陀螺儀傳感器，如圖6所示.

圖6?陀螺儀傳感器

實(shí)驗(yàn)中使用陀螺儀測(cè)得的位姿作為深度相機(jī)位姿的估計(jì)值，一方面作為ICP匹配算法的估計(jì)值，以避免迭代過(guò)程陷入局部最小值；另一方面作為EKF數(shù)據(jù)融合的一個(gè)輸入數(shù)據(jù)與ICP算法計(jì)算出的位姿融合，得到更加精確的位姿，在相機(jī)運(yùn)動(dòng)突然變化時(shí)仍然能夠準(zhǔn)確跟蹤相機(jī)，以此來(lái)增加算法的魯棒性.

2.3?EKF數(shù)據(jù)融合

KinectFusion算法中利用ICP算法計(jì)算出的相機(jī)位姿有時(shí)會(huì)陷入局部最小，而且不可避免地存在定位誤差，即ICP的協(xié)方差[11]，本文中使用費(fèi)舍爾信息矩陣[12]來(lái)進(jìn)行分析和描述.使用EKF[13-14]數(shù)據(jù)融合來(lái)對(duì)3D重建中的位姿進(jìn)行估計(jì)，可以減小誤差帶來(lái)的影響，從而增加算法的魯棒性.

2.3.1?費(fèi)舍爾信息矩陣和誤差協(xié)方差

根據(jù)前面求解點(diǎn)-面的ICP算法過(guò)程中所要求的線性最小二乘公式為

???(7)

為了在EKF中使用ICP算法的估計(jì)結(jié)果，需要定量化通過(guò)ICP算法得到的位姿估計(jì)的誤差，該誤差可以作為測(cè)量噪聲，通常用協(xié)方差矩陣表示.提取協(xié)方差矩陣先將問(wèn)題進(jìn)行線性化得

式中：0為的近似值，；為當(dāng)前幀中的一個(gè)小的剛體轉(zhuǎn)換，，且

式(7)可近似轉(zhuǎn)化為

???(8)

???(9)

2.3.2?EKF數(shù)據(jù)融合

卡爾曼濾波器方法是一種利用線性系統(tǒng)狀態(tài)方程，通過(guò)系統(tǒng)輸入輸出觀察數(shù)據(jù)，對(duì)系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法；然而在實(shí)際中，很多系統(tǒng)都是非線性的，一般通過(guò)擴(kuò)展卡爾曼濾波器(EKF)的方法進(jìn)行數(shù)據(jù)融合.

首先通過(guò)靜態(tài)測(cè)試LPMS-CU(OEM)型號(hào)的陀螺儀傳感器，得到它的協(xié)方差為

其中

本文在程序中通過(guò)迭代更新來(lái)實(shí)現(xiàn)擴(kuò)展卡爾曼濾波器的數(shù)據(jù)融合算法，其更新公式為

圖7?數(shù)據(jù)融合流程

3?實(shí)驗(yàn)及結(jié)果分析

本文實(shí)驗(yàn)是在VS2012集成開發(fā)環(huán)境下進(jìn)行的，并先用MATLAB標(biāo)定工具箱對(duì)Prime-Sense1.09深度相機(jī)進(jìn)行標(biāo)定.

實(shí)驗(yàn)中采用基于Hash結(jié)構(gòu)的KinectFusion算法.使用Hash結(jié)構(gòu)來(lái)存儲(chǔ)場(chǎng)景信息的空間占用率更低，能夠?qū)崿F(xiàn)大場(chǎng)景信息的3D重建.圖8和圖9是針對(duì)KinectFusion算法進(jìn)行改進(jìn)的效果圖.

圖8?未使用EKF數(shù)據(jù)融合重建算法的改進(jìn)效果

圖9?使用EKF數(shù)據(jù)融合重建算法的改進(jìn)效果

圖8和圖9分別為使用原始KinectFusion算法和加入EKF數(shù)據(jù)融合算法后用深度相機(jī)對(duì)某一空間進(jìn)行掃描時(shí)，深度相機(jī)突然加速后重建效果圖，重建圖可以顯示出改進(jìn)算法可以應(yīng)對(duì)掃描的突然變化，算法的魯棒性得到明顯提高.

4?結(jié)?語(yǔ)

本文針對(duì)目前三維重建中當(dāng)相機(jī)突然加速時(shí)，不能實(shí)時(shí)跟蹤的問(wèn)題，通過(guò)引入陀螺儀并結(jié)合ICP算法，使用EKF數(shù)據(jù)融合，得到更精確的位姿，能夠?qū)崿F(xiàn)實(shí)時(shí)跟蹤，增加了算法魯棒性.本文主要用在室內(nèi)場(chǎng)景的實(shí)時(shí)3D重建. 今后的研究會(huì)在現(xiàn)有工作的基礎(chǔ)上，研究能夠運(yùn)用在室外環(huán)境的3D重建算法.

［1］ Hoppe H，Derose T，Duchamp T，et al. Surface reconstruction from unorganized points[J].，1992，26(2)：71-78.

［2］ Bajaj C L，Bernardini F，Xu G. Automatic reconstruction of surfaces and scalar fields from 3D scans[C]//Los Angeles，USA：ACM，1995：109-118.

［3］ Curless B，Levoy M. A volumetric method for building complex models from range images[C]//New York，USA：ACM，1996：303-312.

［4］ Freedman B，Shpunt A，Machline M，et al. Depth Mapping Using Projected Patterns：US 8150142[P]. 2012-04-03.

［5］ Izadi S，Kim D，Hilliges O，et al. Kinectfusion：Real-time 3D reconstruction and interaction using a moving depth camera[C]//USA：IEEE，2011：559-568.

［6］ Newcombe R A，Davison A J，Izadi S，et al. KinectFusion：Real-time dense surface mapping and tracking [C]//USA：IEEE，2011：127-136.

［7］ Nie?ner M，Zollh?fer M，Izadi S，et al. Real-time 3d reconstruction at scale using voxel hashing[J].()，2013，32(6)：169.

［8］ Chen Y，Medioni G. Object modelling by registration of multiple range images[J].，1992，10(3)：145-155.

［9］ Low K. Linear least-squares optimization for point-to-plane ICP surface registration[R]. Charlotte：University of North Carolina，2004.

［10］ Zeng M，Zhao F，Zheng J，et al. Octree-based fusion for real-time 3D reconstruction[J].，2013，75(3)：126-136.

［11］ Censi A. An accurate closed-form estimate of ICP's covariance[C]//. Italy，2007：3167-3172.

［12］ Nguyen T T，Mentré F. Evaluation of the Fisher information matrix in nonlinear mixed effect models using adaptive Gaussian quadrature[J].，2014，80(4)：57-69.

［13］ Ljung L. Asymptotic behavior of the extended Kalman filter as a parameter estimator for linear systems[J].，1979，24(1)：36-50.

［14］ Caron F，Duflos E，Pomorski D，et al. GPS/IMU data fusion using multisensor Kalman filtering：Introduction of contextual aspects[J].，2006，7(2)：221-230.

（責(zé)任編輯：孫立華）

Application of Gyroscope in 3D Reconstruction Based on Hash Structure

Zhang Guoshan，Shang Hongxia，Wang Xinbo

(School of Electrical Engineering and Automation，Tianjin University，Tianjin 300072，China)

In order to track camera and reconstruct scene correctly when depth camera moves suddenly leading to a failure of tracking reconstructed scene，a new 3D reconstruction method，which combines the gyroscope with the KinectFusion algorithm based on Hash structure，is proposed．A more precise pose can be obtained by a data fusion of the extended Kalman filter which integrates the pose data from gyroscope and that from the ICP algorithm，hence improving the effect of 3D reconstruction．Experimental tests show that this algorithm can obtain more accurate predictive model of the system pose and track camera effectively when depth camera moves suddenly.

3D reconstruction；gyroscope；Hash structure；extended Kalman filter(EKF)；ICP algorithm

10.11784/tdxbz201506060

TP391

A

0493-2137(2016)11-1132-06

2015-06-16；

2015-11-24.

國(guó)家自然科學(xué)基金資助項(xiàng)目(61473202).

張國(guó)山（1961—??），男，教授.

張國(guó)山，zhanggs@tju.edu.cn.

2016-01-22.

http://www.cnki.net/kcms/detail/12.1127.N.20160122.1031.004.html.