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

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

Info

Publication number
CN110986968B
CN110986968B CN201910968740.7A CN201910968740A CN110986968B CN 110986968 B CN110986968 B CN 110986968B CN 201910968740 A CN201910968740 A CN 201910968740A CN 110986968 B CN110986968 B CN 110986968B
Authority
CN
China
Prior art keywords
imu
camera
frame
global optimization
information
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910968740.7A
Other languages
English (en)
Other versions
CN110986968A (zh
Inventor
方璐
仲大伟
戴琼海
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Tsinghua University
Original Assignee
Tsinghua University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Tsinghua University filed Critical Tsinghua University
Priority to CN201910968740.7A priority Critical patent/CN110986968B/zh
Publication of CN110986968A publication Critical patent/CN110986968A/zh
Priority to US17/036,457 priority patent/US11514607B2/en
Application granted granted Critical
Publication of CN110986968B publication Critical patent/CN110986968B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/55—Depth or shape recovery from multiple images
    • G06T7/579—Depth or shape recovery from multiple images from motion
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • G01C21/32—Structuring or formatting of map data
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T15/00—3D [Three Dimensional] image rendering
    • G06T15/10—Geometric effects
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T15/00—3D [Three Dimensional] image rendering
    • G06T15/10—Geometric effects
    • G06T15/20—Perspective computation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10024—Color image
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/10—Image acquisition modality
    • G06T2207/10028—Range image; Depth image; 3D point clouds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30232—Surveillance
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Geometry (AREA)
  • Computer Graphics (AREA)
  • Multimedia (AREA)
  • Computing Systems (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)

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信息的全局优化。
进一步地，在本发明的一个实施例中，所述误差函数为：
Figure BDA0002231374180000031
Figure BDA0002231374180000032
Figure BDA0002231374180000033
Figure BDA0002231374180000034
Figure BDA0002231374180000035
其中，
Figure BDA0002231374180000036
和
Figure BDA0002231374180000037
均为相对位姿变化量，i和j表示帧数，
Figure BDA0002231374180000039
为相机的IMU的角速度零偏，
Figure BDA0002231374180000038
为根据IMU数据在初始零偏值的基础上积分得到的相对位姿变化量，bg为IMU的角速度零偏变量，Rci为相机的旋转方向，R0、P0均为相机到IMU的变换矩阵，VIj为第j帧相机IMU的速度，VIi为第i帧相机IMU的速度，Rg为重力的优化方向，g为初始的重力值，Δtij为第i帧和第j帧之间的时间间隔，
Figure BDA00022313741800000310
为第i帧相机IMU的加速度零偏，Pci为相机的平移位置，ba为IMU的加速度零偏变量。
进一步地，在本发明的一个实施例中，还包括：通过李代数的扰动模型求出误差相对于需要优化的状态变量的导数。
为达到上述目的，本发明另一方面实施例提出了一种三维重建中实时全局优化和错误回环判断的装置，包括：构建模块，用于获取视觉信息和IMU信息，通过所述视觉信息的二次统计量和所述IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化；耦合模块，用于在全局优化函数中耦合变换矩阵，来提供准确的变换矩阵估计值，并且在全局优化过程中，提供IMU和相机的状态估计量，使得在快速移动和特征点少的时候，可以通过IMU信息进行定位，从而继续进行建图；识别模块，用于获取IMU零偏的估计量，并根据所述IMU零偏的估计量识别错误回环和正确回环。
本发明实施例的三维重建中实时全局优化和错误回环判断的装置，在不损失精度的前提下实现了高效率的视觉和IMU信息的全局优化，***能够实时运行在可便携设备上；紧耦合的自标定可以得到准确的相机和IMU的变换矩阵，没有因为标定不准确导致的累积误差的问题；根据优化得到的IMU零偏的变化量能够进行准确的回环正确性的判断，简单易实现。
另外，根据本发明上述实施例的三维重建中实时全局优化和错误回环判断的装置还可以具有以下附加的技术特征：
进一步地，在本发明的一个实施例中，所述构建模块进一步用于在优化过程中，采用关键帧策略，以使关键帧的状态量参与全局优化，且其他帧的状态量通过最接近的关键帧进行赋值。
进一步地，在本发明的一个实施例中，在全局优化过程中，还包括：优化模块，用于根据视觉信息的和所述IMU信息的误差项获取雅各比矩阵，并根据所述雅各比矩阵进行误差函数的最小化，并通过稀疏矩阵乘法得到状态更新量，以实现视觉IMU信息的全局优化。
进一步地，在本发明的一个实施例中，所述误差函数为：
Figure BDA0002231374180000041
Figure BDA0002231374180000042
Figure BDA0002231374180000043
Figure BDA0002231374180000044
Figure BDA0002231374180000051
其中，
Figure BDA0002231374180000052
和
Figure BDA0002231374180000053
均为相对位姿变化量，i和j表示帧数，
Figure BDA0002231374180000055
为相机的IMU的角速度零偏，
Figure BDA0002231374180000054
为根据IMU数据在初始零偏值的基础上积分得到的相对位姿变化量，bg为IMU的角速度零偏变量，Rci为相机的旋转方向，R0、P0均为相机到IMU的变换矩阵，VIj为第j帧相机IMU的速度，VIi为第i帧相机IMU的速度，Rg为重力的优化方向，g为初始的重力值，Δtij为第i帧和第j帧之间的时间间隔，
Figure BDA0002231374180000056
为第i帧相机IMU的加速度零偏，Pci为相机的平移位置，ba为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个关键帧写作Fi,包含RGBD相机的拍摄数据彩色图Ii和深度图Di。对于关键帧Fi,需要优化的状态变量分别是Rci、Pci、VIi、
Figure BDA0002231374180000061
表示了相机的旋转方向、平移位置、IMU的速度、IMU的角速度零偏、加速度零偏。RIi、PIi是对应的IMU的旋转和平移，R0、P0是相机到IMU的变换矩阵。g是初始的重力值，Rg是重力的优化方向。通过预先标定或者初始化过程得到的相机IMU变换矩阵和重力方向会引起误差积累，随着路径的增长会逐渐放大微小的误差。
另一方面，通过全局优化得到的相机IMU变换矩阵和重力方向将越来越精确，从而达到全局一致性，避免了误差积累。因此，本发明实施例在一个整体的全局优化函数中估计了相机的状态量Rci、Pci、VIi、
Figure BDA0002231374180000062
相机IMU变换矩阵R0、P0和重力方向Rg。R0、P0和Rg是全局变量。为了简化表示,使用S来表示全局优化中的所有的优化量:S＝{R,P,V,bg,ba,R0,P0,Rg}，其中
Figure BDA0002231374180000063
相邻关键帧之间的IMU数据进行积分可以得到相邻关键帧之间的运动约束
Figure BDA0002231374180000064
在匹配的关键帧对中，视觉找到的匹配特征点构建了视觉约束
Figure BDA0002231374180000065
全部匹配的关键帧对写作Ω。其中，视觉和IMU观测结果构成的约束关系如图3所示。
在步骤S102中，在全局优化函数中耦合变换矩阵，来提供准确的变换矩阵估计值，并且在全局优化过程中，提供IMU和相机的状态估计量，使得在快速移动和特征点少的时候，通过IMU信息进行定位，从而继续进行建图。
可以理解的是，如图2所示，在全局优化函数中耦合了变换矩阵的优化，可以实现在线的标定，不需要视觉初始化的过程，变换矩阵的优化会越来越准确，避免了误差的累积。全局优化提供了准确的状态估计量，使得***在快速移动和特征点少的时候能够继续进行建图。其中，在建图线程中，TSDF融合提供三维模型，三维重建结果如图4所示。
进一步地，在本发明的一个实施例中，在全局优化过程中，还包括：根据视觉信息的和IMU信息的误差项获取雅各比矩阵；根据雅各比矩阵进行误差函数的最小化，并通过稀疏矩阵乘法得到状态更新量，以实现视觉IMU信息的全局优化。
可以理解的是，本发明实施例在视觉和IMU信息的全局优化中采用了视觉特征点的二阶统计量来构建视觉部分的雅各比矩阵，结合IMU的预积分构建了整体的优化方程，从而大大加快了全局优化的计算速度。在全局优化函数中耦合了相机和IMU的变换矩阵，从而在全局优化中实现了自标定，没有标定误差积累的问题。
具体而言，构建下面的误差函数，通过最小化误差函数求出S＝{R,P,V,bg,ba,R0,P0,Rg}的估计值：
Figure BDA0002231374180000071
得到误差函数1后，通过下面的高斯牛顿优化方法来计算状态量的更新量，实现误差函数的最小化：δ＝-(J(ξ)TJ(ξ))-1J(ξ)Tr(ξ) (2)
其中J(ξ)是误差函数1对所有状态量求导得到的雅各比矩阵，r(ξ)是误差函数1得到的误差项，δ是求出的状态量的更新量。J(ξ)TJ(ξ)和J(ξ)Tr(ξ)的计算过程是优化问题中计算复杂度的主要来源，下面介绍J(ξ)TJ(ξ)的计算分解过程和加速方法，J(ξ)Tr(ξ)可以通过同样的方法进行计算加速。
通过图5可以看出，整体的雅各比矩阵J(ξ)是由视觉和IMU误差项分别求出的雅各比矩阵组合而成。CF和CI的计算复杂度是O(k*Ncorr)和O(N)，其中N是关键帧的个数，Ncorr是帧对的个数，k是平均每个帧对的匹配特征点对的个数。关键帧的个数远小于特征点对的个数，所以优化问题的计算复杂度主要存在于CF，即JTJ的计算耗费主要在图5的
Figure BDA0002231374180000081
上。加速计算的关键在于加快
Figure BDA0002231374180000082
的计算。其中JA是特征点对的雅各比矩阵，本发明通过特征点的二阶统计量可以直接得到
Figure BDA0002231374180000083
在不损失计算精度的前提下实现了把CF的计算复杂度从O(k*Ncorr)降到了O(Ncorr)，从而显著降低JTJ的计算复杂度，下面是通过二阶统计量来计算
Figure BDA0002231374180000084
的具体方法：
每个匹配的帧对(Fi,Fj)有很多匹配的特征点对
Figure BDA0002231374180000085
这里Pi k表示第k个特征点在第i帧观测的坐标。对第i帧来说，Tci是欧氏空间中的刚性变换矩阵，ξi是对应的李代数，Rci、Pci是变换矩阵的旋转和平移部分，Tci＝[Rci|Pci]。在匹配的帧对中找到匹配的特征点对之后，对于每个匹配的特征点对，可以写出下面的误差函数：
Figure BDA0002231374180000086
通过组合所有的特征点的误差函数，可以得到下面的视觉部分整体的误差函数：
Figure BDA0002231374180000087
其中是Pi k第i帧相机坐标系下点坐标
Figure BDA0002231374180000088
的齐次坐标，第一帧的相机位姿T0设为单位阵，由此初始化世界坐标系。N是关键帧的总数。对于视觉初始化来说，位姿估计可以通过最小化误差函数4来获得。对于相邻的关键帧Fi和Fj，可以通过视觉求出位姿Rci、Pci和Rcj、Pcj。
Figure BDA0002231374180000089
对位姿Rci和Pci求导的雅各比矩阵如下：
Figure BDA00022313741800000810
对于一个关键帧i和关键帧j之间的特征点对，有下面的子矩阵：
Figure BDA00022313741800000811
对于一个关键帧i和关键帧j的帧对，雅各比矩阵
Figure BDA00022313741800000812
是所有匹配特征点的子矩阵的和：
Figure BDA00022313741800000813
由于存在许多匹配关键帧之间的帧对，
Figure BDA00022313741800000814
是总的视觉雅各比矩阵
Figure BDA00022313741800000815
的子矩阵，所以有下面的关系式
Figure BDA0002231374180000091
Jm只有两个非零的3*6的子矩阵，所以
Figure BDA0002231374180000092
(m∈Ci,j)只有四个非零的处于在相同位置的6*6的子矩阵，作为
Figure BDA0002231374180000093
(m∈Ci,j)的组合，
Figure BDA0002231374180000094
有四个6*6的非零子矩阵，分别写作
Figure BDA0002231374180000095
下面展示了
Figure BDA0002231374180000096
的计算过程，其他的子矩阵可以通过同样的方法计算得到：
Figure BDA0002231374180000097
通过式9可以实现直接由视觉的二阶统计量得到
Figure BDA0002231374180000098
的结果，
Figure BDA0002231374180000099
是
Figure BDA00022313741800000910
的求和。从而可以实现通过视觉的二阶统计量得到
Figure BDA00022313741800000911
的结果，大大降低了视觉部分雅各比矩阵的计算复杂度。
下面介绍IMU部分的雅各比矩阵的计算过程：
相机和IMU的变换矩阵是R0、P0，可以获得下面的从相机坐标系到IMU坐标系的转换：,
RIi＝RciR0
PIi＝Pci+RciP0 (10)
IMU获取的角速度和加速度写作
Figure BDA00022313741800000912
真实的角速度和加速度写作ω、α，角速度和加速度的噪声写作ηg、ηa；有下面的关系式：
Figure BDA00022313741800000913
Figure BDA00022313741800000914
假设在t-[t+Δt]时刻角速度和加速度保持不变，可以通过t时刻的状态获取t+Δt时刻的状态；
RI(t+Δt)＝RI(t)Exp(ω(t)Δt)
VI(t+Δt)＝VI(t)+a(t)Δt (12)
Figure BDA0002231374180000101
把11带入12，可以得到下面的关系式：
Figure BDA0002231374180000102
Figure BDA0002231374180000103
Figure BDA0002231374180000104
通过多段IMU数据的积分可以得到i到j时刻的位姿关系式：
Figure BDA0002231374180000105
Figure BDA0002231374180000106
Figure BDA0002231374180000107
为了避免重复积分，计算下面的位姿变化量：
Figure BDA0002231374180000108
Figure BDA0002231374180000109
Figure BDA00022313741800001010
通过下面的方式分离ΔRij中的噪声：
Figure BDA0002231374180000111
Figure BDA0002231374180000112
通过同样的方法可以得到
Figure BDA0002231374180000113
在通过视觉的方法得到相机位姿之后可以经过转换得到IMU的位姿，从而在相邻的关键帧之间写出下面的位姿变化量：
Figure BDA0002231374180000114
Figure BDA0002231374180000115
Figure BDA0002231374180000116
同时，通过IMU数据的积分获得了关键帧之间的相对位姿变化量
Figure BDA0002231374180000117
和
Figure BDA0002231374180000118
结合式17可以得到下面的误差函数：
Figure BDA0002231374180000119
Figure BDA00022313741800001110
Figure BDA00022313741800001111
通过零偏的线性化可以将式18写成下面的形式：
Figure BDA00022313741800001112
Figure BDA00022313741800001113
Figure BDA00022313741800001114
利用式10把相机位姿带入，通过Rgg来代替Rg，可以得到下面的误差函数：
Figure BDA0002231374180000121
Figure BDA0002231374180000122
Figure BDA0002231374180000123
进一步地，在本发明的一个实施例中，还包括：通过李代数的扰动模型求出误差相对于需要优化的状态变量的导数。
具体地，然后可以通过李代数的扰动模型求出误差
Figure BDA0002231374180000124
相对于需要优化的状态变量(S＝{R,P,V,bg,ba,R0,P0,Rg})的雅各比矩阵。全局变量R0、P0和Rg紧耦合在全局优化函数中，从而实现了在全局优化过程中进行自标定，标定结果不会产生误差积累。得到IMU误差项的雅各比矩阵，即得到了图5中的JB、JC、JD，加上视觉部分已经得到的
Figure BDA0002231374180000125
可以计算得到图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信息的全局优化。
进一步地，在本发明的一个实施例中，误差函数为：
Figure BDA0002231374180000141
Figure BDA0002231374180000142
Figure BDA0002231374180000143
其中，
Figure BDA0002231374180000144
和
Figure BDA0002231374180000145
均为相对位姿变化量，i和j表示帧数，
Figure BDA0002231374180000146
为相机的IMU的角速度零偏，
Figure BDA0002231374180000147
为根据IMU数据在初始零偏值的基础上积分得到的相对位姿变化量，bg为IMU的角速度零偏变量，Rci为相机的旋转方向，R0、P0均为相机到IMU的变换矩阵，VIj为第j帧相机IMU的速度，VIi为第i帧相机IMU的速度，Rg为重力的优化方向，g为初始的重力值，Δtij为第i帧和第j帧之间的时间间隔，
Figure BDA0002231374180000148
为第i帧相机IMU的加速度零偏，Pci为相机的平移位置，ba为IMU的加速度零偏变量。
进一步地，在本发明的一个实施例中，本发明实施例的装置10还包括：求解模块。其中，求解模块用于通过李代数的扰动模型求出误差相对于需要优化的状态变量的导数。
需要说明的是，前述对三维重建中实时全局优化和错误回环判断的方法实施例的解释说明也适用于该实施例的三维重建中实时全局优化和错误回环判断的装置，此处不再赘述。
根据本发明实施例提出的三维重建中实时全局优化和错误回环判断的装置，在不损失精度的前提下实现了高效率的视觉和IMU信息的全局优化，***能够实时运行在可便携设备上；紧耦合的自标定可以得到准确的相机和IMU的变换矩阵，没有因为标定不准确导致的累积误差的问题；根据优化得到的IMU零偏的变化量能够进行准确的回环正确性的判断，简单易实现。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
尽管上面已经示出和描述了本发明的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (8)

1.一种三维重建中实时全局优化和错误回环判断的方法，其特征在于，包括以下步骤：
获取视觉信息和IMU信息，通过所述视觉信息的二阶统计量和所述IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化，其中，通过二阶统计量来计算总的视觉雅各比矩阵
Figure FDA0003512839030000011
的具体方法为：
每个匹配的帧对(Fi,Fj)有多个匹配的特征点对
Figure FDA0003512839030000012
其中，Pi k表示第k个特征点在第i帧观测的坐标，对第i帧来说，Tci是欧氏空间中的刚性变换矩阵，ξi是对应的李代数，Rci、Pci是变换矩阵的旋转和平移部分，Tci＝[Rci|Pci]，在匹配的帧对中找到匹配的特征点对后，对每个匹配的特征点对的误差函数为：
Figure FDA0003512839030000013
通过组合所有的特征点的误差函数，得到视觉部分整体的误差函数：
Figure FDA0003512839030000014
其中是Pi k第i帧相机坐标系下点坐标
Figure FDA0003512839030000015
的齐次坐标，第一帧的相机位姿T0设为单位阵，初始化世界坐标系，N是关键帧的总数，视觉初始化中位姿估计通过最小化误差函数来获得，对于相邻的关键帧Fi和Fj，根据视觉求出位姿Rci、Pci和Rcj、Pcj，
Figure FDA0003512839030000016
对位姿Rci和Pci求导的雅各比矩阵为：
Figure FDA0003512839030000017
一个关键帧i和关键帧j之间的特征点对，有子矩阵为：
Figure FDA0003512839030000018
对于一个关键帧i和关键帧j的帧对，雅各比矩阵
Figure FDA0003512839030000019
是所有匹配特征点的子矩阵的和：
Figure FDA00035128390300000110
Figure FDA00035128390300000111
是总的视觉雅各比矩阵
Figure FDA00035128390300000112
的子矩阵，其关系式为：
Figure FDA00035128390300000113
Figure FDA0003512839030000021
有四个6*6的非零子矩阵，分别写作
Figure FDA0003512839030000022
Figure FDA0003512839030000023
的计算过程为：
Figure FDA0003512839030000024
在全局优化函数中耦合变换矩阵，来提供准确的变换矩阵估计值，并且在全局优化过程中，提供IMU和相机的状态估计量，使得在快速移动和特征点少的时候，通过IMU信息进行定位，从而继续进行建图，其中，第i个关键帧写作Fi,包含RGBD相机的拍摄数据彩色图Ii和深度图Di，对于关键帧Fi,需要优化的状态变量分别是Rci、Pci、VIi、
Figure FDA0003512839030000025
Figure FDA0003512839030000026
表示了相机的旋转方向、平移位置、IMU的速度、IMU的角速度零偏、加速度零偏，RIi、PIi是对应的IMU的旋转和平移，R0、P0是相机到IMU的变换矩阵，g是初始的重力值，Rg是重力的优化方向，在一个整体的全局优化函数中相机的状态量Rci、Pci、VIi、
Figure FDA0003512839030000027
相机IMU变换矩阵R0、P0和重力方向Rg，R0、P0和Rg是全局变量，使用S来表示全局优化中的所有的优化量:S＝{R,P,V,bg,ba,R0,P0,Rg}，其中
Figure FDA0003512839030000028
相邻关键帧之间的IMU数据进行积分得到相邻关键帧之间的运动约束
Figure FDA0003512839030000029
在匹配的关键帧对中，视觉找到的匹配特征点构建视觉约束
Figure FDA00035128390300000210
全部匹配的关键帧对写作Ω，在全局优化过程中，还包括：根据视觉信息的和IMU信息的误差项获取雅各比矩阵；根据雅各比矩阵进行误差函数的最小化，并通过稀疏矩阵乘法得到状态更新量，以实现视觉IMU信息的全局优化，构建误差函数，通过最小化误差函数求出S＝{R,P,V,bg,ba,R0,P0,Rg}的估计值：
Figure FDA00035128390300000211
以及
获取IMU零偏的估计量，并根据所述IMU零偏的估计量识别错误回环和正确回环。
2.根据权利要求1所述的方法，其特征在于，所述获取视觉信息和IMU信息，通过所述视觉信息的二次统计量和所述IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化，包括：
在优化过程中，采用关键帧策略，以使关键帧的状态量参与全局优化，且其他帧的状态量通过最接近的关键帧进行赋值。
3.根据权利要求1所述的方法，其特征在于，所述误差函数为：
Figure FDA0003512839030000031
Figure FDA0003512839030000032
Figure FDA0003512839030000033
Figure FDA0003512839030000034
Figure FDA0003512839030000035
其中，
Figure FDA0003512839030000036
和
Figure FDA0003512839030000037
均为相对位姿变化量，i和j表示帧数，
Figure FDA0003512839030000038
为相机的IMU的角速度零偏，
Figure FDA0003512839030000039
为根据IMU数据在初始零偏值的基础上积分得到的相对位姿变化量，bg为IMU的角速度零偏变量，Rci为相机的旋转方向，R0、P0均为相机到IMU的变换矩阵，VIj为第j帧相机IMU的速度，VIi为第i帧相机IMU的速度，Rg为重力的优化方向，g为初始的重力值，Δtij为第i帧和第j帧之间的时间间隔，
Figure FDA00035128390300000310
为第i帧相机IMU的加速度零偏，Pci为相机的平移位置，ba为IMU的加速度零偏变量。
4.根据权利要求3所述的方法，其特征在于，还包括：
通过李代数的扰动模型求出误差相对于需要优化的状态变量的导数。
5.一种三维重建中实时全局优化和错误回环判断的装置，其特征在于，包括：
构建模块，用于获取视觉信息和IMU信息，通过所述视觉信息的二阶统计量和所述IMU信息的预计分构建优化方程，以进行CPU的实时在线全局优化，其中，通过二阶统计量来计算总的视觉雅各比矩阵
Figure FDA00035128390300000311
的具体方法为：
每个匹配的帧对(Fi,Fj)有多个匹配的特征点对
Figure FDA00035128390300000312
其中，Pi k表示第k个特征点在第i帧观测的坐标，对第i帧来说，Tci是欧氏空间中的刚性变换矩阵，ξi是对应的李代数，Rci、Pci是变换矩阵的旋转和平移部分，Tci＝[Rci|Pci]，在匹配的帧对中找到匹配的特征点对后，对每个匹配的特征点对的误差函数为：
Figure FDA0003512839030000041
通过组合所有的特征点的误差函数，得到视觉部分整体的误差函数：
Figure FDA0003512839030000042
其中是Pi k第i帧相机坐标系下点坐标
Figure FDA0003512839030000043
的齐次坐标，第一帧的相机位姿T0设为单位阵，初始化世界坐标系，N是关键帧的总数，视觉初始化中位姿估计通过最小化误差函数来获得，对于相邻的关键帧Fi和Fj，根据视觉求出位姿Rci、Pci和Rcj、Pcj，
Figure FDA0003512839030000044
对位姿Rci和Pci求导的雅各比矩阵为：
Figure FDA0003512839030000045
一个关键帧i和关键帧j之间的特征点对，有子矩阵为：
Figure FDA0003512839030000046
对于一个关键帧i和关键帧j的帧对，雅各比矩阵
Figure FDA0003512839030000047
是所有匹配特征点的子矩阵的和：
Figure FDA0003512839030000048
Figure FDA0003512839030000049
是总的视觉雅各比矩阵
Figure FDA00035128390300000410
的子矩阵，其关系式为：
Figure FDA00035128390300000411
Figure FDA00035128390300000412
有四个6*6的非零子矩阵，分别写作
Figure FDA00035128390300000413
Figure FDA00035128390300000414
的计算过程为：
Figure FDA00035128390300000415
Figure FDA0003512839030000051
耦合模块，用于在全局优化函数中耦合变换矩阵，来提供准确的变换矩阵估计值，并且在全局优化过程中，提供IMU和相机的状态估计量，使得在快速移动和特征点少的时候，通过IMU信息进行定位，从而继续进行建图，其中，第i个关键帧写作Fi,包含RGBD相机的拍摄数据彩色图Ii和深度图Di，对于关键帧Fi,需要优化的状态变量分别是Rci、Pci、VIi、
Figure FDA0003512839030000052
表示了相机的旋转方向、平移位置、IMU的速度、IMU的角速度零偏、加速度零偏，RIi、PIi是对应的IMU的旋转和平移，R0、P0是相机到IMU的变换矩阵，g是初始的重力值，Rg是重力的优化方向，在一个整体的全局优化函数中相机的状态量Rci、Pci、VIi、
Figure FDA0003512839030000053
相机IMU变换矩阵R0、P0和重力方向Rg，R0、P0和Rg是全局变量，使用S来表示全局优化中的所有的优化量:S＝{R,P,V,bg,ba,R0,P0,Rg}，其中
Figure FDA0003512839030000054
相邻关键帧之间的IMU数据进行积分得到相邻关键帧之间的运动约束
Figure FDA0003512839030000055
在匹配的关键帧对中，视觉找到的匹配特征点构建视觉约束
Figure FDA0003512839030000056
全部匹配的关键帧对写作Ω，在全局优化过程中，还包括：根据视觉信息的和IMU信息的误差项获取雅各比矩阵；根据雅各比矩阵进行误差函数的最小化，并通过稀疏矩阵乘法得到状态更新量，以实现视觉IMU信息的全局优化，构建误差函数，通过最小化误差函数求出S＝{R,P,V,bg,ba,R0,P0,Rg}的估计值：
Figure FDA0003512839030000057
以及
识别模块，用于获取IMU零偏的估计量，并根据所述IMU零偏的估计量识别错误回环和正确回环。
6.根据权利要求5所述的装置，其特征在于，所述构建模块进一步用于在优化过程中，采用关键帧策略，以使关键帧的状态量参与全局优化，且其他帧的状态量通过最接近的关键帧进行赋值。
7.根据权利要求5所述的装置，其特征在于，所述误差函数为：
Figure FDA0003512839030000061
Figure FDA0003512839030000062
Figure FDA0003512839030000063
Figure FDA0003512839030000064
Figure FDA0003512839030000065
其中，
Figure FDA0003512839030000066
和
Figure FDA0003512839030000067
均为相对位姿变化量，i和j表示帧数，
Figure FDA0003512839030000068
为相机的IMU的角速度零偏，
Figure FDA0003512839030000069
为根据IMU数据在初始零偏值的基础上积分得到的相对位姿变化量，bg为IMU的角速度零偏变量，Rci为相机的旋转方向，R0、P0均为相机到IMU的变换矩阵，VIj为第j帧相机IMU的速度，VIi为第i帧相机IMU的速度，Rg为重力的优化方向，g为初始的重力值，Δtij为第i帧和第j帧之间的时间间隔，
Figure FDA00035128390300000610
为第i帧相机IMU的加速度零偏，Pci为相机的平移位置，ba为IMU的加速度零偏变量。
8.根据权利要求7所述的装置，其特征在于，还包括：
求解模块，用于通过李代数的扰动模型求出误差相对于需要优化的状态变量的导数。
CN201910968740.7A 2019-10-12 2019-10-12 三维重建中实时全局优化和错误回环判断的方法及装置 Active CN110986968B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN201910968740.7A CN110986968B (zh) 2019-10-12 2019-10-12 三维重建中实时全局优化和错误回环判断的方法及装置
US17/036,457 US11514607B2 (en) 2019-10-12 2020-09-29 3-dimensional reconstruction method, 3-dimensional reconstruction device, and storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910968740.7A CN110986968B (zh) 2019-10-12 2019-10-12 三维重建中实时全局优化和错误回环判断的方法及装置

Publications (2)

Publication Number Publication Date
CN110986968A CN110986968A (zh) 2020-04-10
CN110986968B true CN110986968B (zh) 2022-05-24

Family

ID=70081865

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910968740.7A Active CN110986968B (zh) 2019-10-12 2019-10-12 三维重建中实时全局优化和错误回环判断的方法及装置

Country Status (2)

Country Link
US (1) US11514607B2 (zh)
CN (1) CN110986968B (zh)

Families Citing this family (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111521187B (zh) * 2020-05-13 2022-04-12 北京百度网讯科技有限公司 自动驾驶定位组合导航方法、装置、设备及存储介质
CN111862316B (zh) * 2020-07-28 2024-01-05 杭州深瞳科技有限公司 一种基于优化的imu紧耦合稠密直接rgbd的三维重建方法
CN112330735B (zh) * 2020-10-26 2022-06-17 武汉中海庭数据技术有限公司 一种车体相对位置测量精度置信度评估方法及***
CN112461228B (zh) * 2020-11-03 2023-05-09 南昌航空大学 一种相似环境下基于imu和视觉的二次回环检测定位方法
CN113644984B (zh) * 2021-10-14 2022-03-11 清华大学 光电数字逻辑运算的光学逻辑元件及其逻辑运算方法
CN115908482B (zh) * 2022-10-14 2023-10-20 荣耀终端有限公司 建模错误数据的定位方法和装置
CN115451958B (zh) * 2022-11-10 2023-02-03 中国人民解放军国防科技大学 基于相对旋转角度的相机绝对姿态优化方法

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9651388B1 (en) * 2007-10-25 2017-05-16 Rockwell Collins, Inc. System and method for improved simultaneous localization and mapping
CN107504969A (zh) * 2017-07-24 2017-12-22 哈尔滨理工大学 基于视觉和惯性组合的四旋翼室内导航方法
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及***
CN108665540A (zh) * 2018-03-16 2018-10-16 浙江工业大学 基于双目视觉特征和imu信息的机器人定位与地图构建***
CN109579840A (zh) * 2018-10-25 2019-04-05 中国科学院上海微***与信息技术研究所 一种点线特征融合的紧耦合双目视觉惯性slam方法
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110030994A (zh) * 2019-03-21 2019-07-19 东南大学 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN110125928A (zh) * 2019-03-27 2019-08-16 浙江工业大学 一种基于前后帧进行特征匹配的双目惯导slam***
CN110207693A (zh) * 2019-05-21 2019-09-06 南京航空航天大学 一种鲁棒立体视觉惯性预积分slam方法
CN110296702A (zh) * 2019-07-30 2019-10-01 清华大学 视觉传感器与惯导紧耦合的位姿估计方法及装置

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9607401B2 (en) * 2013-05-08 2017-03-28 Regents Of The University Of Minnesota Constrained key frame localization and mapping for vision-aided inertial navigation
US9658070B2 (en) * 2014-07-11 2017-05-23 Regents Of The University Of Minnesota Inverse sliding-window filters for vision-aided inertial navigation systems
US10043076B1 (en) * 2016-08-29 2018-08-07 PerceptIn, Inc. Visual-inertial positional awareness for autonomous and non-autonomous tracking
US10032276B1 (en) * 2016-08-29 2018-07-24 PerceptIn, Inc. Visual-inertial positional awareness for autonomous and non-autonomous device
JP2021508393A (ja) * 2018-09-07 2021-03-04 ホアウェイ・テクノロジーズ・カンパニー・リミテッド 同時に位置特定およびマッピングを実行するためのデバイスおよび方法
US20210396522A1 (en) * 2020-06-17 2021-12-23 Microsoft Technology Licensing, Llc Pedestrian dead reckoning using map constraining features

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9651388B1 (en) * 2007-10-25 2017-05-16 Rockwell Collins, Inc. System and method for improved simultaneous localization and mapping
CN107504969A (zh) * 2017-07-24 2017-12-22 哈尔滨理工大学 基于视觉和惯性组合的四旋翼室内导航方法
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及***
CN108665540A (zh) * 2018-03-16 2018-10-16 浙江工业大学 基于双目视觉特征和imu信息的机器人定位与地图构建***
CN109579840A (zh) * 2018-10-25 2019-04-05 中国科学院上海微***与信息技术研究所 一种点线特征融合的紧耦合双目视觉惯性slam方法
CN110030994A (zh) * 2019-03-21 2019-07-19 东南大学 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN110125928A (zh) * 2019-03-27 2019-08-16 浙江工业大学 一种基于前后帧进行特征匹配的双目惯导slam***
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN110207693A (zh) * 2019-05-21 2019-09-06 南京航空航天大学 一种鲁棒立体视觉惯性预积分slam方法
CN110296702A (zh) * 2019-07-30 2019-10-01 清华大学 视觉传感器与惯导紧耦合的位姿估计方法及装置

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
Real-time Indoor Scene Reconstruction with RGBD and Inertial Input;Zunjie Zhu等;《IEEE international conference on multimedia and Expo(ICME)》;20190805;第7-12页 *
Visual-Inertial Monocular SLAM With Map Reuse;Mur-Artal等;《IEEE Robotics and Automation Letters》;20170116;第2卷(第2期);第796-803页 *
基于关键帧的视觉惯性SLAM闭环检测算法;张玉龙等;《计算机科学与探索》;20180411;第12卷(第11期);第1777-1787页 *
视觉惯性SLAM综述;孙永全等;《计算机应用研究》;20190114;第36卷(第12期);第3530-3533、3552页 *

Also Published As

Publication number Publication date
US11514607B2 (en) 2022-11-29
US20210110576A1 (en) 2021-04-15
CN110986968A (zh) 2020-04-10

Similar Documents

Publication Publication Date Title
CN110986968B (zh) 三维重建中实时全局优化和错误回环判断的方法及装置
CN110030994B (zh) 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
Kaiser et al. Simultaneous state initialization and gyroscope bias calibration in visual inertial aided navigation
CN112649016B (zh) 一种基于点线初始化的视觉惯性里程计方法
CN111795686B (zh) 一种移动机器人定位与建图的方法
CN108981693B (zh) 基于单目相机的vio快速联合初始化方法
CN110702107A (zh) 一种单目视觉惯性组合的定位导航方法
CN110455309B (zh) 具备在线时间校准的基于msckf的视觉惯性里程计
US20210183100A1 (en) Data processing method and apparatus
CN113516692B (zh) 一种多传感器融合的slam方法和装置
Zhang et al. Vision-aided localization for ground robots
CN116342661A (zh) 利用路标点偏置位姿修正的双目视觉惯性里程计方法
CN113674412B (zh) 基于位姿融合优化的室内地图构建方法、***及存储介质
Yu et al. Vision-aided inertial navigation with line features and a rolling-shutter camera
WO2023087681A1 (zh) 定位初始化方法、装置、计算机可读存储介质和计算机程序产品
White et al. An iterative pose estimation algorithm based on epipolar geometry with application to multi-target tracking
CN116929338A (zh) 地图构建方法、设备及存储介质
Dinc et al. Mirage: an O (n) time analytical solution to 3D camera pose estimation with multi-camera support
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及***
CN114993338B (zh) 基于多段独立地图序列的一致性高效视觉惯性里程计算法
KR102406240B1 (ko) 강인한 스테레오 비주얼 관성 항법 장치 및 방법
CN111553933B (zh) 一种应用于不动产测量基于优化的视觉惯性组合测量方法
CN115014346A (zh) 一种面向视觉惯性定位的基于地图的一致高效滤波算法
CN114061592A (zh) 基于多模型的自适应鲁棒auv导航方法
Sun et al. Research on combination positioning based on natural features and gyroscopes for AR on mobile phones

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant