張國山，尚紅霞，王欣博

?

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

張國山，尚紅霞，王欣博

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

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

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

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

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

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

1?KinectFusion算法分析

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

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

圖1?深度相機PrimeSense1.09

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

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

圖2?KinectFusion算法流程

1.2?ICP定位方法

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

圖3?兩個表面之間的點-面的誤差

???(1)

其中

因此式(1)可以改寫成

???(2)

???(3)

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

???(4)

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

1.3?TSDF點云融合算法

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

???(5)

???(6)

2?3D重建算法的改進

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

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

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

實際3D重建中顯卡存儲容量是有限的，盡管使用Hash結(jié)構(gòu)，這個問題仍然沒有得到顯著改善，根據(jù)Hash結(jié)構(gòu)能夠快速查找、刪除等特點，將重建過程中活動部分存儲在顯卡內(nèi)存中，其余部分存儲在主機中，當有需要時再交換到顯卡內(nèi)存中，這樣就可以有效解決顯卡內(nèi)存不足的問題．根據(jù)文獻[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?陀螺儀

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

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

圖6?陀螺儀傳感器

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

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

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

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

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

???(7)

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

式中：0為的近似值，；為當前幀中的一個小的剛體轉(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)進行最優(yōu)估計的算法；然而在實際中，很多系統(tǒng)都是非線性的，一般通過擴展卡爾曼濾波器(EKF)的方法進行數(shù)據(jù)融合.

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

其中

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

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

3?實驗及結(jié)果分析

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

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

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

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

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

4?結(jié)?語

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

（責任編輯：孫立華）

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.

國家自然科學基金資助項目(61473202).

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

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

2016-01-22.

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