张国山，尚红霞，王欣博



陀螺仪在基于Hash结构的三维重建中的应用

张国山，尚红霞，王欣博

(天津大学电气与自动化工程学院，天津300072)

针对3D重建过程中出现深度相机突然运动，导致相机无法正确跟踪重建场景，提出了一种结合陀螺仪与基于Hash结构的KinectFusion 3D重建的方法. 将陀螺仪得到的位姿与ICP算法得到的位姿通过扩展卡尔曼滤波器(EKF)数据融合，获得更加精确的位姿，改进了原始算法的3D重建效果. 实验测试结果表明，该算法可以得到更加精确的系统位姿预测模型，在相机突然运动时仍然能够实现有效跟踪.

3D重建；陀螺仪；Hash结构；扩展卡尔曼滤波器；ICP算法

从密集范围数据进行表面重建的技术，是一种3D重建技术，在虚拟现实、动漫制作、医学影像、教育、CAD、工业设计等方面有着重要的应用．该技术最早出现于文献[1]，主要沿着两个方向发展：从无组织的点重建和从隐藏着一定结构的数据中重建．所提出的利用符号距离函数的方法被称为霍普(Hoppe)方法．然后Bajaj等[2]使用-形状来构造一个符号距离函数．Curless等[3]提出的一种基于累加权符号距离函数的体积表示法是这种3D重建技术的代表，使用激光条纹光学三角扫描仪和CCD照相机进行三维重建，但是这种扫描技术只能提供外表面的数据，内部腔体不可见，此外，非常复杂的对象可能需要扫描大量的覆盖表面，但是有光泽的表面会使光打偏，黑色的物体会吸收光，光亮的表面会互相干扰，而且这种设备昂贵且不便于携带，这些技术和设备上的缺陷限制了3D重建技术的发展.

上述的这些问题随着2010年11月，在北美正式发布的一种新型的体感设备Kinect而得到有效解决．Kinect相机使用一种结构光技术[4]来产生包含物理场景的离散范围测量的实时深度图．这些深度图数据可以被映射成一个离散3D点的集合，即3D点云．Kinect相机重建物体时，深度测量经常出现波动，且在没有数据读入的地方深度图像中包含大量的“黑洞”．针对这个问题，文献[5-6]提出KinectFusion算法，该算法连续跟踪相机的6自由度位姿，并将得到的当前图像融入到之前时间内构造的模型中．然而KinectFusion使用文献[3]中的密度体积来存储3D场景信息，由于内存容量等的限制使得KinectFusion只能用于小场景物体的重建，这一缺点限制了3D重建技术的发展. 因此文献[7]提出一种使用压缩空间中的简单的空间散列，即Hash结构的方案，允许进行实时访问和隐面数据的更新，并且不需要定型或分层网格数据结构．基于Hash结构的KinectFusion算法通过实现GPU和CPU数据交换，能够将新的场景信息拼接到已有场景模型中，从而实现大范围场景信息的3D重建，但是由于KinectFusion算法中使用的ICP定位算法对初值敏感，在进行跟踪定位时存在累积误差，而且容易陷入局部最小，相机突然运动时不能有效跟踪．

针对上述问题，本文采用陀螺仪实现对相机的位姿测量，一方面作为ICP匹配算法的估计值，另一方面通过扩展卡尔曼滤波器(EKF)数据融合算法与ICP计算出的位姿进行融合，从而得到更加精确的位姿，增加算法的鲁棒性，提高系统的适用性．

1 KinectFusion算法分析

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

图1为本文中用到的以色列PrimeSense公司的PrimeSense1.09深度相机，它由一个RGB摄像头、一个红外摄像头和一个红外发射器组成．它工作时首先通过RGB摄像头获取彩色信息，利用红外摄像头感知红外线，然后由PrimeSense1.09内部硬件分析处理得到深度信息．PrimeSense1.09每秒可以处理30帧的深度信息，每帧640×480个像素，深度探测范围为0.5～3.0,m.

图1 深度相机PrimeSense1.09

KinectFusion算法使用点-面的ICP算法使深度相机的当前深度帧跟踪场景的全局模型，这个全局模型是使用所有前面的帧得到的．这种跟踪方法相对于帧-帧的跟踪有很多优点，对于3D重建技术是一个很大的进步．它通过将深度相机采集到的深度数据进行匹配定位与融合，实现3D场景重建，图2为算法流程[5].

KinectFusion算法主要由4部分组成[6]：①深度数据处理是将相机得到的原始深度数据转换成3D点云，通过计算得到点云中顶点的3D坐标和法向量；②相机跟踪是使用ICP匹配算法，将当前帧的3D点云与由现有模型生成的3D点云，即预测的3D点云进行ICP匹配，从而计算出相机当前帧的位姿；③点云融合是使用TSDF点云融合算法结合当前相机的位姿，将当前帧的3D点云融合到现有模型中；④场景渲染是对现有模型与通过当前相机位姿预测出当前相机观察到的点云，进行光线跟踪投射，计算结果一方面给用户提供可视化效果，另一方面用于②中的ICP匹配.

图2 KinectFusion算法流程

1.2 ICP定位方法

ICP定位环节将当前帧3D点云和预测得到的3D点云进行匹配．KinectFusion算法中使用的ICP匹配算法是点-面的最小二乘最优化的ICP算法[8-9]，ICP算法的输出是一个3D刚体转换矩阵，它将当前帧的点云数据转换到3D模型的点云中，使两者之间的误差度量最小．当使用点-面的误差度量时，最小化的目的是使每个当前帧中的点和它对应的3D模型中的点的切平面之间的平方距离最小，如图3所示[9].

