CN110986968B - 三维重建中实时全局优化和错误回环判断的方法及装置 - Google Patents

Abstract

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

Description

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

技术领域

本发明涉及相机定位与建图技术领域，特别涉及一种三维重建中实时全局优化和错误回环判断的方法及装置。

背景技术

随着机器人室内导航、虚拟现实及增强现实技术的发展和广泛应用，三维重建需要越来越高的精度和效率，需要应对各种不同的环境和运动状态，实现更加鲁棒的重建过程。而三维重建最需要的是通过视觉进行定位的基于SLAM(SimultaneousLocalizationAndMapping，同时定位与建图)技术在特征点信息丰富的时候能够准确计算相机的姿态，通过回环检测可以实现全局一致性，但是在快速移动和特征点少的时候，相机通过视觉进行的跟踪容易失败，这时候建图和定位都不能继续进行。IMU(Inertial measurement unit，惯性测量单元)的成本低，不受特征点少喝快速运动的影响，但是数据噪声大，积分有严重的累积误差。视觉和IMU信息的融合通常使用基于扩展卡尔曼滤波和基于优化的方法，滤波的方法效率高，但是准确性差；优化方法估计量更准确，但是效率低，无法实现实时的全局优化。相机和IMU的变换矩阵在信息融合中非常重要，这个矩阵通常通过离线标定或者在线的初始化过程来标定。

相关技术，RGB-D-Inertial SLAM实现了实时视觉IMU信息的全局优化，但是全局优化的计算需要GPU(Graphics Processing Unit，图形处理器)，并没有对计算本身进行理论上的加速，无法运行在可便携设备上，相关技术存在的缺点如下：

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数据在初始零偏值的基础上积分得到的相对位姿变化量，b^g为IMU的角速度零偏变量，R_ci为相机的旋转方向，R₀、P₀均为相机到IMU的变换矩阵，V_Ij为第j帧相机IMU的速度，V_Ii为第i帧相机IMU的速度，R_g为重力的优化方向，g为初始的重力值，Δt_ij为第i帧和第j帧之间的时间间隔，

为第i帧相机IMU的加速度零偏，P_ci为相机的平移位置，b^a为IMU的加速度零偏变量。

进一步地，在本发明的一个实施例中，还包括：通过李代数的扰动模型求出误差相对于需要优化的状态变量的导数。

为达到上述目的，本发明另一方面实施例提出了一种三维重建中实时全局优化和错误回环判断的装置，包括：构建模块，用于获取视觉信息和IMU信息，通过所述视觉信息的二次统计量和所述IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化；耦合模块，用于在全局优化函数中耦合变换矩阵，来提供准确的变换矩阵估计值，并且在全局优化过程中，提供IMU和相机的状态估计量，使得在快速移动和特征点少的时候，可以通过IMU信息进行定位，从而继续进行建图；识别模块，用于获取IMU零偏的估计量，并根据所述IMU零偏的估计量识别错误回环和正确回环。

本发明实施例的三维重建中实时全局优化和错误回环判断的装置，在不损失精度的前提下实现了高效率的视觉和IMU信息的全局优化，系统能够实时运行在可便携设备上；紧耦合的自标定可以得到准确的相机和IMU的变换矩阵，没有因为标定不准确导致的累积误差的问题；根据优化得到的IMU零偏的变化量能够进行准确的回环正确性的判断，简单易实现。

另外，根据本发明上述实施例的三维重建中实时全局优化和错误回环判断的装置还可以具有以下附加的技术特征：

进一步地，在本发明的一个实施例中，所述构建模块进一步用于在优化过程中，采用关键帧策略，以使关键帧的状态量参与全局优化，且其他帧的状态量通过最接近的关键帧进行赋值。

进一步地，在本发明的一个实施例中，在全局优化过程中，还包括：优化模块，用于根据视觉信息的和所述IMU信息的误差项获取雅各比矩阵，并根据所述雅各比矩阵进行误差函数的最小化，并通过稀疏矩阵乘法得到状态更新量，以实现视觉IMU信息的全局优化。

进一步地，在本发明的一个实施例中，所述误差函数为：

其中，

和

均为相对位姿变化量，i和j表示帧数，

为相机的IMU的角速度零偏，

