三维重建中实时全局优化和错误回环判断的方法及装置制造方法及图纸

本发明专利技术公开了一种三维重建中实时全局优化和错误回环判断的方法及装置，其中，方法包括以下步骤：获取视觉信息和IMU信息，通过视觉信息的二次统计量和IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化；在全局优化函数中耦合变换矩阵，并且在全局优化过程中，提供状态估计量，使得在快速移动和特征点少的时候，继续进行建图；获取IMU零偏的估计量，并根据IMU零偏的估计量识别错误回环和正确回环。该方法可以在不损失精度的前提下实现了高效率的视觉和IMU信息的全局优化，且没有因为标定不准确导致的累积误差的问题，并可以进行准确的回环正确性的判断，简单易实现。

Method and device of real-time global optimization and error loop judgment in 3D reconstruction

【技术实现步骤摘要】
三维重建中实时全局优化和错误回环判断的方法及装置
本专利技术涉及相机定位与建图
，特别涉及一种三维重建中实时全局优化和错误回环判断的方法及装置。
技术介绍
随着机器人室内导航、虚拟现实及增强现实技术的发展和广泛应用，三维重建需要越来越高的精度和效率，需要应对各种不同的环境和运动状态，实现更加鲁棒的重建过程。而三维重建最需要的是通过视觉进行定位的基于SLAM(SimultaneousLocalizationAndMapping，同时定位与建图)技术在特征点信息丰富的时候能够准确计算相机的姿态，通过回环检测可以实现全局一致性，但是在快速移动和特征点少的时候，相机通过视觉进行的跟踪容易失败，这时候建图和定位都不能继续进行。IMU(Inertialmeasurementunit，惯性测量单元)的成本低，不受特征点少喝快速运动的影响，但是数据噪声大，积分有严重的累积误差。视觉和IMU信息的融合通常使用基于扩展卡尔曼滤波和基于优化的方法，滤波的方法效率高，但是准确性差；优化方法估计量更准确，但是效率低，无法实现实时的全局优化。相机和IMU的变换矩阵在信息融合中非常重要，这个矩阵通常通过离线标定或者在线的初始化过程来标定。相关技术，RGB-D-InertialSLAM实现了实时视觉IMU信息的全局优化，但是全局优化的计算需要GPU(GraphicsProcessingUnit，图形处理器)，并没有对计算本身进行理论上的加速，无法运行在可便携设备上，相关技术存在的缺点如下：1、对于优化方面：(1)卡尔曼滤波：通过滤波的方法对视觉信息和IMU信息进行融合，缺点融合过程损失了一部分信息，得到的估计量不够准确，在有回环的时候不能实现全局一致性；(2)滑动窗口：基于滑动窗口进行的优化虽然实现了高效率，但是放弃了窗口之前的信息，估计量不够准确，也不能实现全局一致性；(3)直接进行的全局优化：效率太低，在没有GPU参与运算的情况下不能实现实时性；(4)松耦合优化：优化过程不稳定，容易震荡，优化量不够准确。2、对于相机IMU相对位置的标定：(1)离线标定：标定过程复杂，标定误差会累积，在IMU和相机发生位置变化的时候需要重新标定；(2)初始化标定：在运行开始选取一段观测量进行初始化，这时候确定变换矩阵。这样得到的变换矩阵不够准确，标定误差会累积；3、对于回环检测：只通过视觉信息进行回环检测很容易发生误判，损害全局一致性，使得三维重建的结果出现混乱。
技术实现思路
本申请是基于专利技术人对以下问题的认识和发现做出的：利用深度相机(RGBD相机)可以准确计算周围环境的三维坐标，从而进行三维重建。然而在快速移动的时候，相机的彩色图和深度图会出现运动模糊，在环境纹理特征少的时候，图像不能准确匹配特征点，这时候基于特征点匹配的定位方式不能对相机位姿进行准确定位而导致重建不能继续进行。通过IMU数据进行积分也可以得到相机的位置，但是IMU数据噪声很大，有明显的累积误差。通过视觉和IMU信息的融合可以实现双方的互补，可以应对快速移动和特征点少的情况，提供更加准确的姿态估计。但是在融合视觉和IMU信息的过程中，需要对大量的状态量进行估计，以往的方法通过优化方式进行信息融合计算效率很低，如果要保证实时性就不能实现全局优化，实现全局优化就不能在线运行在可便携设备上。本专利技术通过分析优化问题中主要计算复杂度的来源，通过二阶统计量直接计算了优化问题中复杂度最高的部分，而且这种加速计算的方式没有精度损失。实现了实时的视觉和IMU数据的全局优化，能够在三位重建中提供更加准确鲁棒的姿态估计。对于相机和IMU的相对位置实现了在线的自标定，自标定是耦合在全局优化上的，没有累积误差的问题。通过全局优化可以得到准确的IMU零偏估计值，根据零偏估计量的变化实现了回环检测的正确性判断。本专利技术旨在至少在一定程度上解决相关技术中的技术问题之一。为此，本专利技术的一个目的在于提出一种三维重建中实时全局优化和错误回环判断的方法，该方法可以在不损失精度的前提下实现了高效率的视觉和IMU信息的全局优化，且没有因为标定不准确导致的累积误差的问题，并可以进行准确的回环正确性的判断，简单易实现。本专利技术的另一个目的在于提出一种三维重建中实时全局优化和错误回环判断的装置。为达到上述目的，本专利技术一方面实施例提出了一种三维重建中实时全局优化和错误回环判断的方法，包括以下步骤：获取视觉信息和IMU信息，通过所述视觉信息的二次统计量和所述IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化；在全局优化函数中耦合变换矩阵，来提供准确的变换矩阵估计值，并且在全局优化过程中，提供IMU和相机的状态估计量，使得在快速移动和特征点少的时候，通过IMU信息进行定位，从而继续进行建图；获取IMU零偏的估计量，并根据所述IMU零偏的估计量识别错误回环和正确回环。本专利技术实施例的三维重建中实时全局优化和错误回环判断的方法，在不损失精度的前提下实现了高效率的视觉和IMU信息的全局优化，系统能够实时运行在可便携设备上；紧耦合的自标定可以得到准确的相机和IMU的变换矩阵，没有因为标定不准确导致的累积误差的问题；根据优化得到的IMU零偏的变化量能够进行准确的回环正确性的判断，简单易实现。另外，根据本专利技术上述实施例的三维重建中实时全局优化和错误回环判断的方法还可以具有以下附加的技术特征：进一步地，在本专利技术的一个实施例中，所述获取视觉信息和IMU信息，通过所述视觉信息的二次统计量和所述IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化，包括：在优化过程中，采用关键帧策略，以使关键帧的状态量参与全局优化，且其他帧的状态量通过最接近的关键帧进行赋值。进一步地，在本专利技术的一个实施例中，在全局优化过程中，还包括：根据视觉信息的和所述IMU信息的误差项获取雅各比矩阵；根据所述雅各比矩阵进行误差函数的最小化，并通过稀疏矩阵乘法得到状态更新量，以实现视觉IMU信息的全局优化。进一步地，在本专利技术的一个实施例中，所述误差函数为：其中，和均为相对位姿变化量，i和j表示帧数，为相机的IMU的角速度零偏，为根据IMU数据在初始零偏值的基础上积分得到的相对位姿变化量，bg为IMU的角速度零偏变量，Rci为相机的旋转方向，R0、P0均为相机到IMU的变换矩阵，VIj为第j帧相机IMU的速度，VIi为第i帧相机IMU的速度，Rg为重力的优化方向，g为初始的重力值，Δtij为第i帧和第j帧之间的时间间隔，为第i帧相机IMU的加速度零偏，Pci为相机的平移位置，ba为IMU的加速度零偏变量。进一步地，在本专利技术的一个实施例中，还包括：通过李代数的扰动模型求出误差相对于需要优化的状态变量的导数。为达到上述目的，本专利技术另一方面实施例提出了一种三维重建中实时全局优化和错误回环判断的装置，包括：构建模块，用于获取视觉信息和IMU信息，通过所述视觉信息的二次统计量和所述IMU信息的预计分构建优化本文档来自技高网...