图3 两个表面之间的点-面的误差

   (1)

其中

因此式(1)可以改写成

   (2)

   (3)

因此整个问题就转化为

   (4)

的最优解问题，可以通过奇异值分解求得.

1.3 TSDF点云融合算法

TSDF算法[3]用一个立方体栅格来表示3D空间(图2步骤3)，物体模型表面与对应的栅格之间的距离存储在该立方体对应的栅格中，而且表面可见一侧和被遮挡一侧是用所存储的值的正负来表示的，物体表面上的点用零值表示，即过零点.当模型中有新的数据加入时，按

   (5)

   (6)

2 3D重建算法的改进

2.1 Hash结构

3D重建中隐式表示3D几何的TSDF存储在不同的数据结构中，3D场景模型可以由具有不同的内存效率和迭代计算效率数据结构表示．KinectFusion算法中使用的数据结构是固定大小的密度体积，这对于大场景的3D重建有一定的限制，因此有研究者提出不同的改进算法，如八叉树[10]和Hash结构[7]的数据结构等都可以对固定大小的密度体积方法呈现的局限性进行改善. 体元分组在预定义大小的块中，所有的体元存储在一个连续数组(即体素块阵列)中. 使用Hash表(如图4所示)能够在体元块数组中找到体元块.

图4 Hash表的逻辑结构

实际3D重建中显卡存储容量是有限的，尽管使用Hash结构，这个问题仍然没有得到显著改善，根据Hash结构能够快速查找、删除等特点，将重建过程中活动部分存储在显卡内存中，其余部分存储在主机中，当有需要时再交换到显卡内存中，这样就可以有效解决显卡内存不足的问题．根据文献[7]的描述，数据交换过程如图5所示.

（a）数据从Host(CPU)换入到GPU

（b）数据从GPU换入到Host(CPU)

图5 数据交换过程

Fig.5 Process of data swapping

2.2 陀螺仪

陀螺仪可以精确实时地测量出与之相连的物体的位姿变化，使用陀螺仪传感器SDK中的getCurrent-Data函数可以直接得到相机欧拉角和线性加速度的值，对线性加速度的值进行积分，并用ICP算法计算出的速度进行校正即可得到相机的平移向量．将陀螺仪得到的欧拉角和平移向量组合起来即是深度相机的位姿变化.

本文中使用LPMS-CU(OEM)型号的陀螺仪传感器，如图6所示.

图6 陀螺仪传感器

实验中使用陀螺仪测得的位姿作为深度相机位姿的估计值，一方面作为ICP匹配算法的估计值，以避免迭代过程陷入局部最小值；另一方面作为EKF数据融合的一个输入数据与ICP算法计算出的位姿融合，得到更加精确的位姿，在相机运动突然变化时仍然能够准确跟踪相机，以此来增加算法的鲁棒性.

2.3 EKF数据融合

KinectFusion算法中利用ICP算法计算出的相机位姿有时会陷入局部最小，而且不可避免地存在定位误差，即ICP的协方差[11]，本文中使用费舍尔信息矩阵[12]来进行分析和描述.使用EKF[13-14]数据融合来对3D重建中的位姿进行估计，可以减小误差带来的影响，从而增加算法的鲁棒性.

2.3.1 费舍尔信息矩阵和误差协方差

根据前面求解点-面的ICP算法过程中所要求的线性最小二乘公式为

   (7)

为了在EKF中使用ICP算法的估计结果，需要定量化通过ICP算法得到的位姿估计的误差，该误差可以作为测量噪声，通常用协方差矩阵表示.提取协方差矩阵先将问题进行线性化得

式中：0为的近似值，；为当前帧中的一个小的刚体转换，，且

式(7)可近似转化为

   (8)

   (9)

2.3.2 EKF数据融合

卡尔曼滤波器方法是一种利用线性系统状态方程，通过系统输入输出观察数据，对系统状态进行最优估计的算法；然而在实际中，很多系统都是非线性的，一般通过扩展卡尔曼滤波器(EKF)的方法进行数据融合.

首先通过静态测试LPMS-CU(OEM)型号的陀螺仪传感器，得到它的协方差为

其中

本文在程序中通过迭代更新来实现扩展卡尔曼滤波器的数据融合算法，其更新公式为

图7 数据融合流程

3 实验及结果分析

本文实验是在VS2012集成开发环境下进行的，并先用MATLAB标定工具箱对Prime-Sense1.09深度相机进行标定.

实验中采用基于Hash结构的KinectFusion算法.使用Hash结构来存储场景信息的空间占用率更低，能够实现大场景信息的3D重建.图8和图9是针对KinectFusion算法进行改进的效果图.

图8 未使用EKF数据融合重建算法的改进效果

图9 使用EKF数据融合重建算法的改进效果

图8和图9分别为使用原始KinectFusion算法和加入EKF数据融合算法后用深度相机对某一空间进行扫描时，深度相机突然加速后重建效果图，重建图可以显示出改进算法可以应对扫描的突然变化，算法的鲁棒性得到明显提高.

4 结 语

本文针对目前三维重建中当相机突然加速时，不能实时跟踪的问题，通过引入陀螺仪并结合ICP算法，使用EKF数据融合，得到更精确的位姿，能够实现实时跟踪，增加了算法鲁棒性.本文主要用在室内场景的实时3D重建. 今后的研究会在现有工作的基础上，研究能够运用在室外环境的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.