为根据IMU数据在初始零偏值的基础上积分得到的相对位姿变化量，b^g为IMU的角速度零偏变量，R_ci为相机的旋转方向，R₀、P₀均为相机到IMU的变换矩阵，V_Ij为第j帧相机IMU的速度，V_Ii为第i帧相机IMU的速度，R_g为重力的优化方向，g为初始的重力值，Δt_ij为第i帧和第j帧之间的时间间隔，

为第i帧相机IMU的加速度零偏，P_ci为相机的平移位置，b^a为IMU的加速度零偏变量。

进一步地，在本发明的一个实施例中，还包括：求解模块，用于通过李代数的扰动模型求出误差相对于需要优化的状态变量的导数。

本发明附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：

图1为根据本发明实施例的三维重建中实时全局优化和错误回环判断的方法的流程图；

图2为根据本发明实施例的系统多线程结构图；

图3为根据本发明实施例的视觉和IMU观测结果构成的约束关系示意图；

图4为根据本发明实施例的三维重建结果示意图；

图5为根据本发明实施例的J(ξ)^TJ(ξ)的计算分解示意图；

图6为根据本发明实施例的实验设备实物图；

图7为根据本发明实施例的三维重建中实时全局优化和错误回环判断的装置的结构示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本发明，而不能理解为对本发明的限制。

下面参照附图描述根据本发明实施例提出的三维重建中实时全局优化和错误回环判断的方法及装置，首先将参照附图描述根据本发明实施例提出的三维重建中实时全局优化和错误回环判断的方法。

图1是本发明一个实施例的三维重建中实时全局优化和错误回环判断的方法的流程图。

如图1所示，该三维重建中实时全局优化和错误回环判断的方法包括以下步骤：

在步骤S101中，获取视觉信息和IMU信息，通过视觉信息的二次统计量和IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化。

可以理解的是，如图2所示，实现了视觉、IMU信息的全局优化，通过视觉的二次统计量和IMU的预计分来构建优化方程，大大加快了优化速度，从而实现了基于CPU的实时在线全局优化。

进一步地，在本发明的一个实施例中，获取视觉信息和IMU信息，通过视觉信息的二次统计量和IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化，包括：在优化过程中，采用关键帧策略，以使关键帧的状态量参与全局优化，且其他帧的状态量通过最接近的关键帧进行赋值。

具体而言，在优化中采用了关键帧策略，只有关键帧的状态量参与全局优化而其他帧的状态量通过最接近的关键帧来赋值。

定义的符号如下：第i个关键帧写作F_i,包含RGBD相机的拍摄数据彩色图I_i和深度图D_i。对于关键帧F_i,需要优化的状态变量分别是R_ci、P_ci、V_Ii、

表示了相机的旋转方向、平移位置、IMU的速度、IMU的角速度零偏、加速度零偏。R_Ii、P_Ii是对应的IMU的旋转和平移，R₀、P₀是相机到IMU的变换矩阵。g是初始的重力值，R_g是重力的优化方向。通过预先标定或者初始化过程得到的相机IMU变换矩阵和重力方向会引起误差积累，随着路径的增长会逐渐放大微小的误差。

另一方面，通过全局优化得到的相机IMU变换矩阵和重力方向将越来越精确，从而达到全局一致性，避免了误差积累。因此，本发明实施例在一个整体的全局优化函数中估计了相机的状态量R_ci、P_ci、V_Ii、

相机IMU变换矩阵R₀、P₀和重力方向R_g。R₀、P₀和R_g是全局变量。为了简化表示,使用S来表示全局优化中的所有的优化量:S＝{R,P,V,b^g,b^a,R₀,P₀,R_g}，其中

相邻关键帧之间的IMU数据进行积分可以得到相邻关键帧之间的运动约束

在匹配的关键帧对中，视觉找到的匹配特征点构建了视觉约束

全部匹配的关键帧对写作Ω。其中，视觉和IMU观测结果构成的约束关系如图3所示。

在步骤S102中，在全局优化函数中耦合变换矩阵，来提供准确的变换矩阵估计值，并且在全局优化过程中，提供IMU和相机的状态估计量，使得在快速移动和特征点少的时候，通过IMU信息进行定位，从而继续进行建图。

可以理解的是，如图2所示，在全局优化函数中耦合了变换矩阵的优化，可以实现在线的标定，不需要视觉初始化的过程，变换矩阵的优化会越来越准确，避免了误差的累积。全局优化提供了准确的状态估计量，使得系统在快速移动和特征点少的时候能够继续进行建图。其中，在建图线程中，TSDF融合提供三维模型，三维重建结果如图4所示。

