CN115388882A - 用于深度辅助视觉惯性测距的方法和装置 - Google Patents

用于深度辅助视觉惯性测距的方法和装置 Download PDF

Info

Publication number
CN115388882A
CN115388882A CN202210569696.4A CN202210569696A CN115388882A CN 115388882 A CN115388882 A CN 115388882A CN 202210569696 A CN202210569696 A CN 202210569696A CN 115388882 A CN115388882 A CN 115388882A
Authority
CN
China
Prior art keywords
depth
imu
feature
residual
anchor frame
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.)
Pending
Application number
CN202210569696.4A
Other languages
English (en)
Inventor
A.蒂亚吉
裵东运
王双全
梁扬文
V.塞图拉曼
X.李
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.)
Samsung Electronics Co Ltd
Original Assignee
Samsung Electronics Co Ltd
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 Samsung Electronics Co Ltd filed Critical Samsung Electronics Co Ltd
Publication of CN115388882A publication Critical patent/CN115388882A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • 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/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • 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/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • 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
    • 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
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • 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)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Multimedia (AREA)
  • Image Processing (AREA)
  • Navigation (AREA)
  • Image Analysis (AREA)

Abstract

提供了一种用于执行视觉惯性测距(VIO)的方法和装置。处理来自惯性测量单元(IMU)、相机和深度传感器的测量。基于经处理的测量来确定至少包括深度残差的关键帧残差。基于从关键帧残差导出的因子来生成和优化滑动窗口图。基于经优化的滑动窗口图来估计对象姿态。

Description