【技术保护点】
1.一种三维重建中实时全局优化和错误回环判断的方法，其特征在于，包括以下步骤：/n获取视觉信息和IMU信息，通过所述视觉信息的二次统计量和所述IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化；/n在全局优化函数中耦合变换矩阵，来提供准确的变换矩阵估计值，并且在全局优化过程中，提供IMU和相机的状态估计量，使得在快速移动和特征点少的时候，通过IMU信息进行定位，从而继续进行建图；以及/n获取IMU零偏的估计量，并根据所述IMU零偏的估计量识别错误回环和正确回环。/n

【技术特征摘要】
1.一种三维重建中实时全局优化和错误回环判断的方法，其特征在于，包括以下步骤：
获取视觉信息和IMU信息，通过所述视觉信息的二次统计量和所述IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化；
在全局优化函数中耦合变换矩阵，来提供准确的变换矩阵估计值，并且在全局优化过程中，提供IMU和相机的状态估计量，使得在快速移动和特征点少的时候，通过IMU信息进行定位，从而继续进行建图；以及
获取IMU零偏的估计量，并根据所述IMU零偏的估计量识别错误回环和正确回环。


2.根据权利要求1所述的方法，其特征在于，所述获取视觉信息和IMU信息，通过所述视觉信息的二次统计量和所述IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化，包括：
在优化过程中，采用关键帧策略，以使关键帧的状态量参与全局优化，且其他帧的状态量通过最接近的关键帧进行赋值。


3.根据权利要求1所述的方法，其特征在于，在全局优化过程中，还包括：
根据视觉信息的和所述IMU信息的误差项获取雅各比矩阵；
根据所述雅各比矩阵进行误差函数的最小化，并通过稀疏矩阵乘法得到状态更新量，以实现视觉IMU信息的全局优化。


4.根据权利要求3所述的方法，其特征在于，所述误差函数为：









其中，和均为相对位姿变化量，i和j表示帧数，为相机的IMU的角速度零偏，为根据IMU数据在初始零偏值的基础上积分得到的相对位姿变化量，bg为IMU的角速度零偏变量，Rci为相机的旋转方向，R0、P0均为相机到IMU的变换矩阵，VIj为第j帧相机IMU的速度，VIi为第i帧相机IMU的速度，Rg为重力的优化方向，g为初始的重力值，Δtij为第i帧和第j帧之间的时间间隔，为第i帧相机IMU的加速度零偏，Pci为相机的平移位置，ba为IMU的加速度零偏变量。


5.根据权利要求4所述的方法，其特征在于，还包括：
通过李代数的扰动模型求出误差相对于需要优化的状态变量...

【专利技术属性】
技术研发人员：方璐，仲大伟，戴琼海，
申请(专利权)人：清华大学，
类型：发明
国别省市：北京;11