进一步地，在本发明的一个实施例中，在全局优化过程中，还包括：根据视觉信息的和IMU信息的误差项获取雅各比矩阵；根据雅各比矩阵进行误差函数的最小化，并通过稀疏矩阵乘法得到状态更新量，以实现视觉IMU信息的全局优化。

可以理解的是，本发明实施例在视觉和IMU信息的全局优化中采用了视觉特征点的二阶统计量来构建视觉部分的雅各比矩阵，结合IMU的预积分构建了整体的优化方程，从而大大加快了全局优化的计算速度。在全局优化函数中耦合了相机和IMU的变换矩阵，从而在全局优化中实现了自标定，没有标定误差积累的问题。

具体而言，构建下面的误差函数，通过最小化误差函数求出S＝{R,P,V,b^g,b^a,R₀,P₀,R_g}的估计值：

得到误差函数1后，通过下面的高斯牛顿优化方法来计算状态量的更新量，实现误差函数的最小化：δ＝-(J(ξ)^TJ(ξ))^-1J(ξ)^Tr(ξ) (2)

其中J(ξ)是误差函数1对所有状态量求导得到的雅各比矩阵，r(ξ)是误差函数1得到的误差项，δ是求出的状态量的更新量。J(ξ)^TJ(ξ)和J(ξ)^Tr(ξ)的计算过程是优化问题中计算复杂度的主要来源，下面介绍J(ξ)^TJ(ξ)的计算分解过程和加速方法，J(ξ)^Tr(ξ)可以通过同样的方法进行计算加速。

通过图5可以看出，整体的雅各比矩阵J(ξ)是由视觉和IMU误差项分别求出的雅各比矩阵组合而成。C^F和C^I的计算复杂度是O(k*N_corr)和O(N)，其中N是关键帧的个数，N_corr是帧对的个数，k是平均每个帧对的匹配特征点对的个数。关键帧的个数远小于特征点对的个数，所以优化问题的计算复杂度主要存在于C^F，即J^TJ的计算耗费主要在图5的

上。加速计算的关键在于加快

的计算。其中J_A是特征点对的雅各比矩阵，本发明通过特征点的二阶统计量可以直接得到

在不损失计算精度的前提下实现了把C^F的计算复杂度从O(k*N_corr)降到了O(N_corr)，从而显著降低J^TJ的计算复杂度，下面是通过二阶统计量来计算

的具体方法：

每个匹配的帧对(F_i,F_j)有很多匹配的特征点对

这里P_i ^k表示第k个特征点在第i帧观测的坐标。对第i帧来说，T_ci是欧氏空间中的刚性变换矩阵，ξ_i是对应的李代数，R_ci、P_ci是变换矩阵的旋转和平移部分，T_ci＝[R_ci|P_ci]。在匹配的帧对中找到匹配的特征点对之后，对于每个匹配的特征点对，可以写出下面的误差函数：

通过组合所有的特征点的误差函数，可以得到下面的视觉部分整体的误差函数：

其中是P_i ^k第i帧相机坐标系下点坐标

的齐次坐标，第一帧的相机位姿T₀设为单位阵，由此初始化世界坐标系。N是关键帧的总数。对于视觉初始化来说，位姿估计可以通过最小化误差函数4来获得。对于相邻的关键帧F_i和F_j，可以通过视觉求出位姿R_ci、P_ci和R_cj、P_cj。

对位姿R_ci和P_ci求导的雅各比矩阵如下：

对于一个关键帧i和关键帧j之间的特征点对，有下面的子矩阵：

对于一个关键帧i和关键帧j的帧对，雅各比矩阵

是所有匹配特征点的子矩阵的和：

由于存在许多匹配关键帧之间的帧对，

是总的视觉雅各比矩阵

的子矩阵，所以有下面的关系式

J_m只有两个非零的3*6的子矩阵，所以

(m∈C_i,j)只有四个非零的处于在相同位置的6*6的子矩阵，作为

(m∈C_i,j)的组合，

有四个6*6的非零子矩阵，分别写作

下面展示了

的计算过程，其他的子矩阵可以通过同样的方法计算得到：

通过式9可以实现直接由视觉的二阶统计量得到

的结果，

是

的求和。从而可以实现通过视觉的二阶统计量得到

的结果，大大降低了视觉部分雅各比矩阵的计算复杂度。

下面介绍IMU部分的雅各比矩阵的计算过程：