用于深度辅助视觉惯性测距的方法和装置
相关申请的交叉引用
本申请基于并要求于2021年5月24日向美国专利和商标局提交并被分配序号63/192,488的美国临时专利申请的优先权，其内容通过引用结合于此。
技术领域
本公开总体上涉及视觉惯性测距系统(visual inertial odometry，VIO)，并且更具体地，涉及将深度测量结合到视觉和惯性测量的视觉惯性测距系统。
背景技术
在现代智能手机中，可以用于增强现实(AR)或三维重建(3DR)应用中定位的传感器数量不断增加。同步定位和测绘(simultaneous localization and mapping，SLAM)系统的类型包括仅视觉SLAM系统和惯性测量单元(inertial measurement unit，IMU)辅助视觉SLAM系统，其被称为VIO系统。使用相机和深度传感器两者来估计相机轨迹的仅视觉SLAM系统包括面向FAST(来自加速分段测试的特征)和旋转的BRIEF(二进制稳健独立基础特征)(oriented FAST and rotated BRIEF，ORB)SLAM版本2(SLAM2)和红绿蓝深度(RGBD)SLAM。这两个系统使用深度测量来改进由基于二维(2D)特征的仅视觉SLAM系统执行的姿态估计。
VIO系统用于使用来自相机图像的数据(例如，视觉数据)和IMU来跟踪对象姿态的六个自由度(six degrees of freedom，6DOF)。VIO系统使用独特的特征(其在相机图像、加速度计数据和陀螺仪数据中被跟踪)作为通过使用数学优化技术的紧密耦合系统来估计对象姿态的测量。
发明内容
根据一个实施例，提供了一种用于在用户设备(UE)处执行VIO的方法。处理来自UE的IMU、相机和深度传感器的测量。基于经处理的测量来确定至少包括深度残差的关键帧残差。基于从关键帧残差导出的因子来生成和优化滑动窗口图。基于经优化的滑动窗口图来估计UE的对象姿态。
根据一个实施例，提供了一种UE，其包括IMU、相机、深度传感器、处理器和存储指令的非暂时性计算机可读存储介质。当被执行时，指令使得处理器处理来自IMU、相机和深度传感器的测量。指令还使得处理器基于经处理的测量来确定至少包括深度残差的关键帧残差，并且基于从关键帧残差中导出的因子来生成和优化滑动窗口图。指令还使得处理器基于经优化的滑动窗口图来估计UE的对象姿态。
附图说明
当结合附图时，根据以下详细描述，本公开的某些实施例的上述和其他方面、特征和优点将变得更加清楚，其中：
图1是示出单目视觉惯性导航系统(monocular visual inertial navigationsystem，VINS-Mono)的因子图的图；
图2是示出根据实施例的MSL深度辅助VIO(MSL-depth-aided VIO，DVIO)的因子图的图；
图3是示出根据实施例的飞行时间(time of flight，TOF)相机的图；
图4是示出根据实施例的结构化光传感器的图；
图5是示出根据实施例的MSL-DVIO的系统流程的图；
图6是示出了根据本公开的实施例的MSL-DVIO的详细系统流程的图；
图7是示出根据实施例的MSL-DVIO 1D特征实现的图；
图8是示出根据实施例的MSL-DVIO用于3D特征参数化的实现细节的图；
图9是示出根据实施例的用于在UE处执行VIO的方法的流程图；并且
图10是根据实施例的网络环境中的电子设备的框图。
具体实施方式
在下文中，参考附图详细描述了本公开的实施例。应当注意，尽管在不同的附图中示出了相同的元素，但是相同的元素将由相同的附图标记表示。在以下描述中，仅提供诸如详细配置和组件的具体细节来帮助对本公开的实施例的整体理解。因此，对于本领域技术人员来说清楚的是，在不脱离本公开的范围的情况下，可以对本文描述的实施例进行各种改变和修改。此外，为了清楚和简洁，省略了对公知功能和结构的描述。下面描述的术语是考虑到本公开中的功能而定义的术语，并且可以根据用户、用户的意图或习惯而不同。因此，术语的定义应该基于整个说明书的内容来确定。
本公开可以具有各种修改和各种实施例，下面参考附图详细描述其中的实施例。然而，应当理解，本公开不限于这些实施例，而是包括本公开范围内的所有修改、等同和替代。
尽管包括序数(诸如第一、第二等)的术语可以用于描述各种元件，但是结构元件不受这些术语的限制。这些术语仅用于区分一个元件和另一个元件。例如，在不脱离本公开的范围的情况下，第一结构元件可以被称为第二结构元件。类似地，第二结构元件也可以被称为第一结构元件。如本文所用，术语“和/或”包括一个或多个关联项目的任何和所有组合。
本文使用的术语仅用于描述本公开的各种实施例，而不旨在限制本公开。单数形式旨在包括复数形式，除非上下文另有明确指示。在本公开中，应该理解，术语“包括”或“具有”指示特征、数字、步骤、操作、结构元件、组件或其组合的存在，并且不排除一个或多个其他特征、数字、步骤、操作、结构元件、组件或其组合的存在或添加的可能性。
除非被不同地定义，否则本文使用的所有术语都具有与本公开所属领域的技术人员所理解的相同的含义。诸如在通用词典中定义的那些术语将被解释为具有与相关技术领域中的上下文含义相同的含义，并且除非在本公开中明确定义，否则不应被解释为具有理想的或过于正式的含义。
电子设备可以是各种类型的电子设备之一。电子设备可以包括例如便携式通信设备(例如，智能手机)、计算机、便携式多媒体设备、便携式医疗设备、相机、可穿戴设备或家用电器。根据本公开的一个实施例，电子设备不限于上述那些。
本公开中使用的术语不旨在限制本公开，而是旨在包括对应的实施例的各种改变、等同或替换。关于附图的描述，相似的附图标记可以用于指代相似或相关的元素。与项目相对应的单数形式名词可以包括事物中的一个或多个，除非相关上下文另有清楚地指示。如本文所使用的，诸如“A或B”、“A和B中的至少一个”、“A或B中的至少一个”、“A、B或C”、“A、B和C中的至少一个”以及“A、B或C中的至少一个”等短语中的每一个都可以包括在对应的一个短语中一起列举的项目的所有可能的组合。如本文所使用的，诸如“第一”、“第二”、“第一个”和“第二个”的术语可以用于将对应的组件与另一个组件区分开，但不旨在在其他方面(例如，重要性或次序上)限制组件。意图是，如果一个元件(例如，第一元件)被称为“与另一个元件(例如，第二元件)耦合”、“耦合到另一个元件”、“与另一个元件连接”或“连接到另一个元件”，无论有无术语“可操作地”或“通信地”，其指示该元件可以直接地(例如，有线地)、无线地或经由第三元件与另一个元件耦合。
如本文所使用的，术语“模块”可以包括以硬件、软件或固件实现的单元，并且可以与其他术语(例如，“逻辑”、“逻辑块”、“组件”和“电路”)互换使用。模块可以是适于执行一个或多个功能的单个集成组件、或者是其最小单元或部分。例如，根据一个实施例，模块可以以专用集成电路(ASIC)的形式来实现。
图1是示出VINS-Mono的因子图的图。xk表示机器人运动的完整状态。
状态变量可以写成下面的等式(1)，如表1所定义的。
Figure BDA0003658645940000041
机器人姿态状态被定义为
Figure BDA0003658645940000042
并且机器人速率(speed)和偏差状态被定义为
Figure BDA0003658645940000043
表1
Figure BDA0003658645940000044
Figure BDA0003658645940000051
对于图1的整个VINS-Mono，用于状态估计的基于滑动窗口的紧密耦合VIO包括n+1个机器人状态xnav和m+1个3D界标逆深度
Figure BDA0003658645940000052
Figure BDA0003658645940000053
定义滑动窗口内的状态向量，如在下面的等式(2)中给出。
X＝[xnav；xfeat] ……(2)
其中，
Figure BDA0003658645940000054
且
Figure BDA0003658645940000055
指示第k关键帧的姿态、速率和偏差，并且xfeat＝[λ0…λm]，指示在第一关键帧中观察到的界标逆深度。界标测量锚定到其的该第一关键帧被称为锚帧。
在VINS-Mono中，寻求下面等式(3)的解。
Figure BDA0003658645940000056
这里，Bw是滑动窗口的索引，
Figure BDA0003658645940000057
是滑动窗口内的一组观察(observation)，
Figure BDA0003658645940000058
是待优化的一组状态变量，rIMU(xk,xk+1)是IMU因子，
Figure BDA0003658645940000059
是视觉因子，rM(Xr)是边缘化因子，Xr是边缘化后的完整状态，
Figure BDA00036586459400000510
是IMU部分生成残差的马氏距离(Mahalanobis distance)的协方差矩阵，以及Q是视觉部分生成残差的马氏距离的协方差矩阵。
因此，图1示出了因子
Figure BDA00036586459400000511
102、
Figure BDA00036586459400000512
104、
Figure BDA00036586459400000513
106、rIMU(x1,x2)108、rIMU(x2,x3)110、
Figure BDA00036586459400000514
112、
Figure BDA00036586459400000515
114、
Figure BDA00036586459400000516
116、
Figure BDA00036586459400000517
118、
Figure BDA00036586459400000518
和
Figure BDA00036586459400000519
120、λj122和λi124之间的关系。
因为MSL-DVIO中的实施例影响上述因子图方法中的视觉因子，所以在下面详细描述视觉因子。将在下文中详细描述IMU因子。
在作为RGBD+IMU系统的MSL-DVIO中，根据相机图像和深度图的特征测量生成视觉因子。MSL-DVIO的IMU因子与VINS-Mono的因子相同。与VINS-Mono不同，对于MSL-DVIO，深度图与相机捕获一起可用。
图2是示出根据实施例的MSL-DVIO的因子图的图。具体地，图2示出了因子
Figure BDA00036586459400000520
202、
Figure BDA00036586459400000521
204、
Figure BDA00036586459400000522
206、
Figure BDA00036586459400000523
208、rIMU(x1,x2)210、rIMU(x2,x3)212、rIMU(x3,x4)214、
Figure BDA0003658645940000061
216、
Figure BDA0003658645940000062
218、
Figure BDA0003658645940000063
220、
Figure BDA0003658645940000064
222、
Figure BDA0003658645940000065
224、
Figure BDA0003658645940000066
和
Figure BDA0003658645940000067
226、
Figure BDA0003658645940000068
228、
Figure BDA0003658645940000069
230、
Figure BDA00036586459400000610
232、
Figure BDA00036586459400000611
234、λj236、λi238和λm240之间的关系。rDP是深度残差。
术语“残差”一般定义状态参数的估计值和来自传感器的测量值之间的计算损失。
在MSL-DVIO中采用了来自VINS-Mono的图像特征检测和跟踪器。对于每个检测到的特征，从深度图中提取对应的深度，并且形成深度残差因子。由于深度图的性质，可能发生遮挡和超范围，并且特征点的对应的深度可能不总是可用的。因此，深度因子的数量小于或等于图像特征因子的数量，如图2所示。
对于RGBD传感器，真实世界3D界标被投影到图像平面上，以生成这些界标在RGBD传感器上的2D位置以及这些界标在深度传感器上的深度。下面的等式(4)给出了相机测量模型。
Figure BDA00036586459400000612
下面的等式(5)给出了深度测量模型。
Figure BDA00036586459400000613
在本文中，
Figure BDA00036586459400000614
是界标在图像帧中的2D位置，
Figure BDA00036586459400000615
是界标在相机参考帧中的深度，K是相机固有矩阵。[R|t]是相机在世界参考帧中的姿态矩阵，
Figure BDA00036586459400000616
是界标l在世界参考帧中的3D位置，以及nfeat和ndepth是测量噪声，其被假设为高斯噪声。
对于给定的界标l，帧i被设置为观察界标l的第一帧。帧i被称为界标l的锚帧。
Figure BDA00036586459400000617
被设置为界标l在帧i中的特征测量，并且
Figure BDA00036586459400000618
被设置为界标l在未来帧j中的特征测量。
由于MSL-DVIO利用经校准的相机，因此特征测量被存储在归一化平面而不是图像平面中。为了计算该测量，使用逆相机投影矩阵将特征点从图像平面提升到归一化平面，如在下面的等式(6)中给出。
Figure BDA0003658645940000071
然后，通过将失真模型递归地应用于归一化特征测量来处理镜头失真。存储界标在相机帧中的最终特征测量，如在下面的等式(7)中给出。
Figure BDA0003658645940000072
由于使用特征在锚帧中的逆深度来估计界标，因此MSL-DVIO的这种实现方式被称为MSL-DVIO 1D。
使用帧i和j中的特征测量，计算特征在非锚帧j中的图像残差，如在下面的等式(8)中给出。
Figure BDA0003658645940000073
这里，
Figure BDA0003658645940000074
是从相机坐标到IMU坐标的变换且
Figure BDA0003658645940000075
是帧j的机器人姿态，并且
Figure BDA0003658645940000076
Figure BDA0003658645940000077
是帧i的机器人姿态，
Figure BDA0003658645940000078
类似地，在深度图中，假设相机和深度图在时间和空间上对齐，测量界标l的深度场(depth field)。界标在锚帧中的估计深度可以用于估计界标在帧j中的深度。因此，在下面的等式(9)中给出深度残差。
Figure BDA0003658645940000079
这里，
Figure BDA0003658645940000081
是界标在相机参考帧中的逆深度测量，其由
Figure BDA0003658645940000082
给出。为了简单起见，可以假设逆深度测量的测量噪声也是高斯噪声。
使用深度残差和图像残差，通过将图像残差和深度残差堆叠在一起来创建非锚帧的视觉因子残差，如在下面的等式(10)中给出。
Figure BDA0003658645940000083
然后，在图优化(graph optimization)中使用该残差，以使用非线性优化来求解机器人状态。在执行优化时，相对于状态变量生成残差的雅可比矩阵。为了计算非锚帧视觉残差的雅可比矩阵，相机域中的界标观察被定义为如在下面的等式(11)中给出。
Figure BDA0003658645940000084
特征的RGBD测量由
Figure BDA0003658645940000085
给出，得到下面的等式(12)。
Figure BDA0003658645940000086
视觉残差的雅可比矩阵可以计算如下。
视觉残差相对于锚帧i的世界位置的雅可比矩阵(3×3矩阵)在下面的等式(13)中给出。
Figure BDA0003658645940000087
Figure BDA0003658645940000091
视觉残差相对于锚帧i的旋转(rotation)的雅可比矩阵(3×3矩阵)在下面的等式(14)中给出。
Figure BDA0003658645940000092
视觉残差相对于帧j的世界位置的雅可比矩阵(3×3矩阵)在下面的等式(15)中给出。
Figure BDA0003658645940000093
视觉残差相对于帧j的旋转的雅可比矩阵(3×3矩阵)在下面的等式(16)中给出。
Figure BDA0003658645940000094
视觉残差相对于特征l的逆深度的雅可比矩阵(3×1矩阵)在下面的等式(17)中给出。
Figure BDA0003658645940000095
在上述场景中，使用锚关键帧i中的原始测量，在图中使用深度测量，并且该测量被传播到关键帧j以计算特征和深度测量的残差。此外，可以为锚关键帧中的深度测量添加块，以对深度更新提供更强的约束。
在这种情况下，所使用的测量模型仅是深度测量，如在下面的等式(18)中给出。
Figure BDA0003658645940000096
因为不对2D特征测量进行更新，所以特征测量的残差为零，并且用于添加参数的残差被减少为仅来自深度测量的残差，如在下面的等式(19)中给出。
Figure BDA0003658645940000101
这将使相对于特征l的逆深度的雅可比矩阵(3×1矩阵)如在下面的等式(20)中给出。
Figure BDA0003658645940000102
包括RGBD的多个传感器和IMU传感器是不同步的。在这种情况下，相机和IMU时间戳之间有存在小的时间偏移。这样的时间偏移通常被认为是恒定但却未知的。为了在线估计时间偏移，该时间偏移被建模为如在下面的等式(21)中给出。
tIMU＝tCAM+Δt ……(21)
这里，tIMU是IMU时钟时间，tCAM是相机时钟时间，并且Δt是两个时钟之间的时间偏移。Δt被建模为随机游走，并且使用特征和深度测量来优化。
根据使用MSL-DVIO的实施例，利用任何给定时间的时间偏移，补偿相机时间戳以使其成为IMU时间戳。为了计算未来任何给定时间处的特征测量，可以使用如下面的等式(22)给出的恒定速度运动模型来传播特征测量。
Figure BDA0003658645940000103
其中，
Figure BDA0003658645940000104
是界标l在帧i中经时间偏移补偿的2D测量，
Figure BDA0003658645940000105
是界标l在帧i中的原始2D测量，Δt是时间偏移的当前估计，Δti是在处理了相机帧i时对时间偏移的估计，并且
Figure BDA0003658645940000106
是在特征跟踪期间计算出的特征的2D速度。
TRi是由滚动快门相机引起的时间偏移，其在下面的等式(23)中给出。
Figure BDA0003658645940000107
其中，TR是滚动快门读出时间，ROW是帧中行的总数，OWi是其中观察到特征的行。
类似地，深度的特征测量在下面的等式(24)中给出。
Figure BDA0003658645940000111
Figure BDA0003658645940000112
是特征深度在深度图中的改变率，如在下面的等式(25)中给出。
Figure BDA0003658645940000113
使用这种新的测量，如在下面的等式(26)、(27)和(28)中给出的来计算残差。
Figure BDA0003658645940000114
Figure BDA0003658645940000115
Figure BDA0003658645940000116
类似于上文所述，相机域中的界标观察被定义为如在下面的等式(29)中给出。
Figure BDA0003658645940000117
特征的RGBD测量由
Figure BDA0003658645940000118
给出。
利用新的残差，视觉残差的雅可比矩阵可以计算如下。
视觉残差相对于锚帧i的世界位置的雅可比矩阵(3×3矩阵)在下面的等式(30)中给出。
Figure BDA0003658645940000119
Figure BDA0003658645940000121
视觉残差相对于锚帧i的旋转的雅可比矩阵(3×3矩阵)在下面的等式(31)中给出。
Figure BDA0003658645940000122
视觉残差相对于帧j的世界位置的雅可比矩阵(3×3矩阵)在下面的等式(32)中给出。
Figure BDA0003658645940000123
视觉残差相对于帧j的旋转的雅可比矩阵(3×3矩阵)在下面的等式(33)中给出。
Figure BDA0003658645940000124
视觉残差相对于特征l的逆深度的雅可比矩阵(3×1矩阵)在下面的等式(34)中给出。
Figure BDA0003658645940000125
视觉残差相对于时间偏移的雅可比矩阵(3×1矩阵)在下面的等式(35)中给出。
Figure BDA0003658645940000126
视觉残差相对于锚帧中的逆深度的雅可比矩阵(3×1矩阵)在下面的等式(36)中给出。
Figure BDA0003658645940000127
在VINS-Mono中，使用锚关键帧中的逆深度对特征进行参数化。因此，锚帧中的特征测量不能用作执行图优化的残差。为了使锚帧中的特征测量能够用作图优化问题中的残差，将特征在锚中的2D位置添加为特征状态的一部分，从而将特征状态参数化转换为3D的。滑动窗口的这种新的状态在下面的等式(37)中给出。
X＝[xnav；xfeat] ……(37)
其中，
Figure BDA0003658645940000131
且
Figure BDA0003658645940000132
指示第k关键帧的姿态、速率和偏移，xfeat＝[f0…fm]，f1＝[u1,v1,λ1]，(u1,v1)是特征在锚帧中的2D位置的估计，并且λ1是特征在锚帧中的逆深度的估计。
由于使用特征在锚帧中的完整3D位置来估计界标，因此MSL-DVIO的这种实现方式被称为MSL-DVIO 3D。
利用上述3D特征参数化，可以计算锚帧和非锚帧两者的视觉残差。
对于非锚帧，3D特征在锚帧中的估计用于在非锚帧中投影以计算残差，如在下面的等式(38)、(39)和(40)中给出。
Figure BDA0003658645940000133
Figure BDA0003658645940000134
Figure BDA0003658645940000135
类似地，使用特征在锚帧中的3D估计来计算锚帧的视觉残差，如在下面的等式(41)、(42)和(43)中给出。
Figure BDA0003658645940000136
Figure BDA0003658645940000137
Figure BDA0003658645940000138
Figure BDA0003658645940000141
类似于上文所述，通过使用来自状态的特征的2D估计值来计算3D参数化的雅可比矩阵，如在下面的等式(44)中给出。
Figure BDA0003658645940000142
特征的RGBD测量由
Figure BDA0003658645940000143
给出，得到下面的等式(45)。
Figure BDA0003658645940000144
视觉残差相对于锚帧i的世界位置的雅可比矩阵(3×3矩阵)在下面的等式(46)中给出。
Figure BDA0003658645940000145
视觉残差相对于锚帧i的旋转的雅可比矩阵(3×3矩阵)在下面的等式(47)中给出。
Figure BDA0003658645940000146
视觉残差相对于帧j的世界位置的雅可比矩阵(3×3矩阵)在下面的等式(48)中给出。
Figure BDA0003658645940000151
视觉残差相对于帧j的旋转的雅可比矩阵(3×3矩阵)在下面的等式(49)中给出。
Figure BDA0003658645940000152
通过对状态中的特征进行3D参数化，残差相对于特征的雅可比矩阵将改变为3×3矩阵，如下所述。
非锚帧的雅可比矩阵在下面的等式(50)、(51)和(52)中给出。
Figure BDA0003658645940000153
Figure BDA0003658645940000154
Figure BDA0003658645940000155
锚帧的雅可比矩阵在下面的等式(53)、(54)和(55)中给出。
Figure BDA0003658645940000156
Figure BDA0003658645940000157
Figure BDA0003658645940000158
如上所述，3D特征参数化将完整的3D特征测量添加到锚帧中作为用于图优化的锚帧残差的一部分。
类似于MSL-DVIO 1D的时间偏移估计过程，可以计算特征的3D位置的经时间偏移校正的测量。除了使用等式(56)的估计值之外，还可以使用类似于MSL-DVIO 1D的时间偏移估计来估计特征的2D位置。
Figure BDA0003658645940000161
类似地，对于深度测量，经时间偏移校正的测量可以如MSL-DVIO 1D中所述来计算，如在下面的等式(57)中给出。
Figure BDA0003658645940000162
使用新的测量，可以计算非锚帧的残差，如在下面的等式(58)、(59)和(60)中给出。
Figure BDA0003658645940000163
Figure BDA0003658645940000164
Figure BDA0003658645940000165
类似地，对于锚帧，残差在下面的等式(61)、(62)和(63)中给出。
Figure BDA0003658645940000166
Figure BDA0003658645940000167
Figure BDA0003658645940000168
该系统的雅可比矩阵计算如下。
视觉残差相对于锚帧i的世界位置的雅可比矩阵(3×3矩阵)在下面的等式(64)中给出。
Figure BDA0003658645940000171
视觉残差相对于锚帧i的旋转的雅可比矩阵(3×3矩阵)在下面的等式(65)中给出。
Figure BDA0003658645940000172
视觉残差相对于帧j的世界位置的雅可比矩阵(3×3矩阵)在下面的等式(66)中给出。
Figure BDA0003658645940000173
视觉残差相对于帧j的旋转的雅可比矩阵(3×3矩阵)在下面的等式(67)中给出。
Figure BDA0003658645940000174
通过对状态中的特征进行3D参数化，残差相对于特征的雅可比矩阵将改变为3×3矩阵，如下所述。
非锚帧的雅可比矩阵在下面的等式(68)、(69)和(70)中给出。
Figure BDA0003658645940000175
Figure BDA0003658645940000176
Figure BDA0003658645940000177
锚帧的雅可比矩阵在下面的等式(71)、(72)和(73)中给出。
Figure BDA0003658645940000181
Figure BDA0003658645940000182
Figure BDA0003658645940000183
视觉残差相对于时间偏移的雅可比矩阵(3×1矩阵)在下面的等式(74)中给出。
Figure BDA0003658645940000184
在表2和表3中，为上述所有组合提供了MSL-DVIO的系统残差和雅可比矩阵比较。
表2-无时间偏移的MSL-DVIO残差和雅可比矩阵
Figure BDA0003658645940000185
Figure BDA0003658645940000191
表3-有时间偏移的MSL-DVIO残差和雅可比矩阵
Figure BDA0003658645940000192
Figure BDA0003658645940000201
上面描述了在MSL-DVIO中图优化的改变。提供了与1D和3D特征参数化相关的实施例。为了估计特征参数的初始值，估计过程被分成两个单独的块。第一块估计特征的逆深度估计，并且第二块估计2D特征测量在锚帧中的初始值。
相对于锚帧计算特征深度。在MSL-DVIO中，使用滑动窗口测量并且使用来自深度图的特征深度测量来计算特征深度的初始估计。
在估计初始深度估计的第一步骤中，来自深度图的特征点的深度测量用于计算特征深度。Kl＝{k1,k2,…,kn}是单个界标l在完整滑动窗口中的3D特征测量。这里，
Figure BDA0003658645940000211
对应于界标l在锚帧中的特征测量，并且k2,…,n对应于界标l在非锚帧中的特征测量。
Figure BDA0003658645940000212
是针对其运行了图优化的滑动窗口的机器人姿态。使用界标l的滑动窗口姿态和特征测量，将3D点投影到锚帧，如在下面的等式(75)中给出。
对于滑动窗口中的所有j帧，
Figure BDA0003658645940000213
锚帧3D特征测量估计被生成为
Figure BDA0003658645940000214
使用2D估计
Figure BDA0003658645940000215
生成特征在锚帧上的重新投影误差。对该重新投影误差应用阈值，以将它们分类为内点(inlier)和外点(outlier)。使用所有内点估计的锚帧深度的平均来初始化特征深度。
为了仅使用滑动窗口数据和特征2D位置来估计锚帧中的深度和特征2D位置估计，使用直接线性变换(direct linear transform，DLT)算法。该问题被公式化为形式AX＝0，并且使用奇异值分解来求解。例如，界标
Figure BDA0003658645940000216
Figure BDA0003658645940000217
可以是在滑动窗口的相机锚帧
Figure BDA0003658645940000218
中观察到的3D点，并且可以具有为Kl＝{k1,k2,…,kn}的特征测量。
Figure BDA0003658645940000219
是锚帧，并且k1是锚帧中的特征测量。使用相机投影矩阵，提供等式(76)的关系。
Figure BDA00036586459400002110
其中j是滑动窗口中的每一个关键帧，
Figure BDA00036586459400002111
是关键帧和锚帧之间使用
Figure BDA00036586459400002112
和
Figure BDA00036586459400002113
中的旋转向量和平移向量的相对变换矩阵，是比例因子。
在相同方向上的两个向量的叉乘为零，由关系等式(77)提供。
Figure BDA00036586459400002114
Figure BDA0003658645940000221
在等式(77)中，
Figure BDA0003658645940000222
被写为其3×4矩阵的扩展形式，并且符号说明(notation)被简化，如在下面的等式(78)中给出。
Figure BDA0003658645940000223
其中
Figure BDA0003658645940000224
Figure BDA0003658645940000225
且
Figure BDA0003658645940000226
叉乘项在下面的等式(79)中给出。
Figure BDA0003658645940000227
等式(79)可以进一步被简化，因为第三行是第一行和第二行的线性组合，所以得到如下给出的单次观察的等式(80)。
Figure BDA0003658645940000228
等式(80)可以写成下面的等式(81)。
AjL＝0 ……(81)
对每个特征的观察进行叠加，并创建形式为AL＝0的一个线性方程组。为了求解该齐次线性组，使用奇异值分解来求解对代数最小平方误差进行最小化，其中解将是与ATA的最小特征值相对应的特征向量，如在下面的等式(82)和(83)中给出。
U∑V＝SVD(A) ……(82)
Figure BDA0003658645940000231
生成界标在锚帧中的2D测量和深度的估计，如在下面的等式(84)中给出。
Figure BDA0003658645940000232
然后，这些估计被用于初始化图优化问题中的状态变量。在上述方法中，特征参数的初始值取决于特征测量和滑动窗口姿态。在深度估计的情况下，所有的深度都用内点深度测量的平均值来初始化，并且只有未使用第一步骤估计的特征才使用第二方法来初始化。在2D特征参数初始化的情况下，只有第二方法用于初始化。
当滑动窗口姿态中存在较大误差时，上述方法容易受到不良初始化的影响。由于估计取决于滑动窗口姿态的质量，因此这个问题在作为初始化步骤的一部分的第一次优化运行中更为明显。为了避免MSL-DVIO系统的不良行为，向优化步骤添加修复，这仅针对作为初始化步骤的一部分的第一次优化运行。
为了执行深度辅助，利用了深度图帧。这些深度图帧是使用深度感测模块生成的。存在两种类型的深度感测模块。第一种类型是被动深度传感器，并且第二种类型是主动深度传感器。被动深度传感器的示例包括来自立体相机对的深度，并且主动深度传感器的示例包括TOF相机或结构化光相机。
所有传感器都基于测量模型来估计场景深度。因为实施例集中于RGBD传感器，所以本文集中于主动传感器。
图3是示出根据实施例的TOF相机的图。TOF相机包括控制器302、传感器304和红外(IR)发射器306。发送信号310和来自对象314的接收信号312之间的相位差308用于估计深度。
图4是示出根据实施例的结构化光传感器的图。使用基线402和焦距404的点图案用于计算投影仪406和相机408之间的视差(disparity)测量，然后该视差测量用于估计点412的深度410。
如上所述，在测量模型的两个简化示例中，来自传感器的深度测量对于所使用的传感器类型是非常特定的。为了将该深度测量与2D特征测量一起用于执行图优化，测量噪声需要基于每个传感器的模型。
当在MSL-DVIO中执行深度辅助时，假设加性高斯模型用于逆深度测量的测量噪声。这种类型的模型对于不同类型的传感器可能不准确。例如，可以使用具有主动IR立体传感器的传感器。这种传感器使用IR和可见光两者来基于它可以看到的特征执行密集立体匹配。由于传感器使用视差作为用于估计特征点的深度的测量，因此对于深度不确定性来说，使用加性高斯噪声用于逆深度是可接受的模型。
在图优化问题中，为了保持优化问题的固定大小，使用滑动窗口来执行图优化。利用这种方法，在非线性优化步骤中保持了固定大小和较短处理时间。一旦滑动窗口被允许维持问题的固定大小的最大数量的节点填满，执行边缘化，其中节点Xm的某个子集被丢弃，并且形成具有剩余节点Xr
Figure BDA0003658645940000241
Figure BDA0003658645940000242
的图。
在批优化(batch optimization)中被最小化的成本函数具有下面等式(85)中所示的形式。
Figure BDA0003658645940000243
这里，
Figure BDA0003658645940000244
是X中包含所有约束的长度L的列向量，并且Ω是作为每个约束的组合权重的L×L对角矩阵。
边缘化后，批优化仅针对剩余变量r。经修正成本函数表示为F(Xr)。边缘化的目的是将提供Xr的相同最优选择的这样的经修正成本函数F(Xr)确定为C(X)。
这对F(Xr)提出了以下要求：找到满足下面的等式(86)的F(Xr)。
Figure BDA0003658645940000245
然后，边缘化的过程可以被概括为以下几个步骤。
给定信息矩阵H和误差向量
Figure BDA0003658645940000246
如在下面的等式(87)中所示：
Figure BDA0003658645940000247
边缘化过程将计算新的信息矩阵和误差向量，如在下面的等式(88)中所示。
Figure BDA0003658645940000251
Figure BDA0003658645940000252
为了执行边缘化过程，需要对与边缘化状态相关的信息矩阵Hmm求逆。取决于边缘化状态的规模，这可能会非常耗时。在诸如VINS-Mono系统中，执行完整矩阵的特征分解来计算矩阵的逆，这可能是非常耗时的过程，并且将影响最终算法的实时性能。
因此，做出了两项改变来优化MSL-DVIO 1D的边缘化过程。首先，问题被分成两个块：1)所有界标的边缘化；以及2)关键帧姿态、速率和偏差项的边缘化。所有界标的边缘化是容易的，因为从1D特征参数化提供了对角矩阵。给定信息矩阵H和误差向量
Figure BDA0003658645940000253
边缘化问题可以被重写为下面的等式(89)。
Figure BDA0003658645940000254
Hmm11是界标的信息矩阵，其是简单的对角矩阵
Figure BDA0003658645940000255
Figure BDA0003658645940000256
在第二步骤中，关键帧姿态、速率和偏差项被边缘化，如下表4所示。
表4 MSL-DVIO 1D的块边缘化
Figure BDA0003658645940000257
Figure BDA0003658645940000261
为了执行关键帧姿态、速率和偏差项的边缘化，使用安全Cholesky算法。这为MSL-DVIO 1D中块边缘化的第二步骤提供了更稳健和更快速的处理。在下面的表5中给出对该算法的概述。
表5-安全Cholesky分解
Figure BDA0003658645940000262
在MSL-DVIO 3D的情况下，不能执行单个简单的步骤来计算界标的块的逆，因为每个界标与图中的3个状态相关。因此，对每个界标逐一处理33矩阵的逆。因此，MSL-DVIO 3D的逐块边缘化过程变为如下表6所示的过程。
表6-MSL-DVIO 3D的块边缘化
Figure BDA0003658645940000263
Figure BDA0003658645940000271
如上所述，对于MSL-DVIO 3D，每个界标被单独边缘化。使用这种方法，由于矩阵求逆的规模很小，因此与试图在单个步骤中对一个大矩阵求逆相比，这要快得多，因此实现了在处理时间上的大幅改进。
VINS-Mono系统的平台设计可以用于上述系统设计。VINS-Mono具有使用图优化方法的当前最先进的VIO实现方式。可以在当前系统基础上添加深度辅助，以避免实现完整VIO系统所需的返工。
通过结合以下改变将深度辅助添加到VINS-Mono的视觉部分。
深度数据输入/输出(I/O)需要读取深度测量，并与2D特征测量同步处理这些测量。
深度测量需要计算与每一个2D特征测量相关联的深度测量，并将该测量添加到当前处理流水线中。
优化基于深度测量和状态参数计算深度残差。该残差作为图优化问题中的参数块被添加。
分别针对锚关键帧的深度辅助、非锚关键帧的深度辅助、非同步传感器的深度辅助以及3D特征参数化的深度辅助来执行深度数据I/O、深度测量以及深度残差和优化。
图5是示出根据实施例的MSL-DVIO的系统流程的图。
在测量处理块502中，从IMU 504、RGB相机506和深度传感器508获取输入数据，然后分别进行处理。RGB相机506和深度传感器508可以组合为RGBD传感器。对于来自IMG 504的IMU数据流，在测量处理块502处执行IMU预集成。对于来自RGB相机506的RGB相机帧，在测量处理块502处执行特征检测和跟踪。对于来自深度传感器508的深度图，在测量处理块502处，为在RGB相机帧中测量的特征生成深度。
在初始化块510中，从测量处理块502接收IMU预集成项和2D特征跟踪，并且执行来自运动的仅视觉结构。还执行视觉惯性对准来为滑动窗口图创建初始关键帧。
在运动BA块512中，从测量处理块502接收2D特征跟踪和IMU预集成项，以执行帧到帧姿态跟踪。
在状态估计器块514中，从初始化块510接收初始关键帧，并且从测量处理块502接收IMU预集成项、2D特征跟踪和深度测量。在状态估计器块514，执行因子图的优化，并且基于图2创建滑动图。一旦创建了图，使用非线性求解器来优化系统。
在环路闭合(loop closure)块516中，来自状态估计器514的姿态图被用于尝试最小化来自状态估计器块514的长期误差。
图6是示出了根据本公开的实施例的MSL-DVIO的详细系统流程的图。
测量处理块502包括IMU预集成块602、特征检测和跟踪块604以及深度测量块606。
状态估计器块514包括关键帧残差块608、滑动窗口图块610和图优化块612。关键帧残差块608包括IMU残差块614、2D特征残差块616和深度残差块618。滑动窗口图块610包括IMU参数块620、锚帧参数块622、非锚帧参数块624和时间偏移参数块626。
IMU残差是关键帧的6DOF姿态、IMU的速度和偏差的估计值与IMU测量(例如，加速度计和陀螺仪测量)之间的损失。图像残差是关键帧的6DOF姿态和界标的位置(1D或3D)的估计值与界标在关键帧上的2D测量之间的损失。深度残差是关键帧的6DOF姿态和界标的位置(1D或3D)的估计值与界标在深度图上的深度测量之间的损失。
图7是示出根据实施例的MSL-DVIO 1D特征实现的图。在MSL-DVIO中，深度辅助改变的实现方式分散在两个线程上。第一线程是特征跟踪器线程702，第二线程是状态估计器线程704。
特征跟踪器线程702包括深度和图像测量处理。来自RGB相机506和深度传感器508的RGBD数据在机器人操作系统(ROS)同步器块706处被同步。ROS同步器块706向特征检测和跟踪块604提供RGB图像消息和深度图像消息。特征检测和跟踪块604向深度测量块606提供特征测量和深度图像消息。深度测量块向速度计算块708提供特征测量和深度测量，该速度计算块708计算图像和深度域中的特征速度。然后，在710处，在ROS平台上发布速度、特征和深度测量。
状态估计器线程704处理图优化的大部分工作。在712处接收到IMU和特征数据。IMU数据被提供给IMU预集成块602，以创建IMU预集成项用于优化。特征数据被提供给关键帧选择块714，以创建可以添加到滑动窗口的关键帧。滑动窗口设置块716接收IMU预集成项和关键帧。在块718处，基于来自滑动窗口设置块716的滑动窗口数据，确定初始化是否完成。如果初始化完成，则滑动窗口数据被提供给三角化(triangulate)特征块720。如果初始化没有完成，则滑动窗口数据首先被提供给初始结构块722。然后，滑动窗口数据被提供给姿态图设置块724，其通过初始化滑动窗口中特征的深度来设置姿态图。
使用姿态图，在块726处确定IMU残差，并且在块728处确定用于姿态图优化的IMU因子。使用姿态图，在块730处确定是否已知时间增量估计。如果时间增量估计未知，则在块732处确定帧是否是锚帧。如果是锚帧，则在块734处确定锚帧深度残差，并且在块736处确定用于姿态图优化的锚帧视觉因子。如果不是锚帧，则在块738处确定非锚帧图像和深度残差，并且在块740处确定用于姿态图优化的非锚帧视觉因子。
一般地，视觉因子是使用视觉传感器生成的，包括关键帧的6DOF姿态、界标的位置、深度残差和图像残差。使用IMU传感器来生成IMU因子，包括关键帧的6DOF姿态、IMU的速度和偏差以及IMU残差。
如果时间增量估计是已知的，则在块742处确定帧是否是锚帧。如果是锚帧，则在块744处确定锚帧时间增量深度残差，并且在块746处确定用于姿态图优化的锚帧时间增量视觉因子。如果不是锚帧，则在块748处确定非锚帧图像和深度时间增量残差，并且在块750处确定用于姿态图优化的非锚帧时间增量视觉因子。
姿态图优化块752使用姿态图、IMU因子、锚帧视觉因子和非锚帧视觉因子来确定经优化的图。在块754处，对经优化的姿态图执行状态更新，并且在块756处执行边缘化，从而在块758处得到最终滑动窗口。
图8是示出根据实施例的MSL-DVIO用于3D特征参数化的实现细节的图。MSL-DVIO3D特征和MSL-DVIO 1D特征之间的主要不同在于MS-DVIO3D特征具有2D特征位置作为状态的一部分，因此在创建姿态图之前，在块802处估计特征的2D位置。第二个不同是锚帧视觉因子具有图像和深度残差两者。具体地，在块834处确定锚帧图像和深度残差，并且在块844处确定锚帧图像和深度时间增量残差。
在MSL-DVIO的这种实现方式中，修改对视觉因子的优化，但是对于整个优化问题IMU因子是不变的。当VIO系统未受到剧烈运动时，这没有任何影响。然而，在某些情况下，当运动相对剧烈时，尤其是在初始化阶段，可能会观察到轨迹的发散。
在作为初始化步骤的一部分运行第一次图优化之后，状态可能开始发散。因此，可以做出在MSL-DVIO和VINS-Mono的初始化步骤之间实现对等(parity)的系统设计决策。为了实现对等，可以如下所述地对系统进行修改。
在MSL-DVIO 1D特征中，对于初始化步骤，对于锚帧关闭对深度残差的计算，并且在非锚帧中深度残差被设置为零。特定于深度残差的视觉因子中的雅可比矩阵也被设置为零。
视觉残差在下面的等式(90)、(91)和(92)中给出。
Figure BDA0003658645940000301
Figure BDA0003658645940000302
Figure BDA0003658645940000303
改变后的雅可比矩阵部分在下面的等式(93)中给出。
Figure BDA0003658645940000304
在MSL-DVIO 3D特征中，可以使用上述估计算法来关闭对状态中的2D特征的初始化。2D特征状态用锚帧中的2D特征测量来初始化。对于锚帧，关闭计算深度和图像残差，并且对于非锚帧，深度残差被设置为零。特定于2D特征和深度残差的视觉因子中的雅可比矩阵被设置为零。
视觉残差在下面的等式(94)、(95)和(96)中给出
Figure BDA0003658645940000311
Figure BDA0003658645940000312
Figure BDA0003658645940000313
改变后的雅可比矩阵部分在下面的等式(97)中给出。
Figure BDA0003658645940000314
特征参数的堆叠雅可比矩阵在下面的等式(98)中给出。
Figure BDA0003658645940000315
在上述深度初始化中，使用深度测量关闭深度初始化。
图9是示出根据实施例的用于在UE处执行VIO的方法的流程图。在902处，处理来自UE的IMU、相机和深度传感器的测量。从IMU接收到数据流，并且执行预集成以生成IMU预集成项。通过相机捕获帧，并且执行特征检测和跟踪以生成2D特征跟踪。通过深度传感器为捕获帧的检测到的特征生成深度测量。
对捕获到的帧的特征检测和跟踪可以与深度测量同步。可以基于特征检测和跟踪以及深度测量来计算图像和深度域中的特征速度。
在904处，使用IMU预集成项和2D特征跟踪来执行关键帧初始化，以生成初始关键帧。
在906处，基于经处理的测量来确定关键帧残差。关键帧残差可以包括IMU残差、2D特征残差和深度残差。使用IMU预集成项来确定IMU残差，根据2D特征跟踪来确定2D特征残差，并且根据深度测量来确定深度残差。具体地，IMU残差是关键帧的6DOF姿态、IMU的速度和偏差的估计值与IMU测量(例如，加速度计和陀螺仪测量)之间的损失。图像残差是关键帧的6DOF姿态和界标的位置(1D或3D)的估计值与界标在关键帧上的2D测量之间的损失。深度残差是关键帧的6DOF姿态和界标的位置(1D或3D)的估计值与界标在深度图上的深度测量之间的损失。
在908处，基于初始关键帧和从关键帧残差导出的因子，生成并优化滑动窗口图。基于IMU残差来确定IMU因子。当捕获到的帧是锚帧时，基于深度残差或者基于2D特征残差和深度残差来确定锚帧视觉因子。当捕获到的帧是非锚帧时，基于2D特征残差和深度残差来确定非锚帧视觉因子。锚帧视觉因子和非锚帧视觉因子可以基于等于相机、深度传感器和IMU的时间戳的时间偏移。
一般地，视觉因子是使用视觉传感器生成的，包括关键帧的6DOF姿态、界标的位置、深度残差和图像残差。使用IMU传感器来生成IMU因子，包括关键帧的6DOF姿态、IMU的速度和偏差以及IMU残差。
基于IMU因子以及锚帧视觉因子和非锚帧视觉因子之一来优化滑动窗口图。优化可以包括上述两步边缘化。
在优化过程中，估计状态参数，使得在任何给定的时间点都存在图的最低残差。
在910处，基于经优化的滑动窗口图来估计UE的对象姿态。
为了比较MSL-DVIO系统的性能，可以使用由序列(其具有IMU相机和深度数据)组成的测试用例以及地面真值(ground truth)轨迹。提供两种类型的序列。
模拟序列生成轨迹的RGBD和IMU数据。
真实世界序列是由VINS RGBD系统创建的，其具有三(3)个在大空间里漫游的手持序列。这些序列可以使用Realsense D435传感器来创建，并且地面真值可以使用MoCap系统来生成。
深度不确定性结合(DUI)-VIO序列可以使用枕部(occipital)结构核心传感器来创建，并且地面真值可以使用MoCap系统来生成。
将MSL-DVIO系统的性能与VINS-Mono和VINS RGBD的性能进行了比较。
用于比较的测试度量是由下面的等式(99)给出的绝对平移误差的均方根误差(RMSE)。
Figure BDA0003658645940000331
其中，Q是地面真值轨迹，P是估计的轨迹，并且S是地面真值轨迹和估计的轨迹之间的刚体变换。
下表7提供了三种系统针对序列的比较性能。
表7-ATE性能结果
Figure BDA0003658645940000332
Figure BDA0003658645940000341
如表7所示，粗体数字表示序列的最佳性能,“X”表示系统无法初始化或导致ATE过大(>50米)。
还可以比较不同系统的时间性能(timing performance)，以确定算法的实时性能和算法改变的影响。为了进行这种比较，在VINS-Mono和VINS-RGBD系统的情况下以及在有快速边缘化改变和没有快速边缘化改变的情况下，对MSL-DVIO 1D和MSL-DVIO 3D系统的时间数字(timing number)进行了评估。结果如下表8所示。
表8-MSL-DVIO的处理时间评估
Figure BDA0003658645940000342
Figure BDA0003658645940000351
如表8所示，在快速边缘化改变的情况下，MSL-DVIO 1D(改进约80％)和MSL-DVIO3D(改进约94％)两者的边缘化处理时间均得到改进。因此，MSL-DVIO 1D的实时性能达到了约18Hz，并且MSL-DVIO 3D的实时性能达到了约11Hz。这两个数字都高于非线性优化线程的10Hz要求，以实现整个VIO系统的实时性能。
图10是示出根据一个实施例的网络环境中的电子设备的框图。参考图10，网络环境1000中的电子设备1001可以经由第一网络1098(例如，短距离无线通信网络)与电子设备1002通信或者经由第二网络1099(例如，长距离无线通信网络)与电子设备1004或服务器1008通信。电子设备1001可以经由服务器1008与电子设备1004通信。电子设备1001可以包括处理器1020、存储器1030、输入设备1050、声音输出设备1055、显示设备1060、音频模块1070、传感器模块1076、接口1077、触觉模块1079、相机模块1080、电力管理模块1088、电池1089、通信模块1090、订户识别模块(SIM)1096或天线模块1097。在一个实施例中，可以从电子设备1001中省略组件中的至少一个(例如，显示设备1060或相机模块1080)或者可以将一个或多个其它组件添加到电子设备1001中。组件中的一些可以被实现为单个集成电路(IC)。例如，传感器模块1076(例如，指纹传感器、虹膜传感器或照度传感器)可以包含在显示设备1060(例如，显示器)中。
处理器1020可以运行例如软件(例如，程序1040)来控制电子设备1001的与处理器1020连接的至少一个其它组件(例如，硬件组件或软件组件)，并且可以执行各种数据处理或计算。作为数据处理或计算的至少部分，处理器1020可以将从另一个组件(例如，传感器模块1076或通信模块1090)接收到的命令或数据加载到易失性存储器1032中，对存储在易失性存储器1032中的命令或数据进行处理，并将得到的数据存储在非易失性存储器1034中。处理器1020可以包括主处理器1021(例如，中央处理器(CPU)或应用处理器(AP))以及与主处理器1021在操作上独立的或者相结合的辅助处理器1023(例如，图形处理单元(GPU)、图像信号处理器(ISP)、传感器中枢处理器或通信处理器(CP))。附加地或可替代地，辅助处理器1023可以被适配为比主处理器1021消耗更少的电力或者被适配为执行特定的功能。辅助处理器1023可以被实现为与主处理器1021分离或者被实现为主处理器1021的一部分。
在主处理器1021处于非活动(例如，睡眠)状态时，辅助处理器1023(而非主处理器1021)可以控制与电子设备1001的组件之中的至少一个组件(例如，显示设备1060、传感器模块1076或通信模块1090)相关的功能或状态中的至少一些、或者在主处理器1021处于活动状态(例如，运行应用)时，辅助处理器1023可以与主处理器1021一起来控制与电子设备1001的组件之中的至少一个组件(例如，显示设备1060、传感器模块1076或通信模块1090)相关的功能或状态中的至少一些。辅助处理器1023(例如，图像信号处理器或通信处理器)可以被实现为在功能上与辅助处理器1023相关的另一个组件(例如，相机模块1080或通信模块1090)的一部分。
存储器1030可以存储由电子设备1001的至少一个组件(例如，处理器1020或传感器模块1076)使用的各种数据。各种数据可以包括例如软件(例如，程序1040)以及针对与其相关的命令的输入数据或输出数据。存储器1030可以包括易失性存储器1032或非易失性存储器1034。
程序1040可以作为软件存储在存储器1030中，并且可以包括例如操作系统(OS)1042、中间件1044或应用1046。
输入设备1050可以从电子设备1001的外部(例如，用户)接收将由电子设备1001的其它组件(例如，处理器1020)使用的命令或数据。输入设备1050可以包括例如麦克风、鼠标或键盘。
声音输出设备1055可以将声音信号输出到电子设备1001的外部。声音输出设备1055可以包括例如扬声器或接收器。扬声器可以用于诸如播放多媒体或播放唱片的通用目的，并且接收器可以用于接收传入的呼叫。接收器可以被实现为与扬声器分离或者被实现为扬声器的一部分。
显示设备1060可以向电子设备1001的外部(例如，用户)视觉地提供信息。显示设备1060可以包括例如显示器、全息设备或投影仪以及用于控制显示器、全息设备和投影仪中对应的一个的控制电路。显示设备1060可以包括被适配为检测触摸的触摸电路或被适配为测量由触摸引起的力的强度的传感器电路(例如，压力传感器)。
音频模块1070可以将声音转换为电信号，反之亦可。音频模块1070可以经由输入设备1050获得声音或者经由声音输出设备1055或与电子设备1001直接(例如，有线地)耦合或无线耦合的外部电子设备1002的耳机输出声音。
传感器模块1076可以检测电子设备1001的操作状态(例如，功率或温度)或电子设备1001外部的环境状态(例如，用户的状态)，然后生成与检测到的状态相对应的电信号或数据值。传感器模块1076可以包括例如姿势传感器、陀螺仪传感器、大气压力传感器、磁传感器、加速度传感器、握持传感器、接近传感器、颜色传感器、红外(IR)传感器、生物测定传感器、温度传感器、湿度传感器或照度传感器。
接口1077可以支持将用于电子设备1001与外部电子设备1002直接(例如，有线地)或无线耦合的一个或多个特定协议。接口1077可以包括例如高清晰度多媒体接口(HDMI)、通用串行总线(USB)接口、安全数字(SD)卡接口或音频接口。
连接终端1078可以包括电子设备1001可以经由其与外部电子设备1002物理连接的连接器。连接终端1078可以包括例如HDMI连接器、USB连接器、SD卡连接器或音频连接器(例如，耳机连接器)。
触觉模块1079可以将电信号转换为可以由用户经由触觉或动觉辨识的机械刺激(例如，振动或运动)或电刺激。触觉模块1079可以包括例如电机、压电元件或电刺激器。
相机模块1080可以捕获静止图像或运动图像。相机模块1080可以包括一个或多个镜头、图像传感器、图像信号处理器或闪光灯。
电力管理模块1088可以管理向电子设备1001供应的电力。电力管理模块1088可以被实现为例如电力管理集成电路(PMIC)的至少部分。
电池1089可以向电子设备1001的至少一个组件供电。电池1089可以包括例如不可再充电的原电池、可再充电的二次电池或燃料电池。
通信模块1090可以支持在电子设备1001与外部电子设备(例如，电子设备1002、电子设备1004或服务器1008)之间建立直接(例如，有线)通信信道或无线通信信道，并经由建立的通信信道执行通信。通信模块1090可以包括能够与处理器1020(例如，AP)独立操作的一个或多个通信处理器，并且支持直接(例如，有线)通信或无线通信。通信模块1090可以包括无线通信模块1092(例如，蜂窝通信模块、短距离无线通信模块或全球导航卫星系统(GNSS)通信模块)或有线通信模块1094(例如，局域网(LAN)通信模块或电力线通信(PLC)模块)。这些通信模块中对应的一个可以经由第一网络1098(例如，短距离通信网络，诸如BluetoothTM、无线保真(Wi-Fi)直连或红外数据协会(IrDA))或第二网络1099(例如，长距离通信网络，诸如蜂窝网络、互联网、或计算机网络(例如，LAN或广域网(WAN)))来与外部电子设备通信。各种类型的通信模块可以被实现为单个组件(例如，单个IC)或者可以被实现为彼此分离的多个组件(例如，多个IC)。无线通信模块1092可以使用存储在订户识别模块1096中的订户信息(例如，国际移动订户识别码(IMSI))来识别并验证通信网络(诸如第一网络1098或第二网络1099)中的电子设备1001。
天线模块1097可以将信号或电力发送到电子设备1001的外部(例如，外部电子设备)或者从电子设备1001的外部(例如，外部电子设备)接收信号或电力。天线模块1097可以包括一个或多个天线，并且可以由例如通信模块1090(例如，无线通信模块1092)来选择适合于在通信网络(诸如第一网络1098或第二网络1099)中使用的通信方案的至少一个天线。然后，可以经由所选至少一个天线在通信模块1090和外部电子设备之间发送或接收信号或电力。
上述组件中的至少一些可以经由外设间通信方案(例如，总线、通用输入输出(GPIO)、串行外设接口(SPI)或移动工业处理器接口(MIPI))相互连接并在它们之间传送信号(例如，命令或数据)。
可以经由与第二网络1099耦合的服务器1008来在电子设备1001和外部电子设备1004之间发送或接收命令或数据。电子设备1002和电子设备1004中的每一个可以是与电子设备1001相同类型的设备或者是与电子设备1001不同类型的设备。将在电子设备1001处运行的全部或一些操作可以在外部电子设备1002、外部电子设备1004或服务器1008中的一个或多个处运行。例如，如果电子设备1001应该自动地执行功能或服务或者应该响应于来自用户或另一个设备的请求执行功能或服务，则电子设备1001可以请求一个或多个外部电子设备执行功能或服务中的至少部分，而不是运行功能或服务；或者电子设备1001除了运行功能或服务以外，还可以请求一个或多个外部电子设备执行功能或服务中的至少部分。接收请求的一个或多个外部电子设备可以执行所请求的功能或服务中的至少部分或者执行与请求相关的附加功能或附加服务，并将执行的结果传送到电子设备1001。电子设备1001可以在对结果进行进一步处理的情况下或者在不对结果进行进一步处理的情况下将结果作为对请求的答复的至少部分来提供。为此，可以使用例如云计算技术、分布式计算技术或客户端-服务器计算技术。
一个实施例可以被实现为包括存储在存储介质(例如，内部存储器1036或外部存储器1038)中的可由机器(例如，电子设备1001)读取的一个或多个指令的软件(例如，程序1040)。例如，在处理器的控制下，电子设备1001的处理器可以在使用或不使用一个或多个其它组件的情况下调用存储在存储介质中的一个或多个指令中的至少一个指令并运行该至少一个指令。因此，可以操作机器以根据所调用的至少一个指令来执行至少一个功能。一个或多个指令可以包括由编译器生成的代码或可由解释器运行的代码。可以以非暂时性存储介质的形式来提供机器可读存储介质。术语“非暂时性存储介质”指示存储介质是有形设备，并且不包括信号(例如，电磁波)，但是该术语并不区分数据被半永久性地存储在存储介质中与数据被临时存储在存储介质中。
根据一个实施例，可以在计算机程序产品中包括和提供本公开的方法。计算机程序产品可以作为产品在销售者和购买者之间进行交易。可以以机器可读存储介质(例如，紧凑盘只读存储器(CD-ROM))的形式来分发计算机程序产品、或可以经由应用商店(例如，Play StoreTM)在线分发(例如，下载或上传)计算机程序产品或者可以直接在两个用户设备(例如，智能手机)之间分发计算机程序产品。如果是在线分发的，则计算机程序产品中的至少部分可以是临时生成的或者是至少临时存储在机器可读存储介质(诸如制造商的服务器、应用商店的服务器或中继服务器的存储器)中。
根据一个实施例，上述组件中的每个组件(例如，模块或程序)可以包括单个实体或多个实体。可以省略上述组件中的一个或多个或者可以添加一个或多个其它组件。可替代地或附加地，可以将多个组件(例如，模块或程序)集成为单个组件。在这种情况下，集成的组件可以仍然按照与由多个组件中对应的一个组件在集成之前执行一个或多个功能相同或相似的方式，来执行多个组件中的每一个的一个或多个功能。根据各种实施例，由模块、程序或另一个组件执行的操作可以顺序地、并行地、重复地或以启发式方式来执行或者操作中的一个或多个可以按照不同的次序来执行或被省略、或者可以添加一个或多个其它操作。
尽管在本公开的详细描述中已经描述了本公开的某些实施例，但是在不脱离本公开的范围的情况下，可以以各种形式修改本公开。因此，本公开的范围不应仅基于所描述的实施例来确定，而是基于所附权利要求及其等同物来确定。

