CN113029134A - 视觉惯性系统的初始化方法、装置、电子设备及存储介质 - Google Patents

视觉惯性系统的初始化方法、装置、电子设备及存储介质 Download PDF

Info

Publication number
CN113029134A
CN113029134A CN202110232556.3A CN202110232556A CN113029134A CN 113029134 A CN113029134 A CN 113029134A CN 202110232556 A CN202110232556 A CN 202110232556A CN 113029134 A CN113029134 A CN 113029134A
Authority
CN
China
Prior art keywords
visual
inertial system
sensor
state
tracking
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.)
Granted
Application number
CN202110232556.3A
Other languages
English (en)
Other versions
CN113029134B (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.)
Zhejiang Shangtang Technology Development Co Ltd
Zhejiang Sensetime Technology Development Co Ltd
Original Assignee
Zhejiang Shangtang Technology Development 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 Zhejiang Shangtang Technology Development Co Ltd filed Critical Zhejiang Shangtang Technology Development Co Ltd
Priority to CN202110232556.3A priority Critical patent/CN113029134B/zh
Publication of CN113029134A publication Critical patent/CN113029134A/zh
Priority to PCT/CN2021/107702 priority patent/WO2022183665A1/zh
Application granted granted Critical
Publication of CN113029134B publication Critical patent/CN113029134B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S11/00—Systems for determining distance or velocity not using reflection or reradiation
    • G01S11/12—Systems for determining distance or velocity not using reflection or reradiation using electromagnetic waves other than radio waves

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Automation & Control Theory (AREA)
  • Gyroscopes (AREA)

Abstract

本申请提供了一种视觉惯性系统的初始化方法、装置、电子设备及存储介质，其中，该方法包括：采用离线标定的参数对视觉惯性系统进行首次初始化；对所述视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数；根据该稳定状态和跟踪状态对所述第一热启动参数进行更新；基于上一次更新得到的第二热启动参数对所述视觉惯性系统进行当前时刻的初始化。本申请实施例有利于提升视觉惯性系统初始化的速度。

Description