相机和IMU的变换矩阵是R₀、P₀，可以获得下面的从相机坐标系到IMU坐标系的转换：,

R_Ii＝R_ciR₀

P_Ii＝P_ci+R_ciP₀ (10)

IMU获取的角速度和加速度写作

真实的角速度和加速度写作ω、α，角速度和加速度的噪声写作η^g、η^a；有下面的关系式：

假设在t-[t+Δt]时刻角速度和加速度保持不变，可以通过t时刻的状态获取t+Δt时刻的状态；

R_I(t+Δt)＝R_I(t)Exp(ω(t)Δt)

V_I(t+Δt)＝V_I(t)+a(t)Δt (12)

把11带入12，可以得到下面的关系式：

通过多段IMU数据的积分可以得到i到j时刻的位姿关系式：

为了避免重复积分，计算下面的位姿变化量：

通过下面的方式分离ΔR_ij中的噪声：

通过同样的方法可以得到

在通过视觉的方法得到相机位姿之后可以经过转换得到IMU的位姿，从而在相邻的关键帧之间写出下面的位姿变化量：

同时，通过IMU数据的积分获得了关键帧之间的相对位姿变化量

和

结合式17可以得到下面的误差函数：

通过零偏的线性化可以将式18写成下面的形式：

利用式10把相机位姿带入，通过R_gg来代替R_g，可以得到下面的误差函数：

进一步地，在本发明的一个实施例中，还包括：通过李代数的扰动模型求出误差相对于需要优化的状态变量的导数。

具体地，然后可以通过李代数的扰动模型求出误差

相对于需要优化的状态变量(S＝{R,P,V,b^g,b^a,R₀,P₀,R_g})的雅各比矩阵。全局变量R₀、P₀和R_g紧耦合在全局优化函数中，从而实现了在全局优化过程中进行自标定，标定结果不会产生误差积累。得到IMU误差项的雅各比矩阵，即得到了图5中的J_B、J_C、J_D，加上视觉部分已经得到的

可以计算得到图5中的J(ξ)^TJ(ξ)，通过同样的方法可以得到J(ξ)^Tr(ξ)。通过稀疏矩阵乘法可以得到状态更新量δ，从而实现了视觉IMU信息的全局优化。

在步骤S103中，获取IMU零偏的估计量，并根据IMU零偏的估计量识别错误回环和正确回环。

可以理解的是，通过IMU零偏的估计量来检测回环检测的正确性，使得系统能够准确识别错误和正确回环；在检测到回环的时候通过视觉和IMU的全局优化实现了全局一致性，消除了误差积累。也就是说，本发明实施例可以在全局优化后通过IMU零偏的变化量来实现错误回环的去除，能够准确识别正确回环和错误回环。

具体而言，错误回环检测的判断：回环检测可以实现建图的全局一致性，而错误的回环检测会破坏这种全局一致性；在发生正确回环检测的时候，零偏的估计量不会发生大的变化，而发生错误的回环检测时破坏了建图的一致性，为了拟合错误回环会产生轨迹的不合理形变，IMU的零偏估计量的更新量会发生突然的增大。所以根据IMU零偏估计量的变化可以判断当前回环检测的正确性。如果IMU零偏估计量发生了突变，就取消当前的回环检测，这样可以实现准确的错误回环的去除，提高了系统的鲁棒性。

综上，本发明实施例的目的为：分析全局优化中主要复杂度的来源，通过推导实现加速运算，使得整体的计算复杂度大大下降，实现了基于CPU的视觉IMU信息实时全局优化，保证了全局一致性，使系统能够运行在可便携设备上。同时在全局优化中加入紧耦合的自标定，避免了离线标定的复杂流程和误差积累，也避免了初始化过程的误差问题。通过全局优化获得准确的IMU零偏估计之后，通过IMU零偏的变化量准确识别错误回环和正确回环。

另外，本发明实施例使用如图6的实验设备：Xtion深度相机+IMU(SC-AHRS-100D2)+微软Surface Pro，进行实验、模拟、使用，实验结果验证了全局优化的速度获得了大幅度提高，可以在可便携设备上实现实时的基于视觉和IMU信息全局优化的三维重建，在视觉跟踪失败的时候利用IMU信息可以实现准确的相机定位，可以成功应对场景纹理特征少和快速移动的情况。能够成功识别回环检测的正确性，从而保证了全局一致性。