Claims (20)

1.一种用于在用户设备UE处执行视觉惯性测距VIO的方法，所述方法包括：
处理来自UE的惯性测量单元IMU、相机和深度传感器的测量；
基于经处理的测量来确定至少包括深度残差的关键帧残差；
基于从所述关键帧残差导出的因子来生成和优化滑动窗口图；以及
基于经优化的滑动窗口图来估计UE的对象姿态。
2.根据权利要求1所述的方法，其中，处理测量包括：
从IMU接收数据流并对所述数据流执行预集成，以生成IMU预集成项；
通过相机捕获帧并对捕获到的帧执行特征检测和跟踪，以生成二维2D特征跟踪；以及
通过深度传感器来为捕获到的帧的检测到的特征生成深度测量。
3.根据权利要求2所述的方法，其中，确定关键帧残差包括：
使用所述IMU预集成项来确定IMU残差；
根据所述2D特征跟踪来确定2D特征残差；以及
根据所述深度测量来确定深度残差。
4.根据权利要求3所述的方法，其中，生成和优化滑动窗口图包括：
基于所述IMU残差来确定IMU因子；
在捕获到的帧是锚帧的情况下，至少基于所述深度残差来确定锚帧视觉因子；
在捕获到的帧是非锚帧的情况下，基于所述2D特征残差和所述深度残差来确定非锚帧视觉因子；以及
基于所述IMU因子以及所述锚帧视觉因子和所述非锚帧视觉因子之一来优化所述滑动窗口图。
5.根据权利要求4所述的方法，其中，所述锚帧视觉因子是基于所述2D特征残差和所述深度残差来确定的。
6.根据权利要求4所述的方法，其中，所述锚帧视觉因子和所述非锚帧视觉因子是基于等于相机、深度传感器和IMU的时间戳的时间偏移的。
7.根据权利要求2所述的方法，还包括：使用所述IMU预集成项和所述2D特征跟踪来执行关键帧初始化，以生成初始关键帧，
其中，所述滑动窗口图是基于所述初始关键帧来生成的。
8.根据权利要求2所述的方法，还包括：将针对捕获到的帧的特征检测和跟踪与检测到的特征的深度测量进行同步。
9.根据权利要求2所述的方法，还包括：基于所述特征检测和跟踪以及所述深度测量来计算图像和深度域中的特征速度。
10.根据权利要求4所述的方法，其中，优化滑动窗口图包括：
在捕获到的帧中执行界标的边缘化；以及
执行关键帧姿态、速率和偏差项的边缘化。
11.一种用户设备UE，包括：
惯性测量单元IMU；
相机；
深度传感器；
处理器；以及
存储指令的非暂时性计算机可读存储介质，所述指令在被执行时使得处理器：
处理来自惯性测量单元(IMU)、相机和深度传感器的测量；
基于经处理的测量来确定至少包括深度残差的关键帧残差；
基于从所述关键帧残差导出的因子来生成和优化滑动窗口图；以及
基于经优化的滑动窗口图来估计UE的对象姿态。
12.根据权利要求11所述的UE，其中，在处理测量时，所述指令还使得处理器：
从IMU接收数据流并对所述数据流执行预集成，以生成IMU预集成项；
通过相机捕获帧并对捕获到的帧执行特征检测和跟踪，以生成二维2D特征跟踪；以及
通过深度传感器来为捕获到的帧的检测到的特征生成深度测量。
13.根据权利要求12所述的UE，其中，在确定关键帧残差时，所述指令还使得处理器：
使用所述IMU预集成项来确定IMU残差；
根据所述2D特征跟踪来确定2D特征残差；以及
根据所述深度测量来确定深度残差。
14.根据权利要求13所述的UE，其中，在生成和优化滑动窗口图时，所述指令还使得处理器：
基于所述IMU残差来确定IMU因子；
在捕获到的帧是锚帧的情况下，至少基于所述深度残差来确定锚帧视觉因子；
在捕获到的帧是非锚帧的情况下，基于所述2D特征残差和所述深度残差来确定非锚帧视觉因子；以及
基于所述IMU因子以及所述锚帧视觉因子和所述非锚帧视觉因子之一来优化所述滑动窗口图。
15.根据权利要求14所述的UE，其中，所述锚帧视觉因子是基于所述2D特征残差和所述深度残差来确定的。
16.根据权利要求14所述的UE，其中，所述锚帧视觉因子和所述非锚帧视觉因子是基于等于相机、深度传感器和IMU的时间戳的时间偏移的。
17.根据权利要求12所述的UE，其中，所述指令还使得处理器：使用所述IMU预集成项和所述2D特征跟踪来执行关键帧初始化，以生成初始关键帧，
其中，所述滑动窗口图是基于所述初始关键帧来生成的。
18.根据权利要求12所述的UE，其中，所述指令还使得处理器：将针对捕获到的帧的特征检测和跟踪与检测到的特征的深度测量进行同步。
19.根据权利要求12所述的UE，其中，所述指令还使得处理器：基于所述特征检测和跟踪以及所述深度测量来计算图像和深度域中的特征速度。
20.根据权利要求14所述的UE，其中，在优化滑动窗口图时，所述指令还使得处理器：
在捕获到的帧中执行界标的边缘化；以及
执行关键帧姿态、速率和偏差项的边缘化。
CN202210569696.4A 2021-05-24 2022-05-24 用于深度辅助视觉惯性测距的方法和装置 Pending CN115388882A (zh)

