張國山，尚紅霞，王欣博

?

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

張國山，尚紅霞，王欣博

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

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

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

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

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

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

1?KinectFusion算法分析

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

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

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

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

KinectFusion算法主要由4部分組成[6]：①深度數(shù)據(jù)處理是將相機(jī)得到的原始深度數(shù)據(jù)轉(zhuǎn)換成3D點(diǎn)云，通過計(jì)算得到點(diǎn)云中頂點(diǎn)的3D坐標(biāo)和法向量；②相機(jī)跟蹤是使用ICP匹配算法，將當(dāng)前幀的3D點(diǎn)云與由現(xiàn)有模型生成的3D點(diǎn)云，即預(yù)測的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)有模型中；④場景渲染是對現(xiàn)有模型與通過當(dāng)前相機(jī)位姿預(yù)測出當(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ù)測得到的3D點(diǎn)云進(jìn)行匹配．KinectFusion算法中使用的ICP匹配算法是點(diǎn)-面的最小二乘最優(yōu)化的ICP算法[8-9]，ICP算法的輸出是一個3D剛體轉(zhuǎn)換矩陣，它將當(dāng)前幀的點(diǎn)云數(shù)據(jù)轉(zhuǎn)換到3D模型的點(diǎn)云中，使兩者之間的誤差度量最?。?dāng)使用點(diǎn)-面的誤差度量時，最小化的目的是使每個當(dāng)前幀中的點(diǎn)和它對應(yīng)的3D模型中的點(diǎn)的切平面之間的平方距離最小，如圖3所示[9].

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

???(1)

其中

因此式(1)可以改寫成

???(2)

???(3)

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

???(4)

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

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

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

???(5)

???(6)

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

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

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

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

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

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

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

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

Fig.5?Process of data swapping

2.2?陀螺儀

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

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

圖6?陀螺儀傳感器

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

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

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

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

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

???(7)

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

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

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

???(8)

???(9)

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

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

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

其中

本文在程序中通過迭代更新來實(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)定工具箱對Prime-Sense1.09深度相機(jī)進(jìn)行標(biāo)定.

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

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

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

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

4?結(jié)?語

本文針對目前三維重建中當(dāng)相機(jī)突然加速時，不能實(shí)時跟蹤的問題，通過引入陀螺儀并結(jié)合ICP算法，使用EKF數(shù)據(jù)融合，得到更精確的位姿，能夠?qū)崿F(xiàn)實(shí)時跟蹤，增加了算法魯棒性.本文主要用在室內(nèi)場景的實(shí)時3D重建. 今后的研究會在現(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.

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

張國山（1961—??），男，教授.

張國山，zhanggs@tju.edu.cn.

2016-01-22.

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