需要说明的是，本发明实施例可以通过变更设计实现三维重建中实时全局优化和错误回环判断的方法，比如，通过视觉信息的统计量构建视觉优化的雅各比矩阵，只进行视觉的优化；更改视觉和IMU全局优化时的整体雅各比矩阵的排列结构；通过IMU零偏的变化判断系统的运行状况，检测视觉跟踪的正确性。且本发明实施例还具有其他用途，比如，在视觉和IMU信息联合优化的时候，通过本方法来加速运算。

根据本发明实施例提出的三维重建中实时全局优化和错误回环判断的方法，在不损失精度的前提下实现了高效率的视觉和IMU信息的全局优化，系统能够实时运行在可便携设备上；紧耦合的自标定可以得到准确的相机和IMU的变换矩阵，没有因为标定不准确导致的累积误差的问题；根据优化得到的IMU零偏的变化量能够进行准确的回环正确性的判断，简单易实现。

其次参照附图描述根据本发明实施例提出的三维重建中实时全局优化和错误回环判断的装置。

图7是本发明一个实施例的三维重建中实时全局优化和错误回环判断的装置的结构示意图。

如图7所示，该三维重建中实时全局优化和错误回环判断的装置10包括：构建模块100、耦合模块200和识别模块300。

其中，构建模块100用于获取视觉信息和IMU信息，通过视觉信息的二次统计量和IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化。耦合模块200用于在全局优化函数中耦合变换矩阵，来提供准确的变换矩阵估计值，并且在全局优化过程中，提供IMU和相机的状态估计量，使得在快速移动和特征点少的时候，通过IMU信息进行定位，从而继续进行建图。识别模块300用于获取IMU零偏的估计量，并根据IMU零偏的估计量识别错误回环和正确回环。本发明实施例的装置10可以在不损失精度的前提下实现了高效率的视觉和IMU信息的全局优化，且没有因为标定不准确导致的累积误差的问题，并可以进行准确的回环正确性的判断，简单易实现。

进一步地，在本发明的一个实施例中，构建模块100进一步用于在优化过程中，采用关键帧策略，以使关键帧的状态量参与全局优化，且其他帧的状态量通过最接近的关键帧进行赋值。

进一步地，在本发明的一个实施例中，在全局优化过程中，本发明实施例的装置10还包括：优化模块。其中，优化模块用于根据视觉信息的和IMU信息的误差项获取雅各比矩阵，并根据雅各比矩阵进行误差函数的最小化，并通过稀疏矩阵乘法得到状态更新量，以实现视觉IMU信息的全局优化。

进一步地，在本发明的一个实施例中，误差函数为：

其中，

和

均为相对位姿变化量，i和j表示帧数，

为相机的IMU的角速度零偏，

为根据IMU数据在初始零偏值的基础上积分得到的相对位姿变化量，b^g为IMU的角速度零偏变量，R_ci为相机的旋转方向，R₀、P₀均为相机到IMU的变换矩阵，V_Ij为第j帧相机IMU的速度，V_Ii为第i帧相机IMU的速度，R_g为重力的优化方向，g为初始的重力值，Δt_ij为第i帧和第j帧之间的时间间隔，

为第i帧相机IMU的加速度零偏，P_ci为相机的平移位置，b^a为IMU的加速度零偏变量。

进一步地，在本发明的一个实施例中，本发明实施例的装置10还包括：求解模块。其中，求解模块用于通过李代数的扰动模型求出误差相对于需要优化的状态变量的导数。

需要说明的是，前述对三维重建中实时全局优化和错误回环判断的方法实施例的解释说明也适用于该实施例的三维重建中实时全局优化和错误回环判断的装置，此处不再赘述。

根据本发明实施例提出的三维重建中实时全局优化和错误回环判断的装置，在不损失精度的前提下实现了高效率的视觉和IMU信息的全局优化，系统能够实时运行在可便携设备上；紧耦合的自标定可以得到准确的相机和IMU的变换矩阵，没有因为标定不准确导致的累积误差的问题；根据优化得到的IMU零偏的变化量能够进行准确的回环正确性的判断，简单易实现。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。

尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (8)

1.一种三维重建中实时全局优化和错误回环判断的方法，其特征在于，包括以下步骤：

获取视觉信息和IMU信息，通过所述视觉信息的二阶统计量和所述IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化，其中，通过二阶统计量来计算总的视觉雅各比矩阵

的具体方法为：

每个匹配的帧对(F_i,F_j)有多个匹配的特征点对

其中，P_i ^k表示第k个特征点在第i帧观测的坐标，对第i帧来说，T_ci是欧氏空间中的刚性变换矩阵，ξ_i是对应的李代数，R_ci、P_ci是变换矩阵的旋转和平移部分，T_ci＝[R_ci|P_ci]，在匹配的帧对中找到匹配的特征点对后，对每个匹配的特征点对的误差函数为：

通过组合所有的特征点的误差函数，得到视觉部分整体的误差函数：

其中是P_i ^k第i帧相机坐标系下点坐标

的齐次坐标，第一帧的相机位姿T₀设为单位阵，初始化世界坐标系，N是关键帧的总数，视觉初始化中位姿估计通过最小化误差函数来获得，对于相邻的关键帧F_i和F_j，根据视觉求出位姿R_ci、P_ci和R_cj、P_cj，

对位姿R_ci和P_ci求导的雅各比矩阵为：

一个关键帧i和关键帧j之间的特征点对，有子矩阵为：

对于一个关键帧i和关键帧j的帧对，雅各比矩阵

是所有匹配特征点的子矩阵的和：

是总的视觉雅各比矩阵

的子矩阵，其关系式为：

有四个6*6的非零子矩阵，分别写作

的计算过程为：

在全局优化函数中耦合变换矩阵，来提供准确的变换矩阵估计值，并且在全局优化过程中，提供IMU和相机的状态估计量，使得在快速移动和特征点少的时候，通过IMU信息进行定位，从而继续进行建图，其中，第i个关键帧写作F_i,包含RGBD相机的拍摄数据彩色图I_i和深度图D_i，对于关键帧F_i,需要优化的状态变量分别是R_ci、P_ci、V_Ii、

表示了相机的旋转方向、平移位置、IMU的速度、IMU的角速度零偏、加速度零偏，R_Ii、P_Ii是对应的IMU的旋转和平移，R₀、P₀是相机到IMU的变换矩阵，g是初始的重力值，R_g是重力的优化方向，在一个整体的全局优化函数中相机的状态量R_ci、P_ci、V_Ii、

相机IMU变换矩阵R₀、P₀和重力方向R_g，R₀、P₀和R_g是全局变量，使用S来表示全局优化中的所有的优化量:S＝{R,P,V,b^g,b^a,R₀,P₀,R_g}，其中

相邻关键帧之间的IMU数据进行积分得到相邻关键帧之间的运动约束

在匹配的关键帧对中，视觉找到的匹配特征点构建视觉约束

全部匹配的关键帧对写作Ω，在全局优化过程中，还包括：根据视觉信息的和IMU信息的误差项获取雅各比矩阵；根据雅各比矩阵进行误差函数的最小化，并通过稀疏矩阵乘法得到状态更新量，以实现视觉IMU信息的全局优化，构建误差函数，通过最小化误差函数求出S＝{R,P,V,b^g,b^a,R₀,P₀,R_g}的估计值：

以及

获取IMU零偏的估计量，并根据所述IMU零偏的估计量识别错误回环和正确回环。

2.根据权利要求1所述的方法，其特征在于，所述获取视觉信息和IMU信息，通过所述视觉信息的二次统计量和所述IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化，包括：

在优化过程中，采用关键帧策略，以使关键帧的状态量参与全局优化，且其他帧的状态量通过最接近的关键帧进行赋值。

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

其中，

和

均为相对位姿变化量，i和j表示帧数，

为相机的IMU的角速度零偏，

为根据IMU数据在初始零偏值的基础上积分得到的相对位姿变化量，b^g为IMU的角速度零偏变量，R_ci为相机的旋转方向，R₀、P₀均为相机到IMU的变换矩阵，V_Ij为第j帧相机IMU的速度，V_Ii为第i帧相机IMU的速度，R_g为重力的优化方向，g为初始的重力值，Δt_ij为第i帧和第j帧之间的时间间隔，

为第i帧相机IMU的加速度零偏，P_ci为相机的平移位置，b^a为IMU的加速度零偏变量。

4.根据权利要求3所述的方法，其特征在于，还包括：

通过李代数的扰动模型求出误差相对于需要优化的状态变量的导数。

5.一种三维重建中实时全局优化和错误回环判断的装置，其特征在于，包括：