Applications Claiming Priority (4)

Application Number Priority Date Filing Date Title
US202163192488P 2021-05-24 2021-05-24
US63/192,488 2021-05-24
US17/726,124 US20220414919A1 (en) 2021-05-24 2022-04-21 Method and apparatus for depth-aided visual inertial odometry
US17/726,124 2022-04-21

Publications (1)

Publication Number Publication Date
CN115388882A true CN115388882A (zh) 2022-11-25

Family

ID=83997287

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210569696.4A Pending CN115388882A (zh) 2021-05-24 2022-05-24 用于深度辅助视觉惯性测距的方法和装置

Country Status (5)

Country Link
US (1) US20220414919A1 (zh)
KR (1) KR20220158628A (zh)
CN (1) CN115388882A (zh)
DE (1) DE102022111926A1 (zh)
TW (1) TW202247099A (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117710449B (zh) * 2024-02-05 2024-04-16 中国空气动力研究与发展中心高速空气动力研究所 基于numa的实时位姿视频测量流水线模型优化方法

Also Published As

Publication number Publication date
KR20220158628A (ko) 2022-12-01
TW202247099A (zh) 2022-12-01
US20220414919A1 (en) 2022-12-29
DE102022111926A1 (de) 2022-12-01

Similar Documents

Publication Publication Date Title
US11140379B2 (en) Mapping and tracking system with features in three-dimensional space
CN109029433B (zh) 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
TWI834744B (zh) 使用具有不同視野的相機進行差異估測的電子裝置及方法
Kneip et al. Robust real-time visual odometry with a single camera and an IMU
Tanskanen et al. Live metric 3D reconstruction on mobile phones
Sweeney et al. Solving for relative pose with a partially known rotation is a quadratic eigenvalue problem
US9243916B2 (en) Observability-constrained vision-aided inertial navigation
CN113874870A (zh) 基于图像的定位
TW202030697A (zh) 電子裝置及恢復深度圖的方法
CN104106262A (zh) 使用深度相机的头部姿态跟踪
US11423510B2 (en) System and method for providing dolly zoom view synthesis
CN116205947B (zh) 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质
WO2023087681A1 (zh) 定位初始化方法、装置、计算机可读存储介质和计算机程序产品
Huai et al. Real-time large scale 3D reconstruction by fusing Kinect and IMU data
Sartipi et al. Decentralized visual-inertial localization and mapping on mobile devices for augmented reality
CN115388882A (zh) 用于深度辅助视觉惯性测距的方法和装置
Örnhag et al. Trust your IMU: Consequences of ignoring the IMU drift
US20240087155A1 (en) Prior informed pose and scale estimation
CN113847907A (zh) 定位方法及装置、设备、存储介质
Gui et al. Robust direct visual inertial odometry via entropy-based relative pose estimation
Gedik et al. Fusing 2D and 3D clues for 3D tracking using visual and range data
US20240029350A1 (en) Computing apparatus and model generation method
US20230290101A1 (en) Data processing method and apparatus, electronic device, and computer-readable storage medium
CN114943765A (zh) 一种室内姿态估计方法及装置
CN116958198A (zh) 基于视觉惯性里程计的运动轨迹漂移检测方法及装置

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