视觉惯性系统的初始化方法、装置、电子设备及存储介质
技术领域
本申请涉及计算机视觉技术领域，尤其涉及一种视觉惯性系统的初始化方法、装置、电子设备及存储介质。
背景技术
随着计算机视觉技术的发展，基于视觉和惯性传感器的同时定位与地图构建(Visual-Inertial Simultaneous Localization and Mapping，VI-SLAM)系统(以下简称视觉惯性系统)被广泛应用于虚拟现实、无人机、移动机器人、无人驾驶等领域。视觉惯性系统能够利用视觉传感器和惯性传感器采集的数据获得高精度的位姿，并完成一维或多维环境地图的构建，然而视觉惯性系统在启动时每次都要进行初始化，且每次初始化都使用离线标定的参数，这就使得系统状态的稳定性会受到影响，从而导致系统初始化的速度较慢。
发明内容
针对上述问题，本申请提供了一种视觉惯性系统的初始化方法、装置、电子设备及存储介质，有利于提升视觉惯性系统的初始化速度。
为实现上述目的，本申请实施例提供了一种视觉惯性系统的初始化方法，该方法包括：
采用离线标定的参数对视觉惯性系统进行首次初始化；
对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数；
根据该稳定状态和跟踪状态对第一热启动参数进行更新；
基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化。
在一种可能的实施方式中，对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数，包括：
根据视觉惯性系统的第一状态数据确定视觉惯性系统状态稳定以及跟踪稳定；
在视觉惯性系统状态稳定以及跟踪稳定的情况下，将第一状态数据存储为第一热启动参数。
在一种可能的实施方式中，根据该稳定状态和跟踪状态对第一热启动参数进行更新，包括：
在视觉惯性系统跟踪的图像每满足第一预设帧数的情况下，执行对上述稳定状态和上述跟踪状态进行判定的操作；
在确定视觉惯性系统状态稳定以及跟踪稳定的情况下，对第一热启动参数进行更新。
在一种可能的实施方式中，视觉惯性系统包括惯性传感器，第一状态数据包括：视觉惯性系统在初始化第二预设帧数的图像的情况下计算得到的滑动窗口内线性加速度的协方差、惯性传感器的旋转积累次数、惯性传感器的加速度计和陀螺仪的第一零偏、视觉惯性系统的第一速度以及视觉惯性系统跟踪的图像特征的数量。
在一种可能的实施方式中，根据视觉惯性系统的第一状态数据确定视觉惯性系统状态稳定以及跟踪稳定，包括：
根据线性加速度的协方差及旋转积累次数确定惯性传感器得到激励；
在惯性传感器得到激励、第一零偏收敛以及第一速度收敛的情况下，确定视觉惯性系统状态稳定；
在视觉惯性系统跟踪的图像特征的数量大于或等于第一阈值，且图像特征未出现跟踪丢失的情况下，确定视觉惯性系统跟踪稳定。
在一种可能的实施方式中，根据线性加速度的协方差及旋转积累次数确定惯性传感器得到激励，包括：
在线性加速度的协方差大于第二阈值且旋转积累次数大于第三阈值的情况下，确定惯性传感器得到激励。
在一种可能的实施方式中，视觉惯性系统还包括视觉传感器，第一状态数据还包括：视觉惯性系统的第一时间、第一温度、第一零偏的协方差、视觉传感器和惯性传感器之间的第一时间偏移、视觉传感器到惯性传感器外参的第一平移和第一旋转、视觉传感器的第一视场角、视觉传感器采集的图像的第一宽度和第一高度、视觉传感器的第一内参。
在一种可能的实施方式中，基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化，包括：
在进行当前时刻的初始化时，确定第二热启动参数存在，并载入第二热启动参数的部分或全部；第二热启动参数的部分或全部包括视觉惯性系统的第二时间、第二温度、视觉传感器的第二视场角、视觉传感器采集的图像的第二宽度和第二高度、惯性传感器的加速度计和陀螺仪的第二零偏、第二零偏的协方差、视觉传感器和惯性传感器之间的第二时间偏移、视觉传感器到惯性传感器外参的第二平移和第二旋转以及视觉传感器的第二内参；
在视觉传感器验证通过的情况下，利用第二时间、第二温度对第二零偏的协方差进行膨胀，得到膨胀后的协方差；
在第二零偏验证通过，且视觉传感器未采用在线标定的内参的情况下，采用第二视场角、第二宽度和第二高度、第二零偏、膨胀后的协方差、第二时间偏移、第二平移和第二旋转以及第二内参进行当前时刻的初始化。
在一种可能的实施方式中，该方法还包括：
在第二零偏验证通过，且视觉传感器采用在线标定的内参的情况下，对在线标定的内参进行验证；
在在线标定的内参验证通过的情况下，采用第二视场角、第二宽度和第二高度、第二零偏、膨胀后的协方差、第二时间偏移、第二平移和第二旋转以及在线标定的内参进行当前时刻的初始化。
在一种可能的实施方式中，在载入第二热启动参数的部分或全部之后，该方法还包括：
利用第二宽度、第二高度以及第二视场角对视觉传感器进行验证；
在第二宽度等于第一宽度、第二高度等于第一高度且第二视场角等于第一视场角的情况下，确定视觉传感器验证通过。
本申请另一些实施例提供了一种视觉惯性系统的初始化装置，该装置包括：
第一初始化模块，用于采用离线标定的参数对视觉惯性系统进行首次初始化；
状态判定模块，用于对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数；
先验获取模块，用于根据该稳定状态和跟踪状态对第一热启动参数进行更新；
第二初始化模块，用于基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化。
本申请另一些实施例提供了一种电子设备，该电子设备包括输入设备和输出设备，还包括处理器，适于实现一条或多条指令；以及，计算机存储介质，所述计算机存储介质存储有一条或多条指令，所述一条或多条指令适于由所述处理器加载并执行以下步骤：
采用离线标定的参数对视觉惯性系统进行首次初始化；
对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数；
根据该稳定状态和跟踪状态对第一热启动参数进行更新；
基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化。
本申请另一些实施例提供了一种计算机存储介质，所述计算机存储介质存储有一条或多条指令，所述一条或多条指令适于由处理器加载并执行以下步骤：
采用离线标定的参数对视觉惯性系统进行首次初始化；
对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数；
根据该稳定状态和跟踪状态对第一热启动参数进行更新；
基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化。
本申请另一些实施例提供了一种计算机程序产品，其中，上述计算机程序产品包括存储了计算机程序的非瞬时性计算机可读存储介质，该计算机程序产品可以为一个软件安装包。上述计算机程序可操作来使计算机执行以下步骤：
采用离线标定的参数对视觉惯性系统进行首次初始化；
对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数；
根据该稳定状态和跟踪状态对第一热启动参数进行更新；
基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化。
可以看出，本申请实施例通过采用离线标定的参数对视觉惯性系统进行首次初始化；对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数；根据该稳定状态和跟踪状态对第一热启动参数进行更新；基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化。由于第一热启动参数是在视觉惯性系统状态稳定以及跟踪稳定的情况下存储的，且在之后的图像跟踪中会根据视觉惯性系统的稳定状态和跟踪状态不断对第一热启动参数进行更新，且最近一次更新得到的第二热启动参数同样是在视觉惯性系统状态稳定以及跟踪稳定的情况下存储的，相对于离线标定的参数，该第二热启动参数稳定性更高，在当前时刻的初始化中能够使视觉惯性系统快速趋于稳定，从而有利于提升系统初始化的速度。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的一种视觉惯性系统的初始化方法的流程示意图；
图2为本申请实施例提供的一种获取热启动的先验参数的示意图；
图3为本申请实施例提供的一种进行K次初始化的示意图；
图4为本申请实施例提供的另一种视觉惯性系统的初始化方法的流程示意图；
图5为本申请实施例提供的一种视觉惯性系统的初始化装置的结构示意图；
图6为本申请实施例提供的一种电子设备的结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分的实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。
本申请说明书、权利要求书和附图中出现的术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。此外，术语“第一”、“第二”和“第三”等是用于区别不同的对象，而并非用于描述特定的顺序。
以下结合相关附图对本申请实施例提供的视觉惯性系统的初始化方法进行详细阐述。
请参见图1，图1为本申请实施例提供的一种视觉惯性系统的初始化方法的流程示意图，应用于电子设备，该电子设备可以是视觉惯性系统的参数标定设备，比如包含该视觉惯性系统的终端设备或服务器，其中，该终端设备可以是用户设备(User Equipment，UE)、移动设备、蜂窝电话、个人数字助理、手持设备、车载设备或可穿戴设备等，服务器可以是独立的物理服务器，也可以是服务器集群或者分布式系统，还可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务、云通信、中间件服务、域名服务、安全服务、以及大数据和人工智能平台等基础云计算服务的云服务器。如图1所示，该视觉惯性系统的初始化方法包括步骤S11-S14：
S11，采用离线标定的参数对视觉惯性系统进行首次初始化。
应当理解的，视觉惯性系统中通常包括视觉传感器和惯性传感器，视觉传感器是利用光学元件和成像装置获取外部环境图像信息的仪器，比如可以采用单目相机、双目相机、多目相机或RGBD(Red，红；Green，绿；Blue，蓝；Deep，深度)相机等，惯性传感器是利用传感质量惯性力进行测量的传感器，主要包含三轴的加速度计和三轴的陀螺仪，也被称作惯性测量单元(Inertial Measurement Unit，IMU)。视觉惯性系统通过视觉传感器、惯性传感器、超声波、激光测距仪、激光雷达、超宽带(Ultra Wide Band，UWB)和全球导航卫星系统(Global Navigation Satellite System，GNSS)中的一种或多种进行定位和地图构建。
本公开实施例中，在视觉惯性系统首次开机初始化时，仍然采用离线标定的参数进行，其中，离线标定的参数包括但不限于视觉传感器的内参、视觉传感器和惯性传感器之间的时间偏移、视觉传感器和惯性传感器的外参、惯性传感器的加速度计和陀螺仪的零偏，通过将上述离线标定的参数中的一种或多种载入视觉惯性系统，对视觉惯性系统进行首次初始化。
S12，对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数。
本公开实施例中，在首次初始化之后实时对视觉惯性系统的稳定状态和跟踪状态进行判定，具体的，如图2所示，一种可能性的对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数的方法包括步骤S1201-S1202：
S1201，根据视觉惯性系统的第一状态数据确定视觉惯性系统状态稳定以及跟踪稳定；
S1202，在视觉惯性系统状态稳定以及跟踪稳定的情况下，将第一状态数据存储为第一热启动参数。
本公开实施例中，上述第一状态数据是指首次初始化之后视觉惯性系统的实时状态数据，包括视觉惯性系统在初始化第二预设帧数的图像的情况下计算得到的滑动窗口内线性加速度的协方差covacc、惯性传感器的旋转积累次数rotation、惯性传感器的加速度计和陀螺仪的第一零偏rba和rbg、视觉惯性系统的第一速度v、视觉惯性系统跟踪的图像特征的数量feacnt、视觉惯性系统的第一时间rt、第一温度rT、第一零偏rba的协方差rcovba和rbg的协方差rcovbg、视觉传感器和惯性传感器之间的第一时间偏移rtd、视觉传感器到惯性传感器外参的第一平移rTIC和第一旋转rQIC、视觉传感器的第一视场角rfov、视觉传感器采集的图像的第一宽度rIw和第一高度rIh以及视觉传感器的第一内参。其中，该第一内参包括视觉传感器的第一焦距rfx和rfy、第一光心rcx和rcy、第一径向畸变参数rk1和rk2、第一切向畸变参数rp1和rp2。
具体的，第二预设帧数可以预先设定，在视觉惯性系统初始化第二预设帧数的图像后，采用以下公式计算上述线性加速度的协方差covacc：
Figure BDA0002958096590000081
Figure BDA0002958096590000082
Figure BDA0002958096590000083
其中，vi表示滑动窗口内第i帧图像的速度，
Figure BDA0002958096590000084
表示第i-1帧到第i帧图像之间的时间间隔，acci表示滑动窗口内第i帧图像的线性加速度，meaacc表示视觉惯性系统的线性加速度平均值，w表示滑动窗口的数量，比如可以取20。
采用以下公式计算惯性传感器的旋转积累次数rotation：
Figure BDA0002958096590000085
其中，sumangle表示惯性传感器的旋转积累次数rotation，anglemin表示最小旋转角度阈值，当第i帧图像的累计旋转角度大于或等于anglemin时将会滑入下一帧进行判断，m表示判断的帧数，
Figure BDA0002958096590000086
表示第i-1帧到第i帧图像的积累旋转角度，(x,y,z)表示坐标轴。
其中，在上述线性加速度的协方差covacc大于第二阈值covmin且惯性传感器的旋转积累次数rotation大于第三阈值rotationmin的情况下，确定惯性传感器得到充分激励。示例性的，covmin可以取1.0，rotationmin可以取15。
进一步的，在确定惯性传感器得到充分激励之后，判断惯性传感器的加速度计和陀螺仪的第一零偏rba和rbg是否收敛，具体实施中，采用以下公式计算惯性传感器的加速度计和陀螺仪的第一零偏rba的协方差covrba和rbg的协方差covrbg：
Figure BDA0002958096590000087
Figure BDA0002958096590000088
Figure BDA0002958096590000089
Figure BDA00029580965900000810
其中，mearba和mearbg分别表示滑动窗口内惯性传感器加速度计和陀螺仪的第一零偏的平均值，rbai和rbgi分别表示第i帧图像估计得到的惯性传感器的加速度计和陀螺仪的第一零偏，在covrba小于第四阈值
Figure BDA00029580965900000811
且covrbg小于第五阈值
Figure BDA0002958096590000091
的情况下，确定惯性传感器的加速度计和陀螺仪的第一零偏rba和rbg收敛。其中，第四阈值
Figure BDA0002958096590000092
表示加速度计的第一零偏rba满足收敛的最大值，第五阈值
Figure BDA0002958096590000093
表示陀螺仪的第一零偏rbg满足收敛的最大值，示例性的，第四阈值
Figure BDA0002958096590000094
可以取0.0002，第五阈值
Figure BDA0002958096590000095
可以取0.00005。
进一步的，在确定惯性传感器的加速度计和陀螺仪的第一零偏rba和rbg收敛之后，判断上述第一速度v是否收敛，具体实施中，在非静止的情况下采用以下公式计算滑动窗口内每帧图像的第一速度v的协方差covv：
Figure BDA0002958096590000096
Figure BDA0002958096590000097
其中，meav表示滑动窗口内第一速度v的平均值，vi表示第第i帧图像的第一速度。在covv小于第六阈值
Figure BDA0002958096590000098
的情况下，确定第一速度v收敛，第六阈值
Figure BDA0002958096590000099
表示第一速度v满足收敛的最大值，示例性的，第六阈值
Figure BDA00029580965900000910
可以取1.0。
本公开实施例中，在惯性传感器得到激励、第一零偏rba和rbg收敛、第一速度v收敛的情况下，确定视觉惯性系统状态稳。另外，在视觉惯性系统跟踪的图像特征的数量feacnt大于或等于第一阈值，且图像特征未出现跟踪丢失的情况下，确定视觉惯性系统跟踪稳定。比如，在feacnt超过20个且都没有跟丢的情况下，可确定视觉惯性系统跟踪稳定，其中，图像特征可以是跟踪的地标、角点等。在视觉惯性系统的状态稳定以及跟踪稳定的情况下，对第一状态数据进行存储，得到视觉惯性系统初次存储的第一热启动参数。
S13，根据该稳定状态和跟踪状态对第一热启动参数进行更新。
本公开实施例中，随着视觉惯性系统的运行，电子设备会在系统稳定且跟踪稳定的情况下，对之前存储的第一热启动参数进行更新，一种可能性的根据视该稳定状态和跟踪状态对第一热启动参数进行更新的方法包括：
在视觉惯性系统跟踪的图像每满足第一预设帧数的情况下，执行对上述稳定状态和上述跟踪状态进行判定的操作；
在确定视觉惯性系统状态稳定以及跟踪稳定的情况下，对第一热启动参数进行更新。
本公开实施例中，第一预设帧数可以是300帧，比如对视觉惯性系统每隔300帧进行一次稳定状态和跟踪状态的判定，其判定过程如步骤S12所述。在视觉惯性系统的状态稳定且跟踪稳定的情况下，对第一热启动参数进行更新。相反的，在该稳定状态和跟踪状态中的至少一者为不稳定的情况下，不执行更新操作。具体的，在满足以下任一种的情况下，认为视觉惯性系统不稳定：第一零偏rba或rbg不收敛；第一速度v不收敛；跟踪的图像特征的数量feacnt小于第一阈值；跟踪的图像特征的数量feacnt大于或等于第一阈值但出现跟踪丢失的情况。需要说明的是，由于惯性传感器得到激励，此处不再对此进行重复判断。对第一热启动参数进行更新的操作可以是在首次初始化之后的某次初始化中进行的，比如在第二次初始化中，每当视觉惯性系统跟踪的图像达到300帧时，执行对稳定状态和跟踪状态进行判定的操作，若第二次初始化中未对第一热启动参数进行更新，则仍基于第一热启动参数进行第三次初始化，可以理解的，若第二次初始化中对第一热启动参数进行了更新，则基于最近一次更新后的热启动参数进行第三次初始化，每次初始化中更新的情况可以相同。
S14，基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化。
本公开实施例中，上一次是指距当前时刻最近一次更新热启动参数的时间点，比如在进行第二次初始化时，若未对第一热启动参数进行更新，则上一次更新得到的第二热启动参数即为上述第一热启动参数，若对第一热启动参数进行了更新，则上一次更新得到的第二热启动参数即为最近一次更新后的热启动参数。再比如在进行第三次初始化时，若第二次初始化未对第一热启动参数进行更新，则上一次更新得到的第二热启动参数仍为上述第一热启动参数，若在第二次初始化中对第一热启动参数进行了更新，则上一次更新得到的第二热启动参数即为第二次初始化中最近一次更新后的热启动参数。
在对视觉惯性系统进行当前时刻的初始化时，首先检测热启动的文件中是否存储了上述第二热启动参数，在确定存在该第二热启动参数的情况下，将该第二热启动参数作为先验参数，执行如图3中所示的步骤S1401-S1403：
S1401，载入该先验参数的部分或全部。
具体的，该先验参数的部分或全部包括视觉惯性系统的第二时间pt、第二温度pT、视觉传感器的第二视场角pfov、视觉传感器采集的图像的第二宽度pIw和第二高度pIh、惯性传感器的加速度计和陀螺仪的第二零偏pba和pbg、第二零偏pba的协方差pcovba和第二零偏pbg的协方差pcovbg、视觉传感器和惯性传感器之间的第二时间偏移ptd、视觉传感器到惯性传感器外参的第二平移pTIC和第二旋转pQIC以及视觉传感器的第二内参。其中，该第二内参包括视觉传感器的第一焦距pfx和pfy、第一光心pcx和pcy、第一径向畸变参数pk1和pk2、第一切向畸变参数pp1和pp2。
S1402，在视觉传感器验证通过的情况下，利用第二时间、第二温度对第二零偏的协方差进行膨胀，得到膨胀后的协方差。
具体的，在载入先验参数的部分或全部之后，利用第二宽度pIw、第二高度pIh以及第二视场角pfov对视觉传感器进行验证，在第二宽度pIw等于第一宽度rIw、第二高度pIh等于第一高度rIh且第二视场角pfov等于第一视场角rfov的情况下，确定视觉传感器验证通过。即满足以下等式确定视觉传感器验证通过：
pIw＝rIw；
pIh＝rIh；
pfov＝rfov；
该实施方式中，在进行当前时刻的初始化时先对视觉传感器进行验证，以防止视觉传感器更换或图像分辨率调整导致初始化出错。
具体的，采用以下公式对第二零偏pba的协方差pcovba和第二零偏pbg的协方差pcovbg进行膨胀：
ecovba＝Et*pcovba+(rt-pt+|rT-pT|)*pbaw*pbaw；
ecovbg＝Et*pcovbg+(rt-pt+|rT-pT|)*pbgw*pbgw；
其中，ecovba表示协方差pcovba膨胀后的协方差，ecovbg表示协方差pcovbg膨胀后的协方差，Et表示膨胀系数。
该实施方式中，由于惯性传感器的性能容易受到温度的影响，并且随着时间的增长会增加更多噪声，这就会导致存储的先验参数偏移稳定数值，因此利用第二时间pt、第二温度pT对第二零偏pba的协方差pcovba和第二零偏pbg的协方差pcovbg进行膨胀，利用膨胀后的参数ecovba和ecovbg进行当前时刻的初始化，有利于提高视觉惯性系统初始化时的鲁棒性。
S1403，在第二零偏验证通过，且视觉传感器未采用在线标定的内参的情况下，采用第二视场角、第二宽度和第二高度、第二零偏、膨胀后的协方差、第二时间偏移、第二平移和第二旋转以及第二内参进行当前时刻的初始化。
具体的，在对第二零偏pba的协方差pcovba和第二零偏pbg的协方差pcovbg进行膨胀之后，该方法还包括：
对第二零偏进行验证；其验证过程如下式所示：
eba＝pba，0<pba<1；eba＝0，pba>1；当第二零偏pba∈(0,1)时，第二零偏pba验证通过，采用第二零偏pba进行当前时刻的初始化，当第二零偏pba大于1时，验证不通过，采用0进行当前时刻的初始化，eba即最终采用的零偏。
ebg＝pbg，0<pbg<0.1；eba＝0，pbg>0.1；当第二零偏pbg属于(0,0.1)时，第二零偏pbg验证通过，采用第二零偏pbg进行当前时刻的初始化，当第二零偏pbg大于0.1时，验证不通过，采用0进行当前时刻的初始化，ebg即最终采用的零偏。
在一种可能的实施方式中，该方法还包括：
在第二零偏验证通过，且视觉传感器采用在线标定的内参的情况下，对在线标定的内参进行验证；
在在线标定的内参验证通过的情况下，采用第二视场角、第二宽度和第二高度、第二零偏、膨胀后的协方差、第二时间偏移、第二平移和第二旋转以及在线标定的内参进行当前时刻的初始化。
本公开实施例中，在视觉传感器使用在线标定的内参的情况下，执行对在线标定的内参进行验证的操作，其中。在线标定的内参包括视觉传感器的第三焦距efx和efy、第三光心ecx和ecy、第三径向畸变参数ek1和ek2、第三切向畸变参数ep1和ep2。
第三焦距efx和efy的验证过程如下式所示：
efx＝efx，0.4*(rIw+rIh)<efx<0.6*(rIw+rIh)；efx＝rIw/2*tan(rfov÷2)，其它；当efx属于(0.4*(rIw+rIh)，0.6*(rIw+rIh))时，验证通过，采用efx进行当前时刻的初始化，当efx不属于(0.4*(rIw+rIh)，0.6*(rIw+rIh))时，验证不通过，第三焦距efx采用rIw/2*tan(rfov÷2)进行当前时刻的初始化。
efy＝efy，0.4*(rIw+rIh)<efy<0.6*(rIw+rIh)；efy＝rIw/2*tan(rfov÷2)，其它；当efy属于(0.4*(rIw+rIh)，0.6*(rIw+rIh))时，验证通过，采用efy进行当前时刻的初始化；当efy不属于(0.4*(rIw+rIh)，0.6*(rIw+rIh))时，验证不通过，第三焦距efy采用rIw/2*tan(rfov÷2)进行当前时刻的初始化。
第三光心ecx和ecy的验证过程如下式所示：
ecx＝ecx，0.15*rIh<ecx<0.85*rIh；ecx＝rIh/2，其它；当ecx属于(0.15*rIh，0.85*rIh)时，验证通过，采用efy进行当前时刻的初始化；当ecx不属于(0.15*rIh，0.85*rIh)时，验证不通过，第三光心ecx采用rIh/2进行当前时刻的初始化。
ecy＝ecy，0.15*rIw<ecy<0.85*rIw；ecy＝rIw/2，其它；当ecy属于(0.15*rIw，0.85*rIw)时，验证通过，采用ecy进行当前时刻的初始化；当ecy不属于(0.15*rIw，0.85*rIw)时，验证不通过，第三光心ecy采用rIw/2进行当前时刻的初始化。
第三径向畸变参数ek1和ek2的验证过程如下式所示：
ek1＝ek1，|ek1|<0.1；ek1＝0，其它；当ek1的绝对值小于0.1时，采用ek1进行当前时刻的初始化，当ek1的绝对值大于或等于0.1时，第三径向畸变参数ek1采用0进行当前时刻的初始化。
ek2＝ek2，|ek2|<0.5；ek2＝0，其它；当ek2的绝对值小于0.5时，采用ek2进行当前时刻的初始化，当ek2的绝对值大于或等于0.5时，第三径向畸变参数ek2采用0进行当前时刻的初始化。
第三切向畸变参数ep1和ep2验证过程如下式所示：
epj＝epj，|epj|<0.005；epj＝0，其它；j∈(1,2)；当第三切向畸变参数ep1和ep2的绝对值小于0.005时，采用第三切向畸变参数ep1和ep2进行当前时刻的初始化，当第三切向畸变参数ep1和ep2的绝对值大于或等于0.005时，第三切向畸变参数ep1和ep2采用0进行当前时刻的初始化。
可以看出，本申请实施例通过采用离线标定的参数对视觉惯性系统进行首次初始化；对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数；根据该稳定状态和跟踪状态对第一热启动参数进行更新；基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化。由于第一热启动参数是在视觉惯性系统状态稳定以及跟踪稳定的情况下存储的，且在之后的图像跟踪中会根据视觉惯性系统的稳定状态和跟踪状态不断对第一热启动参数进行更新，且最近一次更新得到的第二热启动参数同样是在视觉惯性系统状态稳定以及跟踪稳定的情况下存储的，相对于离线标定的参数，该第二热启动参数稳定性更高，在当前时刻的初始化中能够使视觉惯性系统快速趋于稳定，从而有利于提升系统初始化的速度。另外，由于先验参数(第二热启动参数)中惯性传感器加速度计和陀螺仪的零偏、视觉传感器和惯性传感器的速度都处于收敛，且在进行当前时刻的初始化时对惯性传感器加速度计和陀螺仪的零偏进行了膨胀，有利于消除噪声和温度的影响，基于这样的先验参数进行当前时刻的初始化，有利于提升视觉惯性系统定位和建图的精度。
请参见图4，图4为本申请实施例提供的另一种视觉惯性系统的初始化方法的流程示意图，如图4所示，包括步骤S41-S45：
S41，采用离线标定的参数对视觉惯性系统进行首次初始化；
S42，根据视觉惯性系统的第一状态数据确定视觉惯性系统状态稳定以及跟踪稳定；
S43，在视觉惯性系统状态稳定以及跟踪稳定的情况下，将第一状态数据存储为第一热启动参数；
S44，根据视觉惯性系统的稳定状态和跟踪状态对第一热启动参数进行更新；
S45，基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化。
其中，上述步骤S41-S45的具体实施方式，在图1-图3所示的实施例中已有相关说明，且能达到相同或相似的有益效果，此处不再赘述。
基于图1或图4所示方法实施例的描述，本申请实施例还提供一种视觉惯性系统的初始化装置，请参见图5，图5为本申请实施例提供的一种视觉惯性系统的初始化装置的结构示意图，如图5所示，该装置包括：
第一初始化模块51，用于采用离线标定的参数对视觉惯性系统进行首次初始化；
状态判定模块52，用于对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数；
先验获取模块53，用于根据该稳定状态和跟踪状态对第一热启动参数进行更新；
第二初始化模块54，用于基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化。
在一种可能的实施方式中，在对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数方面，状态判定模块52具体用于：
根据视觉惯性系统的第一状态数据确定视觉惯性系统状态稳定以及跟踪稳定；
在视觉惯性系统状态稳定以及跟踪稳定的情况下，将第一状态数据存储为第一热启动参数。
在一种可能的实施方式中，在根据该稳定状态和跟踪状态对第一热启动参数进行更新方面，先验获取模块53具体用于：
在视觉惯性系统跟踪的图像每满足第一预设帧数的情况下，执行对上述稳定状态和上述跟踪状态进行判定的操作；
在确定视觉惯性系统状态稳定以及跟踪稳定的情况下，对第一热启动参数进行更新。
在一种可能的实施方式中，视觉惯性系统包括惯性传感器，第一状态数据包括：视觉惯性系统在初始化第二预设帧数的图像的情况下计算得到的滑动窗口内线性加速度的协方差、惯性传感器的旋转积累次数、惯性传感器的加速度计和陀螺仪的第一零偏、视觉惯性系统的第一速度以及视觉惯性系统跟踪的图像特征的数量。
在一种可能的实施方式中，在根据视觉惯性系统的第一状态数据确定视觉惯性系统状态稳定以及跟踪稳定方面，先验获取模块53具体用于：
根据线性加速度的协方差及旋转积累次数确定惯性传感器得到激励；
在惯性传感器得到激励、第一零偏收敛以及第一速度收敛的情况下，确定视觉惯性系统状态稳定；
在视觉惯性系统跟踪的图像特征的数量大于或等于第一阈值，且图像特征未出现跟踪丢失的情况下，确定视觉惯性系统跟踪稳定。
在一种可能的实施方式中，在根据线性加速度的协方差及旋转积累次数确定惯性传感器得到激励方面，先验获取模块53具体用于：
在线性加速度的协方差大于第二阈值且旋转积累次数大于第三阈值的情况下，确定惯性传感器得到激励。
在一种可能的实施方式中，视觉惯性系统还包括视觉传感器，第一状态数据还包括：视觉惯性系统的第一时间、第一温度、第一零偏的协方差、视觉传感器和惯性传感器之间的第一时间偏移、视觉传感器到惯性传感器外参的第一平移和第一旋转、视觉传感器的第一视场角、视觉传感器采集的图像的第一宽度和第一高度、视觉传感器的第一内参。
在一种可能的实施方式中，在基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化方面，第二初始化模块54具体用于：
在进行当前时刻的初始化时，确定第二热启动参数存在，并载入第二热启动参数的部分或全部；第二热启动参数的部分或全部包括视觉惯性系统的第二时间、第二温度、视觉传感器的第二视场角、视觉传感器采集的图像的第二宽度和第二高度、惯性传感器的加速度计和陀螺仪的第二零偏、第二零偏的协方差、视觉传感器和惯性传感器之间的第二时间偏移、视觉传感器到惯性传感器外参的第二平移和第二旋转以及视觉传感器的第二内参；
在视觉传感器验证通过的情况下，利用第二时间、第二温度对第二零偏的协方差进行膨胀，得到膨胀后的协方差；
在第二零偏验证通过，且视觉传感器未采用在线标定的内参的情况下，采用第二视场角、第二宽度和第二高度、第二零偏、膨胀后的协方差、第二时间偏移、第二平移和第二旋转以及第二内参进行当前时刻的初始化。
在一种可能的实施方式中，第二初始化模块54还用于：
在第二零偏验证通过，且视觉传感器采用在线标定的内参的情况下，对在线标定的内参进行验证；
在在线标定的内参验证通过的情况下，采用第二视场角、第二宽度和第二高度、第二零偏、膨胀后的协方差、第二时间偏移、第二平移和第二旋转以及在线标定的内参进行当前时刻的初始化。
在一种可能的实施方式中，第二初始化模块54还用于：
利用第二宽度、第二高度以及第二视场角对视觉传感器进行验证；
在第二宽度等于第一宽度、第二高度等于第一高度且第二视场角等于第一视场角的情况下，确定视觉传感器验证通过。
根据本申请的一个实施例，图5所示的视觉惯性系统的初始化装置中的各个单元可以分别或全部合并为一个或若干个另外的单元来构成，或者其中的某个(些)单元还可以再拆分为功能上更小的多个单元来构成，这可以实现同样的操作，而不影响本申请的实施例的技术效果的实现。上述单元是基于逻辑功能划分的，在实际应用中，一个单元的功能也可以由多个单元来实现，或者多个单元的功能由一个单元实现。在本申请的其它实施例中，基于视觉惯性系统的初始化装置也可以包括其它单元，在实际应用中，这些功能也可以由其它单元协助实现，并且可以由多个单元协作实现。
根据本申请的另一个实施例，可以通过在包括中央处理单元(CPU)、随机存取存储介质(RAM)、只读存储介质(ROM)等处理元件和存储元件的例如计算机的通用计算设备上运行能够执行如图1或图4中所示的相应方法所涉及的各步骤的计算机程序(包括程序代码)，来构造如图5中所示的视觉惯性系统的初始化装置设备，以及来实现本申请实施例的视觉惯性系统的初始化方法。所述计算机程序可以记载于例如计算机可读记录介质上，并通过计算机可读记录介质装载于上述计算设备中，并在其中运行。
基于上述方法实施例和装置实施例的描述，本申请实施例还提供一种电子设备。请参见图6，该电子设备至少包括处理器61、输入设备62、输出设备63以及计算机存储介质64。其中，电子设备内的处理器61、输入设备62、输出设备63以及计算机存储介质64可通过总线或其他方式连接。
计算机存储介质64可以存储在电子设备的存储器中，所述计算机存储介质64用于存储计算机程序，所述计算机程序包括程序指令，所述处理器61用于执行所述计算机存储介质64存储的程序指令。处理器61(或称CPU(Central Processing Unit，中央处理器))是电子设备的计算核心以及控制核心，其适于实现一条或多条指令，具体适于加载并执行一条或多条指令从而实现相应方法流程或相应功能。
在一个实施例中，本申请实施例提供的电子设备的处理器61可以用于进行一系列视觉惯性系统的初始化处理：
采用离线标定的参数对视觉惯性系统进行首次初始化；
对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数；
根据该稳定状态和跟踪状态对第一热启动参数进行更新；
基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化。
再一个实施例中，处理器61执行上述对视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数，包括：
根据视觉惯性系统的第一状态数据确定视觉惯性系统状态稳定以及跟踪稳定；
在视觉惯性系统状态稳定以及跟踪稳定的情况下，将第一状态数据存储为第一热启动参数。
再一个实施例中，处理器61执行上述根据该稳定状态和跟踪状态对第一热启动参数进行更新，包括：
在视觉惯性系统跟踪的图像每满足第一预设帧数的情况下，执行对上述稳定状态和上述跟踪状态进行判定的操作；
在确定视觉惯性系统状态稳定以及跟踪稳定的情况下，对第一热启动参数进行更新。
再一个实施例中，视觉惯性系统包括惯性传感器，第一状态数据包括：视觉惯性系统在初始化第二预设帧数的图像的情况下计算得到的滑动窗口内线性加速度的协方差、惯性传感器的旋转积累次数、惯性传感器的加速度计和陀螺仪的第一零偏、视觉惯性系统的第一速度以及视觉惯性系统跟踪的图像特征的数量。
再一个实施例中，处理器61执行上述根据视觉惯性系统的第一状态数据确定视觉惯性系统状态稳定以及跟踪稳定，包括：
根据线性加速度的协方差及旋转积累次数确定惯性传感器得到激励；
在惯性传感器得到激励、第一零偏收敛以及第一速度收敛的情况下，确定视觉惯性系统状态稳定；
在视觉惯性系统跟踪的图像特征的数量大于或等于第一阈值，且图像特征未出现跟踪丢失的情况下，确定视觉惯性系统跟踪稳定。
再一个实施例中，处理器61执行上述根据线性加速度的协方差及旋转积累次数确定惯性传感器得到激励，包括：
在线性加速度的协方差大于第二阈值且旋转积累次数大于第三阈值的情况下，确定惯性传感器得到激励。
再一个实施例中，视觉惯性系统还包括视觉传感器，第一状态数据还包括：视觉惯性系统的第一时间、第一温度、第一零偏的协方差、视觉传感器和惯性传感器之间的第一时间偏移、视觉传感器到惯性传感器外参的第一平移和第一旋转、视觉传感器的第一视场角、视觉传感器采集的图像的第一宽度和第一高度、视觉传感器的第一内参。
再一个实施例中，处理器61执行上述基于上一次更新得到的第二热启动参数对视觉惯性系统进行当前时刻的初始化，包括：
在进行当前时刻的初始化时，确定第二热启动参数存在，并载入第二热启动参数的部分或全部；第二热启动参数的部分或全部包括视觉惯性系统的第二时间、第二温度、视觉传感器的第二视场角、视觉传感器采集的图像的第二宽度和第二高度、惯性传感器的加速度计和陀螺仪的第二零偏、第二零偏的协方差、视觉传感器和惯性传感器之间的第二时间偏移、视觉传感器到惯性传感器外参的第二平移和第二旋转以及视觉传感器的第二内参；
在视觉传感器验证通过的情况下，利用第二时间、第二温度对第二零偏的协方差进行膨胀，得到膨胀后的协方差；
在第二零偏验证通过，且视觉传感器未采用在线标定的内参的情况下，采用第二视场角、第二宽度和第二高度、第二零偏、膨胀后的协方差、第二时间偏移、第二平移和第二旋转以及第二内参进行当前时刻的初始化。
再一个实施例中，处理器61还用于：
在第二零偏验证通过，且视觉传感器采用在线标定的内参的情况下，对在线标定的内参进行验证；
在在线标定的内参验证通过的情况下，采用第二视场角、第二宽度和第二高度、第二零偏、膨胀后的协方差、第二时间偏移、第二平移和第二旋转以及在线标定的内参进行当前时刻的初始化。
再一个实施例中，在载入第二热启动参数的部分或全部之后，处理器61还用于：
利用第二宽度、第二高度以及第二视场角对视觉传感器进行验证；
在第二宽度等于第一宽度、第二高度等于第一高度且第二视场角等于第一视场角的情况下，确定视觉传感器验证通过。
示例性的，电子设备可包括但不仅限于处理器61、输入设备62、输出设备63以及计算机存储介质64，输入设备62可以是键盘、触摸屏等，输出设备63可以是扬声器、显示器、射频发送器等。本领域技术人员可以理解，所述示意图仅仅是电子设备的示例，并不构成对电子设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件。
需要说明的是，由于电子设备的处理器61执行计算机程序时实现上述的视觉惯性系统的初始化方法中的步骤，因此上述视觉惯性系统的初始化方法的实施例均适用于该电子设备，且均能达到相同或相似的有益效果。
本申请实施例还提供了一种计算机存储介质(Memory)，所述计算机存储介质是电子设备中的记忆设备，用于存放程序和数据。可以理解的是，此处的计算机存储介质既可以包括终端中的内置存储介质，当然也可以包括终端所支持的扩展存储介质。计算机存储介质提供存储空间，该存储空间存储了终端的操作系统。并且，在该存储空间中还存放了适于被处理器61加载并执行的一条或多条的指令，这些指令可以是一个或一个以上的计算机程序(包括程序代码)。需要说明的是，此处的计算机存储介质可以是高速RAM存储器，也可以是非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器；可选的，还可以是至少一个位于远离前述处理器61的计算机存储介质。在一个实施例中，可由处理器61加载并执行计算机存储介质中存放的一条或多条指令，以实现上述有关视觉惯性系统的初始化方法的相应步骤。
示例性的，计算机存储介质的计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。
需要说明的是，由于计算机存储介质的计算机程序被处理器执行时实现上述的视觉惯性系统的初始化方法中的步骤，因此上述视觉惯性系统的初始化方法的所有实施例均适用于该计算机存储介质，且均能达到相同或相似的有益效果。
本申请实施例还提供了一种计算机程序产品，其中，上述计算机程序产品包括存储了计算机程序的非瞬时性计算机可读存储介质，该计算机程序产品可以为一个软件安装包。上述计算机程序可操作来使计算机执行上述有关视觉惯性系统的初始化方法的相应步骤。
以上对本申请实施例进行了详细介绍，本文中应用了具体个例对本申请的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本申请的方法及其核心思想；同时，对于本领域的一般技术人员，依据本申请的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本申请的限制。

Claims (13)

1.一种视觉惯性系统的初始化方法，其特征在于，所述方法包括：
采用离线标定的参数对视觉惯性系统进行首次初始化；
对所述视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数；
根据所述稳定状态和所述跟踪状态对所述第一热启动参数进行更新；
基于上一次更新得到的第二热启动参数对所述视觉惯性系统进行当前时刻的初始化。
2.根据权利要求1所述的方法，其特征在于，所述对所述视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数，包括：
根据所述视觉惯性系统的第一状态数据确定所述视觉惯性系统状态稳定以及跟踪稳定；
在所述视觉惯性系统状态稳定以及跟踪稳定的情况下，将所述第一状态数据存储为所述第一热启动参数。
3.根据权利要求2所述的方法，其特征在于，所述根据所述稳定状态和所述跟踪状态对所述第一热启动参数进行更新，包括：
在所述视觉惯性系统跟踪的图像每满足第一预设帧数的情况下，执行对所述稳定状态和所述跟踪状态进行判定的操作；
在确定所述视觉惯性系统状态稳定以及跟踪稳定的情况下，对所述第一热启动参数进行更新。
4.根据权利要求2所述的方法，其特征在于，所述视觉惯性系统包括惯性传感器，所述第一状态数据包括：所述视觉惯性系统在初始化第二预设帧数的图像的情况下计算得到的滑动窗口内线性加速度的协方差、所述惯性传感器的旋转积累次数、所述惯性传感器的加速度计和陀螺仪的第一零偏、所述视觉惯性系统的第一速度以及所述视觉惯性系统跟踪的图像特征的数量。
5.根据权利要求4所述的方法，其特征在于，所述根据所述视觉惯性系统的第一状态数据确定所述视觉惯性系统状态稳定以及跟踪稳定，包括：
根据所述线性加速度的协方差及所述旋转积累次数确定所述惯性传感器得到激励；
在所述惯性传感器得到激励、所述第一零偏收敛以及所述第一速度收敛的情况下，确定所述视觉惯性系统状态稳定；
在所述视觉惯性系统跟踪的图像特征的数量大于或等于第一阈值，且所述图像特征未出现跟踪丢失的情况下，确定所述视觉惯性系统跟踪稳定。
6.根据权利要求5所述的方法，其特征在于，所述根据所述线性加速度的协方差及所述旋转积累次数确定所述惯性传感器得到激励，包括：
在所述线性加速度的协方差大于第二阈值且所述旋转积累次数大于第三阈值的情况下，确定所述惯性传感器得到激励。
7.根据权利要求3-6任一项所述的方法，其特征在于，所述视觉惯性系统还包括视觉传感器，所述第一状态数据还包括：所述视觉惯性系统的第一时间、第一温度、所述第一零偏的协方差、所述视觉传感器和所述惯性传感器之间的第一时间偏移、所述视觉传感器到所述惯性传感器外参的第一平移和第一旋转、所述视觉传感器的第一视场角、所述视觉传感器采集的图像的第一宽度和第一高度、所述视觉传感器的第一内参。
8.根据权利要求7所述的方法，其特征在于，所述基于上一次更新得到的第二热启动参数对所述视觉惯性系统进行当前时刻的初始化，包括：
在进行当前时刻的初始化时，确定所述第二热启动参数存在，并载入所述第二热启动参数的部分或全部；所述第二热启动参数的部分或全部包括所述视觉惯性系统的第二时间、第二温度、所述视觉传感器的第二视场角、所述视觉传感器采集的图像的第二宽度和第二高度、所述惯性传感器的加速度计和陀螺仪的第二零偏、所述第二零偏的协方差、所述视觉传感器和所述惯性传感器之间的第二时间偏移、所述视觉传感器到所述惯性传感器外参的第二平移和第二旋转以及所述视觉传感器的第二内参；
在所述视觉传感器验证通过的情况下，利用所述第二时间、所述第二温度对所述第二零偏的协方差进行膨胀，得到膨胀后的协方差；
在所述第二零偏验证通过，且所述视觉传感器未采用在线标定的内参的情况下，采用所述第二视场角、所述第二宽度和所述第二高度、所述第二零偏、所述膨胀后的协方差、所述第二时间偏移、所述第二平移和所述第二旋转以及所述第二内参进行当前时刻的初始化。
9.根据权利要求8所述的方法，其特征在于，所述方法还包括：
在所述第二零偏验证通过，且所述视觉传感器采用在线标定的内参的情况下，对所述在线标定的内参进行验证；
在所述在线标定的内参验证通过的情况下，采用所述第二视场角、所述第二宽度和所述第二高度、所述第二零偏、所述膨胀后的协方差、所述第二时间偏移、所述第二平移和所述第二旋转以及所述在线标定的内参进行当前时刻的初始化。
10.根据权利要求8所述的方法，其特征在于，在载入所述第二热启动参数的部分或全部之后，所述方法还包括：
利用所述第二宽度、所述第二高度以及所述第二视场角对所述视觉传感器进行验证；
在所述第二宽度等于所述第一宽度、所述第二高度等于所述第一高度且所述第二视场角等于所述第一视场角的情况下，确定所述视觉传感器验证通过。
11.一种视觉惯性系统的初始化装置，其特征在于，所述装置包括：
第一初始化模块，用于采用离线标定的参数对视觉惯性系统进行首次初始化；
状态判定模块，用于对所述视觉惯性系统的稳定状态和跟踪状态进行判定，以得到第一热启动参数；
先验获取模块，用于根据所述稳定状态和所述跟踪状态对所述第一热启动参数进行更新；
第二初始化模块，用于基于上一次更新得到的第二热启动参数对所述视觉惯性系统进行当前时刻的初始化。
12.一种电子设备，包括输入设备和输出设备，其特征在于，还包括：
处理器，适于实现一条或多条指令；以及，
计算机存储介质，所述计算机存储介质存储有一条或多条指令，所述一条或多条指令适于由所述处理器加载并执行如权利要求1-10任一项所述的方法。
13.一种计算机存储介质，其特征在于，所述计算机存储介质存储有一条或多条指令，所述一条或多条指令适于由处理器加载并执行如权利要求1-10任一项所述的方法。
CN202110232556.3A 2021-03-02 2021-03-02 视觉惯性系统的初始化方法、装置、电子设备及存储介质 Active CN113029134B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202110232556.3A CN113029134B (zh) 2021-03-02 2021-03-02 视觉惯性系统的初始化方法、装置、电子设备及存储介质
PCT/CN2021/107702 WO2022183665A1 (zh) 2021-03-02 2021-07-21 初始化方法及装置、电子设备、存储介质和程序产品

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110232556.3A CN113029134B (zh) 2021-03-02 2021-03-02 视觉惯性系统的初始化方法、装置、电子设备及存储介质