构建模块，用于获取视觉信息和IMU信息，通过所述视觉信息的二阶统计量和所述IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化，其中，通过二阶统计量来计算总的视觉雅各比矩阵

的具体方法为：

每个匹配的帧对(F_i,F_j)有多个匹配的特征点对

其中，P_i ^k表示第k个特征点在第i帧观测的坐标，对第i帧来说，T_ci是欧氏空间中的刚性变换矩阵，ξ_i是对应的李代数，R_ci、P_ci是变换矩阵的旋转和平移部分，T_ci＝[R_ci|P_ci]，在匹配的帧对中找到匹配的特征点对后，对每个匹配的特征点对的误差函数为：

通过组合所有的特征点的误差函数，得到视觉部分整体的误差函数：

其中是P_i ^k第i帧相机坐标系下点坐标

的齐次坐标，第一帧的相机位姿T₀设为单位阵，初始化世界坐标系，N是关键帧的总数，视觉初始化中位姿估计通过最小化误差函数来获得，对于相邻的关键帧F_i和F_j，根据视觉求出位姿R_ci、P_ci和R_cj、P_cj，

对位姿R_ci和P_ci求导的雅各比矩阵为：

一个关键帧i和关键帧j之间的特征点对，有子矩阵为：

对于一个关键帧i和关键帧j的帧对，雅各比矩阵

是所有匹配特征点的子矩阵的和：

是总的视觉雅各比矩阵

的子矩阵，其关系式为：

有四个6*6的非零子矩阵，分别写作

的计算过程为：

耦合模块，用于在全局优化函数中耦合变换矩阵，来提供准确的变换矩阵估计值，并且在全局优化过程中，提供IMU和相机的状态估计量，使得在快速移动和特征点少的时候，通过IMU信息进行定位，从而继续进行建图，其中，第i个关键帧写作F_i,包含RGBD相机的拍摄数据彩色图I_i和深度图D_i，对于关键帧F_i,需要优化的状态变量分别是R_ci、P_ci、V_Ii、

表示了相机的旋转方向、平移位置、IMU的速度、IMU的角速度零偏、加速度零偏，R_Ii、P_Ii是对应的IMU的旋转和平移，R₀、P₀是相机到IMU的变换矩阵，g是初始的重力值，R_g是重力的优化方向，在一个整体的全局优化函数中相机的状态量R_ci、P_ci、V_Ii、

相机IMU变换矩阵R₀、P₀和重力方向R_g，R₀、P₀和R_g是全局变量，使用S来表示全局优化中的所有的优化量:S＝{R,P,V,b^g,b^a,R₀,P₀,R_g}，其中

相邻关键帧之间的IMU数据进行积分得到相邻关键帧之间的运动约束

在匹配的关键帧对中，视觉找到的匹配特征点构建视觉约束

全部匹配的关键帧对写作Ω，在全局优化过程中，还包括：根据视觉信息的和IMU信息的误差项获取雅各比矩阵；根据雅各比矩阵进行误差函数的最小化，并通过稀疏矩阵乘法得到状态更新量，以实现视觉IMU信息的全局优化，构建误差函数，通过最小化误差函数求出S＝{R,P,V,b^g,b^a,R₀,P₀,R_g}的估计值：

以及

识别模块，用于获取IMU零偏的估计量，并根据所述IMU零偏的估计量识别错误回环和正确回环。

6.根据权利要求5所述的装置，其特征在于，所述构建模块进一步用于在优化过程中，采用关键帧策略，以使关键帧的状态量参与全局优化，且其他帧的状态量通过最接近的关键帧进行赋值。

7.根据权利要求5所述的装置，其特征在于，所述误差函数为：

其中，

和

均为相对位姿变化量，i和j表示帧数，

为相机的IMU的角速度零偏，

为根据IMU数据在初始零偏值的基础上积分得到的相对位姿变化量，b^g为IMU的角速度零偏变量，R_ci为相机的旋转方向，R₀、P₀均为相机到IMU的变换矩阵，V_Ij为第j帧相机IMU的速度，V_Ii为第i帧相机IMU的速度，R_g为重力的优化方向，g为初始的重力值，Δt_ij为第i帧和第j帧之间的时间间隔，

为第i帧相机IMU的加速度零偏，P_ci为相机的平移位置，b^a为IMU的加速度零偏变量。

8.根据权利要求7所述的装置，其特征在于，还包括：

求解模块，用于通过李代数的扰动模型求出误差相对于需要优化的状态变量的导数。

Images