Publications (2)

Publication Number Publication Date
CN113029134A true CN113029134A (zh) 2021-06-25
CN113029134B CN113029134B (zh) 2022-04-08

Family

ID=76466362

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110232556.3A Active CN113029134B (zh) 2021-03-02 2021-03-02 视觉惯性系统的初始化方法、装置、电子设备及存储介质

Country Status (2)

Country Link
CN (1) CN113029134B (zh)
WO (1) WO2022183665A1 (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114485649A (zh) * 2022-02-09 2022-05-13 北京自动化控制设备研究所 面向无人机的惯性、视觉和高度信息融合导航方法
WO2022183665A1 (zh) * 2021-03-02 2022-09-09 浙江商汤科技开发有限公司 初始化方法及装置、电子设备、存储介质和程序产品

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115540854A (zh) * 2022-12-01 2022-12-30 成都信息工程大学 一种基于uwb辅助的主动定位方法、设备和介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101598556A (zh) * 2009-07-15 2009-12-09 北京航空航天大学 一种未知环境下无人机视觉/惯性组合导航方法
CN109767470A (zh) * 2019-01-07 2019-05-17 浙江商汤科技开发有限公司 一种跟踪系统初始化方法及终端设备
US20190163198A1 (en) * 2017-11-29 2019-05-30 Qualcomm Incorporated Radar aided visual inertial odometry initialization
CN110119189A (zh) * 2018-02-05 2019-08-13 浙江商汤科技开发有限公司 Slam系统的初始化、ar控制方法、装置和系统
CN111156997A (zh) * 2020-03-02 2020-05-15 南京航空航天大学 一种基于相机内参在线标定的视觉/惯性组合导航方法
CN111551191A (zh) * 2020-04-28 2020-08-18 浙江商汤科技开发有限公司 传感器外参数标定方法及装置、电子设备和存储介质
CN112229424A (zh) * 2020-11-16 2021-01-15 浙江商汤科技开发有限公司 视觉惯性系统的参数标定方法及装置、电子设备和介质
CN112284381A (zh) * 2020-10-19 2021-01-29 北京华捷艾米科技有限公司 视觉惯性实时初始化对准方法及系统

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9678102B2 (en) * 2011-11-04 2017-06-13 Google Inc. Calibrating intertial sensors using an image sensor
CN107850436B (zh) * 2015-05-23 2021-03-05 深圳市大疆创新科技有限公司 使用惯性传感器和图像传感器的传感器融合
CN108489482B (zh) * 2018-02-13 2019-02-26 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及系统
CN111220155A (zh) * 2020-03-04 2020-06-02 广东博智林机器人有限公司 基于双目视觉惯性里程计估计位姿的方法、装置与处理器
CN111486867B (zh) * 2020-03-19 2023-08-11 天津大学 一种视觉和惯性混合跟踪组件安装参数的标定装置及方法
CN113029134B (zh) * 2021-03-02 2022-04-08 浙江商汤科技开发有限公司 视觉惯性系统的初始化方法、装置、电子设备及存储介质

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101598556A (zh) * 2009-07-15 2009-12-09 北京航空航天大学 一种未知环境下无人机视觉/惯性组合导航方法
US20190163198A1 (en) * 2017-11-29 2019-05-30 Qualcomm Incorporated Radar aided visual inertial odometry initialization
CN110119189A (zh) * 2018-02-05 2019-08-13 浙江商汤科技开发有限公司 Slam系统的初始化、ar控制方法、装置和系统
CN109767470A (zh) * 2019-01-07 2019-05-17 浙江商汤科技开发有限公司 一种跟踪系统初始化方法及终端设备
CN111156997A (zh) * 2020-03-02 2020-05-15 南京航空航天大学 一种基于相机内参在线标定的视觉/惯性组合导航方法
CN111551191A (zh) * 2020-04-28 2020-08-18 浙江商汤科技开发有限公司 传感器外参数标定方法及装置、电子设备和存储介质
CN112284381A (zh) * 2020-10-19 2021-01-29 北京华捷艾米科技有限公司 视觉惯性实时初始化对准方法及系统
CN112229424A (zh) * 2020-11-16 2021-01-15 浙江商汤科技开发有限公司 视觉惯性系统的参数标定方法及装置、电子设备和介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
TONG QIN等: "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator", 《IEEE TRANSACTIONS ON ROBOTICS》 *
陈常: "基于视觉和惯导融合的巡检机器人定位与建图技术研究", 《中国优秀博硕士学位论文全文数据库(硕士) 信息科技辑》 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2022183665A1 (zh) * 2021-03-02 2022-09-09 浙江商汤科技开发有限公司 初始化方法及装置、电子设备、存储介质和程序产品
CN114485649A (zh) * 2022-02-09 2022-05-13 北京自动化控制设备研究所 面向无人机的惯性、视觉和高度信息融合导航方法
CN114485649B (zh) * 2022-02-09 2023-09-12 北京自动化控制设备研究所 面向无人机的惯性、视觉和高度信息融合导航方法

Also Published As

Publication number Publication date
WO2022183665A1 (zh) 2022-09-09
CN113029134B (zh) 2022-04-08

Similar Documents

Publication Publication Date Title
CN113029134B (zh) 视觉惯性系统的初始化方法、装置、电子设备及存储介质
US10636168B2 (en) Image processing apparatus, method, and program
CN110084832B (zh) 相机位姿的纠正方法、装置、系统、设备和存储介质
CN110927708B (zh) 智能路侧单元的标定方法、装置及设备
WO2018142900A1 (ja) 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
JP7131994B2 (ja) 自己位置推定装置、自己位置推定方法、自己位置推定プログラム、学習装置、学習方法及び学習プログラム
US9082008B2 (en) System and methods for feature selection and matching
EP2915139B1 (en) Adaptive scale and gravity estimation
CN112230242A (zh) 位姿估计系统和方法
CN108780577A (zh) 图像处理方法和设备
EP2317280A2 (en) Uncertainty estimation of planar features
KR101985344B1 (ko) 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치
CN113066127B (zh) 一种在线标定设备参数的视觉惯性里程计方法和系统
CN110118572B (zh) 多目立体视觉与惯性导航系统及相对位姿参数确定方法
KR20180076441A (ko) 적응적 관심영역 및 탐색창을 이용한 객체 검출 방법 및 장치
CN114485641A (zh) 一种基于惯导卫导方位融合的姿态解算方法及装置
KR20200037502A (ko) 포즈 정보를 출력하는 방법 및 장치
US20130279755A1 (en) Information processing system, information processing method, and information processing program
CN114022556A (zh) 定位初始化方法、装置和计算机可读存储介质
CN114413898B (zh) 多传感器数据融合方法、装置、计算机设备及存储介质
CN110207687B (zh) 物体姿态测量方法、测量装置、电子设备及存储介质
CN112595325B (zh) 初始位置确定方法、装置、电子设备和存储介质
CN117058430B (zh) 用于视场匹配的方法、装置、电子设备和存储介质
CN110660134B (zh) 三维地图构建方法、三维地图构建装置及终端设备
US20150092985A1 (en) Updating filter parameters of a system

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
REG Reference to a national code

Ref country code: HK

Ref legal event code: DE

Ref document number: 40046345

Country of ref document: HK

GR01 Patent grant
GR01 Patent grant