CN116989775A - 基于视觉惯性里程计的位姿确定方法、装置、电子设备 - Google Patents

基于视觉惯性里程计的位姿确定方法、装置、电子设备 Download PDF

Info

Publication number
CN116989775A
CN116989775A CN202310749517.XA CN202310749517A CN116989775A CN 116989775 A CN116989775 A CN 116989775A CN 202310749517 A CN202310749517 A CN 202310749517A CN 116989775 A CN116989775 A CN 116989775A
Authority
CN
China
Prior art keywords
coordinate system
map
determining
pose
rotation matrix
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
CN202310749517.XA
Other languages
English (en)
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.)
Beijing Baidu Netcom Science and Technology Co Ltd
Original Assignee
Beijing Baidu Netcom Science and Technology 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 Beijing Baidu Netcom Science and Technology Co Ltd filed Critical Beijing Baidu Netcom Science and Technology Co Ltd
Priority to CN202310749517.XA priority Critical patent/CN116989775A/zh
Publication of CN116989775A publication Critical patent/CN116989775A/zh
Pending legal-status Critical Current

Links

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
    • 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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F16/00—Information retrieval; Database structures therefor; File system structures therefor
    • G06F16/20—Information retrieval; Database structures therefor; File system structures therefor of structured data, e.g. relational data
    • G06F16/29—Geographical information databases
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T19/00—Manipulating 3D models or images for computer graphics
    • G06T19/006—Mixed reality
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V10/00—Arrangements for image or video recognition or understanding
    • G06V10/40—Extraction of image or video features
    • G06V10/46—Descriptors for shape, contour or point-related descriptors, e.g. scale invariant feature transform [SIFT] or bags of words [BoW]; Salient regional features
    • G06V10/462—Salient features, e.g. scale invariant feature transforms [SIFT]

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Engineering & Computer Science (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Databases & Information Systems (AREA)
  • Multimedia (AREA)
  • Computer Hardware Design (AREA)
  • Software Systems (AREA)
  • Computer Graphics (AREA)
  • Data Mining & Analysis (AREA)
  • Image Analysis (AREA)

Abstract

本公开提供了一种基于视觉惯性里程计的位姿确定方法，涉及人工智能技术领域，具体涉及计算机视觉、图像处理、深度学习等技术领域，可应用于智慧城市等场景。具体实现方案为：提取当前接收的图像帧的关键特征点，并根据所述关键特征点确定所述图像帧基于视觉惯性里程计的世界坐标系的第一位姿；在预建立的视觉地图上确定与所述关键特征点在相匹配的地图特征点，其中，所述视觉地图为预建立的与所述图像帧相对应的视觉地图；根据所述关键特征点在所述图像帧的原位置和所述地图特征点投影在所述图像帧的投影位置，确定重投影误差；根据所述重投影误差更新所述第一位姿，得到所述图像帧基于视觉地图的地图坐标系的第二位姿。

Description

基于视觉惯性里程计的位姿确定方法、装置、电子设备
技术领域
本公开涉及人工智能技术领域，具体涉及计算机视觉、图像处理、深度学习等技术领域，可应用于智慧城市等场景。
背景技术
视觉惯性里程计(Visual-Inertial Odometry，VIO)包括相机部件和惯性部件，相机部件输入图像，惯性部件输入惯性运动数据，用来持续跟踪用户的位姿，通常会利用图像序列中的稀疏特征点，并计算这些稀疏特征点在三维空间中的位置。但是因为VIO输出的位姿定义在局部坐标系(世界坐标系)下，长时间跟踪后会发生轨迹漂移，即：用户回到原先的起点后，所估计的位姿和起点不重合，会导致在虚拟现实场景中，虚拟物体不贴合的情况。
发明内容
本公开提供了一种用于解决上述技术问题中的至少一项的基于视觉惯性里程计的位姿确定方法、装置、电子设备。
根据本公开的一方面，提供了一种基于视觉惯性里程计的位姿确定方法，所述方法包括：
提取当前接收的图像帧的关键特征点，并根据所述关键特征点确定所述图像帧基于视觉惯性里程计的世界坐标系的第一位姿；
在预建立的视觉地图上确定与所述关键特征点在相匹配的地图特征点，其中，所述视觉地图为预建立的与所述图像帧相对应的视觉地图；
根据所述关键特征点在所述图像帧的原位置和所述地图特征点投影在所述图像帧的投影位置，确定重投影误差；
根据所述重投影误差更新所述第一位姿，得到所述图像帧基于视觉地图的地图坐标系的第二位姿。
根据本公开的另一方面，提供了一种基于视觉惯性里程计的位姿确定装置，所述装置包括：
关键点提取模块，用于提取当前接收的图像帧的关键特征点，并根据所述关键特征点确定所述图像帧基于视觉惯性里程计的世界坐标系的第一位姿；
地图特征点确定模块，用于在预建立的视觉地图上确定与所述关键特征点在相匹配的地图特征点，其中，所述视觉地图为预建立的与所述图像帧相对应的视觉地图；
重投影误差确定模块，用于根据所述关键特征点在所述图像帧的原位置和所述地图特征点投影在所述图像帧的投影位置，确定重投影误差；
位姿确定模块，用于根据所述重投影误差更新所述第一位姿，得到所述图像帧基于视觉地图的地图坐标系的第二位姿。
根据本公开的另一方面，提供了一种电子设备，包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执上述方法。
根据本公开的另一方面，提供了一种存储有计算机指令的非瞬时计算机可读存储介质，其中，所述计算机指令用于使所述计算机执行根据上述方法。
根据本公开的另一方面，提供了一种计算机程序产品，包括计算机程序，所述计算机程序在被处理器执行时实现根据上述方法。
应当理解，本部分所描述的内容并非旨在标识本公开的实施例的关键或重要特征，也不用于限制本公开的范围。本公开的其它特征将通过以下的说明书而变得容易理解。
附图说明
附图用于更好地理解本方案，不构成对本公开的限定。其中：
图1是本公开第一实施例提供的一种基于视觉惯性里程计的位姿确定方法的流程示意图；
图2是本公开第二实施例提供的一种基于视觉惯性里程计的位姿确定方法的流程示意图；
图3是本公开第三实施例提供的一种基于视觉惯性里程计的位姿确定装置的结构示意图；
图4是用来实现本公开实施例的方法的电子设备的框图。
具体实施方式
以下结合附图对本公开的示范性实施例做出说明，其中包括本公开实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本公开的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
在不冲突的情况下，本公开各实施例及实施例中的各特征可相互组合。
如本文所使用的，术语“和/或”包括一个或多个相关列举条目的任何和所有组合。
本文所使用的术语仅用于描述特定实施例，且不意欲限制本公开。如本文所使用的，单数形式“一个”和“该”也意欲包括复数形式，除非上下文另外清楚指出。
除非另外限定，否则本文所用的所有术语(包括技术和科学术语)的含义与本领域普通技术人员通常理解的含义相同。还将理解，诸如那些在常用字典中限定的那些术语应当被解释为具有与其在相关技术以及本公开的背景下的含义一致的含义，且将不解释为具有理想化或过度形式上的含义，除非本文明确如此限定。
根据本公开的车辆安全资产的文件夹的复制方法可以由终端设备或服务器等电子设备执行，终端设备可以为车载设备、用户设备(User Equipment，UE)、移动设备、用户终端、终端、蜂窝电话、无绳电话、个人数字助理(Personal Digital Assistant，PDA)、手持设备、计算设备、车载设备、可穿戴设备等，方法可以通过处理器调用存储器中存储的计算机可读程序指令的方式来实现。或者，可通过服务器执行本公开提供的方法。
为了便于说明，以下首先对本公开涉及的技术名词进行解释。
视觉惯性里程计：由相机部件和惯性部件(例如：惯性测量单元(Inertialmeasurement unit，IMU))所组成的视觉传感设备；相机部件采集图像(包含多帧图像帧)，IMU输入惯性数据，例如：陀螺仪和加速度计的偏置。VIO的任务是估算相邻图像帧间相机部件的运动，以及局部地图的地图形貌。基于特征点的VIO在估算相邻图像间相机的运动时，首先提取前后两个关键帧的特征点，然后做特征点匹配，匹配好特征点之后，再根据匹配好的特征点确定表示运动的位姿(即得到两图像帧之间的选择矩阵和平移位置，即实现了视觉惯性里程计)。
IMU：是用于测量物体的三轴姿态角(或角速率)以及加速度的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计用于检测物体在三维坐标系中每个坐标轴上的加速度信号，进而计算得到位移向量；而陀螺用于检测物体在三维坐标系中的旋转矩阵。可选地，IMU包括陀螺仪、加速度计和地磁传感器。
位姿(6D位姿)：位置和姿态，是估计从一个坐标系(例如：世界坐标系G)到另一坐标系(例如：相机坐标系C)的刚性转换，包括三维旋转R(根据物体的表面纹理信息影响物体外观)和三维平移P(P决定物体在图片中的位置和比例)。
扩展卡尔曼滤波器(ExtendedKalman Filter，EKF)：卡尔曼滤波器是一种高效率的递归滤波器，它能够从一系列的不完全及包含噪音的测量中，估计动态系统的状态。在状态方程或测量方程为非线性时，通常采用扩展卡尔曼滤波器(EKF)估计动态系统的状态。EKF对非线性函数的泰勒展开式进行一阶线性化截断，忽略其余高阶项，从而将非线性问题转化为线性，可以将卡尔曼线性滤波算法应用于非线性系统中。
漂移误差：受传感器测量噪声影响，先前时刻的估计误差，会累加到后面时间的运动之上，这种现象称为漂移，形成的误差叫做漂移误差。
在公开第一实施例中，参见图1，图1示出本公开第一实施例提供的一种基于视觉惯性里程计的位姿确定方法的流程示意图。该方法包括：
S101、提取当前接收的图像帧的关键特征点，并根据关键特征点确定图像帧基于视觉惯性里程计的世界坐标系的第一位姿。
提取当前接收的图像帧的关键特征点的方法有很多，例如：采用预训练的特征点提取模型或者，采用旋转不变二进制鲁棒独立基本特征(Oriented FAST and RotatedBRIEF，ORB)算法对输入的图像帧进行特征点提取和描述，特征点提取采用改进的基于加速分割测试的特征(Features from Accelerated Segment Test，FAST)算法实现，特征点描述采用二进制鲁棒独立基本特征(Binary Robust IndependentElementary Features，BRIEF)算法实现进行特征描述，在此不做限定。
需要说明的是，本公开包括四个基础坐标系，即相机部件采集的图像帧的相机坐标系C、IMU的惯性坐标系I、VIO的世界坐标系G和本公开预建立的视觉地图的地图坐标系W，以下任一参数的表达式的角中C、I、G、W如上述含义；而位姿T[R，P]中，R表示旋转矩阵，P表示平移位置，位姿还可以包括v,v表示IMU输出的速度，相应地，以下任一R、P、v的角标包括的I、C、G、W即代表不同坐标系之间的转换，例如，或表示地图坐标系和相机坐标系之间的转换；或表示地图坐标系和世界坐标系之间的转换；或表示相机坐标系和惯性坐标系之间的转换；或表示惯性坐标系到世界地图坐标系之间的转换，例如：第一位姿表示为和，和即为VIO基于世界坐标系G的输出(即未经视觉地图W修正的VIO输出)；第二位姿表示位和，和表示VIO的输出变换到地图坐标系后的第二位姿。。
在S101中，根据关键特征点确定图像帧基于视觉惯性里程计的世界坐标系的第一位姿，第一位姿表示为TIG[RIG,pIG]。
S102、在预建立的视觉地图上确定与关键特征点在相匹配的地图特征点。
其中，视觉地图为预建立的与图像帧相对应的视觉地图。预先根据装载相机部件的设备的活动区域，生成与活动区域的环境信息相匹配的视觉地图，视觉地图中每个点的坐标都是确定的，从而能够通过位置确定的视觉地图的点与图像帧中的点的对应关系，去修正运动轨迹的漂移。。
S102的目的为将图像帧定位到视觉地图中，具体地，采用图像搜索、特征匹配、位姿求解等步骤，即可以确定相机坐标系在视觉地图坐标系中的位姿TCW，TCW＝[RCW，pCW]，位姿TCW＝[RCW，pCW]也可即定位信息，其中，RCW表示地图坐标系到相机坐标系之间的第六旋转矩阵，pCW表示地图坐标系到相机坐标系之间的第六平移位置，具体地表示地图坐标系的中心在相机坐标系的位置，后续，可根据第六旋转矩阵和第六平移位置。确定图像帧在地图坐标系的第五平移位置pwc，即：pWC＝-RCW TpCW；并且，图像帧在地图坐标系的第五旋转矩阵与第六旋转矩阵相同，即为RCW。
S103、根据关键特征点在图像帧的原位置和地图特征点投影在图像帧的投影位置，确定重投影误差。
关键特征点在图像帧的原位置，即关键特征点在图像帧的像素位置，图像帧上每个像素点都具有像素坐标信息，因此根据关键特征点的像素坐标信息及可确定像素位置(原位置)；投影位置：先确定地图特征点投影到相机坐标系的空间位置，再根据空间位置和预设的相机部件的相机内参矩阵，确定地图特征点投影在图像帧的投影位置；确定原位置和投影位置后，即可确定二者之间的距离(例如：欧式距离)，以距离作为重投影误差。S103的目的为：以关键特征点对应的地图特征点，确定其与当前测量得到的关键特征点的距离，以确定漂移量。
S104、根据重投影误差更新第一位姿，得到图像帧基于视觉地图的地图坐标系的第二位姿。
根据重投影误差确定第一位姿的更新量，再基于更新量更新第一位姿，得到基于视觉地图的地图坐标系修正漂移后的第二位姿。
采用这种方式，首先预先建立与装载相机部件的设备的行动区域相匹配的视觉地图，以提供位置不变且坐标确定的视觉地图特征点，从而在运动过程中，能够就视觉地图约束当前的位姿，根据约束确定的重投影误差，去修正第一位姿得到第二位姿，从而能够消除VIO输出的位姿(即第一位姿)的漂移，得到修正后的准确表示运动状态的位姿(即第二位姿)。
在公开第二实施例中，参见图2，图2示出本公开第二实施例提供的一种基于视觉惯性里程计的位姿确定方法的流程示意图。该方法包括：
S201、提取当前接收的图像帧的关键特征点，并根据关键特征点确定图像帧基于视觉惯性里程计的世界坐标系的第一位姿。
提取当前接收的图像帧的关键特征点的方法有很多，例如：采用预训练的特征点提取模型或者，采用旋转不变二进制鲁棒独立基本特征(Oriented FAST and RotatedBRIEF，ORB)算法对输入的图像帧进行特征点提取和描述，特征点提取采用改进的基于加速分割测试的特征(Features from Accelerated Segment Test，FAST)算法实现，特征点描述采用二进制鲁棒独立基本特征(Binary Robust IndependentElementary Features，BRIEF)算法实现进行特征描述，在此不做限定。
在S201中，根据关键特征点确定图像帧基于视觉惯性里程计的世界坐标系的第一位姿，第一位姿表示为TIG[RIG,pIG]。
S202、通过特征匹配，确定视觉地图上与关键特征点相匹配的地图特征点。
S203、确定地图坐标系到相机坐标系之间的第六旋转矩阵和第六平移位置。
其中，视觉地图为预建立的与图像帧相对应的视觉地图。预先建立装载相机部件的设备的行动区域的视觉地图，以提供位置确定的视觉地图特征点，在运动过程中，可以根据行动轨迹，按照多种方式更新行动区域对应的视觉地图。
S202和S203的目的为将图像帧定位到视觉地图中，具体地，采用图像搜索、特征匹配、位姿求解等步骤，即可以确定相机坐标系在视觉地图坐标系中的位姿TCW，TCW＝[RCW，pCW]，位姿TCW＝[RCW，pCW]也可即定位信息，其中，RCW表示地图坐标系到相机坐标系之间的第六旋转矩阵，pCW表示地图坐标系到相机坐标系之间的第六平移位置，具体地表示地图坐标系的中心在相机坐标系的位置。
其中，S203具体包括：根据第六旋转矩阵、第五平移位置确定第六平移位置，其中，第六旋转矩阵与第五旋转矩阵相同。
可根据第六旋转矩阵和第六平移位置，确定图像帧在地图坐标系的第五平移位置pwc，即：pWC＝-RCW TpCW；并且，图像帧在地图坐标系的第五旋转矩阵与第六旋转矩阵相同，即为RCW。
需要说明的是，S202和S203为S102的一种实现方式，当前，S102还具有其他实现方式，在此不做限定。
S204、确定第一位姿对应的坐标系和地图坐标系之间的第二旋转矩阵、第二平移位置和第二速度。
其中，第一位姿对应的坐标系的旋转矩阵(即第三旋转矩阵RIG)和平移位置(即第三平移位置pIG)，也即VIO未经视觉地图修正的位姿输出的第一位姿TIG[RIG,pIG]。S204的目的为将基于世界地图G输出的VIO的坐标系的第一位姿TIG[RIG,pIG]，转换为基于地图坐标系W生成的第二旋转矩阵RIW、第二平移位置pIW和第二速度vIW，其中，第二旋转矩阵RIW、第二平移位置pIW和第二速度vIW也称为状态向量，则S204的目的为：将常规VIO的状态向量转换到地图坐标系W中。
在一些示例中，S204包括：
S2041、根据世界坐标系和地图坐标系之间的第四旋转矩阵、第一位姿对应的坐标系的第三旋转矩阵确定第二旋转矩阵。
具体按照如下公式确定第二旋转矩阵RIW：
RIW＝RIGRGW (1)
其中，RIG为第一位姿对应的坐标系的第三旋转矩阵；RGW为世界坐标系和地图坐标系之间的第四旋转矩阵。
S2042、根据世界坐标系和地图坐标系之间的第四旋转矩阵和第四平移位置、第一位姿对应的坐标系的第三平移位置确定第二平移位置。
具体按照如下公式确定第二平移位置pWI：
pWI＝RGW TpGI+pWG (2)
其中，RGW为世界坐标系和地图坐标系之间的第四旋转矩阵；pWG为世界坐标系和地图坐标系之间的第四平移位置；pGI为第一位姿对应的坐标系的第三平移位置。
S2043、根据世界坐标系和地图坐标系之间的第四旋转矩阵、第一位姿对应的坐标系的第三速度，确定第二速度。
具体按照如下公式确定第二速度vWI：
vWI＝RGW TvGI (3)
其中，RGW为世界坐标系和地图坐标系之间的第四旋转矩阵；vGI为第一位姿对应的坐标系的第三速度。
在一些示例中，第四旋转矩阵RGW按照如下方式获得：
根据第三旋转矩阵、第一旋转矩阵、图像帧到地图坐标系的第五旋转矩阵确定第四旋转矩阵。
具体按照如下公式确定第四旋转矩阵RGW：
RGW＝RIG TRCI TRCW (4)
其中，RIG为第一位姿对应的坐标系的第三旋转矩阵；RCI为相机坐标系和惯性坐标系之间的第一旋转矩阵；RCW为图像帧到地图坐标系的第五旋转矩阵。
在一些示例中，第四平移位置按照如下方式获得：
根据第五旋转矩阵、第一旋转矩阵和第一平移位置、第三旋转矩阵和第三平移位置、图像帧到地图坐标系的第五平移位置确定第四平移位置。
具体按照如下公式确定第四平移位置RWG：
pWG＝-RCW TRCIRIGpGI+RCW TpCI+pWC (5)
其中，RCW为图像帧到地图坐标系的第五旋转矩阵；RCI、pCI为相机坐标系和惯性坐标系之间的第一旋转矩阵和第一平移位置；RIG、pGI为第一位姿对应的坐标系的第三旋转矩阵和第三平移位置、pWC为图像帧到地图坐标系的第五平移位置。
S2044、根据第一位姿对应的坐标系的第一协方差矩阵、第一雅可比矩阵和预设的第一噪声参数，确定地图坐标系对应的第二协方差矩阵。
其中，第一噪声参数用于消除S202和S203中确定投影位置的位置误差。
具体按照如下公式确定第二协方差矩阵pxx_new：
Pxx_new＝ΦPxx_oldΦT+Q (6)
其中，pxx_old为第一位姿对应的坐标系的第一协方差矩阵，其按照EKF标标准协方差矩阵的算法生成；φ为第一雅可比矩阵；Q为预设的第一噪声参数。
在一些情况下，例如非EKF滤波的算法中，可以省略S044，即只用更新状态向量(包括旋转矩阵、平移位置、速度)，而不用计算协方差矩阵，在此不做限定。
进一步地，将式(4)和式(5)代入式(1)(2)(3)，得到式(1.1)(2.2)(3.3)：
RIW＝RCI TRCW (1.1)
pWI＝RCW TpCI+pWC (2.2)
vWI＝RCW TRCIRIGvGI (3.3)
根据上述可知，因RCW，pWC是图像帧在地图坐标系W的定位结果，是已知的，因此可省略，从而可知：RIW的协方差只与RCI的协方差相关；pWI的协方差只与pCI的协方差相关；vWI与RCI，RIG，vGI相关。
在式(6)中，第一协方差矩阵为原变量xold(基于世界坐标系G的VIO变量)的协方差矩阵，xold＝[RIG pGI vGI RCI pCI]，即除定位机构以外的相关量；第二协方差矩阵为新变量xnew的协方差矩阵，xnew＝[RIW pWI vWI]。第一雅可比矩阵即xnew对xold的雅可比矩阵。
其中，第一雅可比矩阵按照如下方式计算：
子步骤A、确定第二旋转矩阵RIW对第一旋转矩阵RCI的第一子雅可比。
参见式(1.1)，RIW的协方差只与RCI的协方差相关，因此第一子雅可比按照下式计算：
子步骤B、确定第二平移位置pWI对第一平移位置pCI的第二子雅可比。
参见式(2.2)，pWI的协方差只与pCI的协方差相关，因此第二子雅可比按照下式计算：
参见式(3.3)，vWI与RCI，RIG，vGI相关，因此：
子步骤C1、确定第二速度vWI对第一旋转矩阵RCI的第三子雅可比。
第三子雅可比按照下式计算：
其中，×的运算符号表示：将括号内相乘的结果向量变为反对称矩阵。
子步骤C2、确定第二速度vWI对第一位姿对应的坐标系的第三旋转矩阵RIG的第四子雅可比。
第四子雅可比按照下式计算：
子步骤C3、确定第二速度vWI对第一位姿对应的坐标系的第三速度vGI的第五子雅可比。
第五子雅可比按照下式计算：
子步骤D、根据第一子雅可比、第二子雅可比、第三子雅可比、第四子雅可比和第五子雅可比确定第一雅可比矩阵。
将xnew对xold的第一雅可比矩阵可写为分块矩阵形式：
式(7.6)计算出的φ代入式(6)。
S205、根据相机部件对应的相机坐标系和惯性部件对应的惯性坐标系之间的第一旋转矩阵和第一平移位置、第一位姿对应的坐标系和地图坐标系之间的第二旋转矩阵和第二平移位置，确定地图特征点投影到相机坐标系的空间位置。
具体地，按照下式确定所述空间位置：
其中，为地图特征点投影到相机坐标系的空间位置的三个向量；WPk表示地图特征点在地图坐标系的位置；其中，角标k表示第k图像帧，k为大于等于1的整数，/>表示姿态四元数，根据姿态四元数可表示旋转矩阵，即第二旋转矩阵表示为/>
S206、根据投影到相机坐标系的空间位置和预设的相机内参矩阵，确定地图特征点投影在图像帧的投影位置。
具体地，按照下式确定所述投影位置：
其中，特征点i(地图特征点或关键特征点)在第k帧图像上的位置表示为：
其中，为相机内参矩阵，其中，fx、fy为相机内参矩阵的焦距；cx、cy为相机内参的主点，将式(8)代入(9)。
综合式(8)和(9)可知，重投影误差项可以直接约束EKF更新变量中的和/>即直接约束转换到地图坐标的姿态和位置。
S207、根据原位置和投影位置，确定重投影误差。
具体地，按照下式确定所述重投影误差：
ere-pro＝pk+pk' (10)
其中，pk是关键特征点在图像帧的原位置；pk'是关键特征点对应的地图特征点投影到图像帧上的投影位置。
其中，S205-S207为S103的一种实现方式，当前，S103还具有其他实现方式，在此不做限定。
S208、根据重投影误差更新包含第一位姿的第一扩展卡尔曼滤波变量，得到包含第二位姿的第二扩展卡尔曼滤波变量。
其中，S208为S104的一种实现方式，当前，S104还可以采取其他滤波方式，在此不做限定。在基于EKF的框架中，根据扩展卡尔曼滤波变量表示位姿，具体地，需要基于视觉地图的修正更新EKF变量，并且还需要更新EKF变量的协方差矩阵。
其中，第一位姿的第一扩展卡尔曼滤波变量按照常规扩展卡尔曼滤波算法确定，后续基于重投影误差修正该第一扩展卡尔曼滤波变量，得到包含第二位姿的第二扩展卡尔曼滤波变量。
需要说明的是，第二扩展卡尔曼滤波变量xekf的定义按照如下：
ximu＝[qIW，bg，vWI，ba，pWI] (11.2)
其中，qIW表示姿态四元数(基于地图坐标系W的)，vWI为第二速度，qWI为第二平移位置，bg，ba分别表示陀螺仪和加速度计的偏置。
并且，第二扩展卡尔曼滤波变量xekf包含过去M帧图像的位姿(表示为姿态四元素和平移位置)。
在一些示例中，S208具体包括：
S2081、确定针对重投影误差的卡尔曼增益。
在一些示例中，S2081包括：
根据第二协方差矩阵、第二雅可比矩阵、预设的第二噪声参数确定卡尔曼增益。
其中，第二协方差矩阵pxx_new上述式(6)确定；第二雅可比矩阵表示重投影误差对第二旋转矩阵ere-pro对应的姿态四元数和第二平移位置(的雅可比矩阵；第二噪声参数用于消除提取关键特征点的测量误差。
具体地，按照下式确定所述卡尔曼增益Kk：
其中，Hk为第二雅可比矩阵；Rk为第二噪声参数；为式(6)中的第二协方差矩阵。
S2082、根据卡尔曼增益和重投影误差确定卡尔曼滤波变量的增量。
具体地，按照下式确定所述卡尔曼滤波变量的增量：
δxk＝Kkere-pro (13)
S2083、根据卡尔曼滤波变量的增量和第一扩展卡尔曼滤波变量，确定第二扩展卡尔曼滤波变量。
具体地，按照下式确定所述第二扩展卡尔曼滤波变量：
xk＝xk+δxk (14)
其中，等式左侧的xk为更新后的第二扩展卡尔曼滤波变量，右侧的xk为更新前的第一扩展卡尔曼滤波变量。
S209、更新第二扩展卡尔曼滤波变量对应的第二协方差矩阵，得到更新后的第二协方差矩阵。
需要说明的是，在一些实现方式中，例如不采用EKF的方式中，可以省略S209，在此不做限定。
在以下示例中，S209包括：
根据卡尔曼增益、第二扩展卡尔曼滤波变量对应的第二协方差矩阵、单位矩阵、第二雅可比矩阵、预设的第二噪声参数，计算更新后的第二协方差矩阵。
具体地，按照下式确定所述更新后的第二协方差矩阵：
P′xx_new＝(I-KkHk)Pxx_new(I-KkHk)T+KkRkKk T (15)
其中，第二协方差矩阵pxx_new上述式(6)确定。
综上所述，采用上述方式，先将图像帧定位到视觉地图的地图坐标中，在基于此得到转化到地图坐标中的状态向量和第二协方差矩阵，再基于状态向量得到重投影误差，从而基于重投影误差约束第二扩展卡尔曼滤波变量的更新，并约束第二协方差矩阵的更新，在EKF方式的VIO中，基于扩展卡尔曼滤波变量和协方差矩阵确定位姿，从而能够基于重投影误差约束第一位姿的漂移，得到消除漂移的第二位姿。
在公开第三实施例中，基于与图1相同的原理，图3示出本公开第三实施例提供的基于视觉惯性里程计的位姿确定装置30，该装置包括：
关键点提取模块301，用于提取当前接收的图像帧的关键特征点，并根据关键特征点确定图像帧基于视觉惯性里程计的世界坐标系的第一位姿；
地图特征点确定模块302，用于在预建立的视觉地图上确定与关键特征点在相匹配的地图特征点，其中，视觉地图为预建立的与图像帧相对应的视觉地图；
重投影误差确定模块303，用于根据关键特征点在图像帧的原位置和地图特征点投影在图像帧的投影位置，确定重投影误差；
位姿确定模块304，用于根据重投影误差更新第一位姿，得到图像帧基于视觉地图的地图坐标系的第二位姿。
在一些示例中，其中，视觉惯性里程计包括相机部件和惯性部件；
重投影误差确定模块具体用于：
根据相机部件对应的相机坐标系和惯性部件对应的惯性坐标系之间的第一旋转矩阵和第一平移位置、第一位姿对应的坐标系和地图坐标系之间的第二旋转矩阵和第二平移位置，确定地图特征点投影到相机坐标系的空间位置；
根据空间位置和预设的相机内参矩阵，确定地图特征点投影在图像帧的投影位置；
根据原位置和投影位置，确定重投影误差。
在一些示例中，该装置还包括：
第一确定模块，用于确定第一位姿对应的坐标系和地图坐标系之间的第二旋转矩阵、第二平移位置和第二速度。
在一些示例中，第一确定模块具体用于：
根据世界坐标系和地图坐标系之间的第四旋转矩阵、第一位姿对应的坐标系的第三旋转矩阵确定第二旋转矩阵；
根据世界坐标系和地图坐标系之间的第四旋转矩阵和第四平移位置、第一位姿对应的坐标系的第三平移位置确定平移位置；
根据世界坐标系和地图坐标系之间的第四旋转矩阵、第一位姿对应的坐标系的第三速度，确定第二速度。
在一些示例中，第四旋转矩阵按照如下方式获得：
根据第三旋转矩阵、第一旋转矩阵、图像帧到地图坐标系的第五旋转矩阵确定第四旋转矩阵；
第四平移位置按照如下方式获得：
根据第五旋转矩阵、第一旋转矩阵和第一平移位置、第三旋转矩阵和第三平移位置、图像帧到地图坐标系的第五平移位置确定第四平移位置。
在一些示例中，该装置还包括：
第一协方差矩阵，用于根据第一位姿对应的坐标系的第一协方差矩阵、第一雅可比矩阵和预设的第一噪声参数，确定地图坐标系对应的第二协方差矩阵；
其中，第一噪声参数用于消除投影位置的位置误差。
在一些示例中，第一雅可比矩阵按照如下方式计算：
确定第二旋转矩阵对第一旋转矩阵的第一子雅可比；
确定第二平移位置对第一平移位置的第二子雅可比；
确定第二速度对第一旋转矩阵的第三子雅可比；
确定第二速度对第一位姿对应的坐标系的第三旋转矩阵的第四子雅可比；
确定第二速度对第一位姿对应的坐标系的第三速度的第五子雅可比；
根据第一子雅可比、第二子雅可比、第三子雅可比、第四子雅可比和第五子雅可比确定第一雅可比矩阵。
在一些示例中，位姿确定模块304包括：
卡尔曼滤波模块，用于根据重投影误差更新包含第一位姿的第一扩展卡尔曼滤波变量，得到包含第二位姿的第二扩展卡尔曼滤波变量。
在一些示例中，卡尔曼滤波模块具体用于：
确定针对重投影误差的卡尔曼增益；
根据卡尔曼增益和重投影误差确定卡尔曼滤波变量的增量；
根据卡尔曼滤波变量的增量和第一扩展卡尔曼滤波变量，确定第二扩展卡尔曼滤波变量。
在一些示例中，卡尔曼滤波模块具体用于：
根据第二协方差矩阵、第二雅可比矩阵、预设的第二噪声参数确定卡尔曼增益；
其中，第二协方差矩阵根据权利要求6或7的方法确定；第二雅可比矩阵表示重投影误差对第二旋转矩阵对应的姿态四元数和第二平移位置的雅可比矩阵；第二噪声参数用于消除提取关键特征点的测量误差。
在一些示例中，本装置还包括：
卡尔曼协方差模块，用于更新第二扩展卡尔曼滤波变量对应的第二协方差矩阵，得到更新后的第二协方差矩阵。
在一些示例中，卡尔曼协方差模块具体用于：
根据卡尔曼增益、第二扩展卡尔曼滤波变量对应的第二协方差矩阵、单位矩阵、第二雅可比矩阵、预设的第二噪声参数，计算更新后的第二协方差矩阵。
在一些示例中，地图特征点确定模块302具体用于：
通过特征匹配，确定视觉地图上与关键特征点相匹配的地图特征点；
确定地图坐标系到相机坐标系之间的第六旋转矩阵和第六平移位置。
在一些实力中，地图特征点确定模块302具体用于：
根据第六旋转矩阵、第五平移位置确定第六平移位置，其中，第六旋转矩阵与第五旋转矩阵相同。
本公开的技术方案中，所涉及的用户个人信息的获取，存储和应用等，均符合相关法律法规的规定，且不违背公序良俗。
根据本公开的实施例，本公开还提供了一种电子设备、一种可读存储介质和一种计算机程序产品。
图4示出了可以用来实施本公开的实施例的示例电子设备400的示意性框图。电子设备旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本公开的实现。
如图4所示，设备400包括计算单元401，其可以根据存储在只读存储器(ROM)402中的计算机程序或者从存储单元403加载到随机访问存储器(RAM)403中的计算机程序，来执行各种适当的动作和处理。在RAM 403中，还可存储设备400操作所需的各种程序和数据。计算单元401、ROM 402以及RAM 402通过总线404彼此相连。输入/输出(I/O)接口405也连接至总线404。
设备400中的多个部件连接至I/O接口405，包括：输入单元406，例如键盘、鼠标等；输出单元407，例如各种类型的显示器、扬声器等；存储单元408，例如磁盘、光盘等；以及通信单元409，例如网卡、调制解调器、无线通信收发机等。通信单元409允许设备400通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
计算单元401可以是各种具有处理和计算能力的通用和/或专用处理组件。计算单元401的一些示例包括但不限于中央处理单元(CPU)、图形处理单元(GPU)、各种专用的人工智能(AI)计算芯片、各种运行机器学习模型算法的计算单元、数字信号处理器(DSP)、以及任何适当的处理器、控制器、微控制器等。计算单元401执行上文所描述的各个方法和处理，例如基于视觉惯性里程计的位姿确定方法。例如，在一些实施例中，基于视觉惯性里程计的位姿确定方法可被实现为计算机软件程序，其被有形地包含于机器可读介质，例如存储单元408。在一些实施例中，计算机程序的部分或者全部可以经由ROM 402和/或通信单元409而被载入和/或安装到设备400上。当计算机程序加载到RAM 403并由计算单元401执行时，可以执行上文描述的基于视觉惯性里程计的位姿确定方法的一个或多个步骤。备选地，在其他实施例中，计算单元401可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行基于视觉惯性里程计的位姿确定方法。
本文中以上描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、现场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、负载可编程逻辑设备(CPLD)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
用于实施本公开的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
在本公开的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
为了提供与用户的交互，可以在计算机上实施此处描述的系统和技术，该计算机具有：用于向用户显示信息的显示装置(例如，CRT(阴极射线管)或者LCD(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如，作为数据服务器)、或者包括中间件部件的计算系统(例如，应用服务器)、或者包括前端部件的计算系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(LAN)、广域网(WAN)和互联网。
计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，也可以为分布式系统的服务器，或者是结合了区块链的服务器。
应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本公开中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本公开公开的技术方案所期望的结果，本文在此不进行限制。
上述具体实施方式，并不构成对本公开保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本公开的精神和原则之内所作的修改、等同替换和改进等，均应包含在本公开保护范围之内。

Claims (20)

1.一种基于视觉惯性里程计的位姿确定方法，所述方法包括：
提取当前接收的图像帧的关键特征点，并根据所述关键特征点确定所述图像帧基于视觉惯性里程计的世界坐标系的第一位姿；
在预建立的视觉地图上确定与所述关键特征点在相匹配的地图特征点，其中，所述视觉地图为预建立的与所述图像帧相对应的视觉地图；
根据所述关键特征点在所述图像帧的原位置和所述地图特征点投影在所述图像帧的投影位置，确定重投影误差；
根据所述重投影误差更新所述第一位姿，得到所述图像帧基于视觉地图的地图坐标系的第二位姿。
2.根据权利要求1所述的方法，其中，所述视觉惯性里程计包括相机部件和惯性部件；
所述根据所述关键特征点在所述图像帧的原位置和所述地图特征点投影在所述图像帧的投影位置，确定重投影误差，包括：
根据所述相机部件对应的相机坐标系和所述惯性部件对应的惯性坐标系之间的第一旋转矩阵和第一平移位置、所述第一位姿对应的坐标系和所述地图坐标系之间的第二旋转矩阵和第二平移位置，确定所述地图特征点投影到相机坐标系的空间位置；
根据所述空间位置和预设的相机内参矩阵，确定所述地图特征点投影在所述图像帧的投影位置；
根据所述原位置和所述投影位置，确定所述重投影误差。
3.根据权利要求2所述的方法，其中，所述在预建立的视觉地图上确定与所述关键特征点在相匹配的地图特征点之后，所述根据所述关键特征点在所述图像帧的原位置和所述地图特征点投影在所述图像帧的投影位置，确定重投影误差之前，所述方法还包括：
确定所述第一位姿对应的坐标系和所述地图坐标系之间的第二旋转矩阵、第二平移位置和第二速度。
4.根据权利要求3所述的方法，其中，所述确定所述第一位姿对应的坐标系和所述地图坐标系之间的第二旋转矩阵、第二平移位置和第二速度，包括：
根据所述世界坐标系和所述地图坐标系之间的第四旋转矩阵、所述第一位姿对应的坐标系的第三旋转矩阵确定所述第二旋转矩阵；
根据所述世界坐标系和所述地图坐标系之间的第四旋转矩阵和第四平移位置、所述第一位姿对应的坐标系的第三平移位置确定所述第二平移位置；
根据所述世界坐标系和所述地图坐标系之间的第四旋转矩阵、所述第一位姿对应的坐标系的第三速度，确定所述第二速度。
5.根据权利要求4所述的方法，其中，所述第四旋转矩阵按照如下方式获得：
根据所述第三旋转矩阵、所述第一旋转矩阵、所述图像帧到所述地图坐标系的第五旋转矩阵确定所述第四旋转矩阵；
所述第四平移位置按照如下方式获得：
根据所述第五旋转矩阵、所述第一旋转矩阵和所述第一平移位置、所述第三旋转矩阵和所述第三平移位置、所述图像帧到所述地图坐标系的第五平移位置确定所述第四平移位置。
6.根据权利要求3-5任一所述的方法，其中，所述确定所述第一位姿对应的坐标系和所述地图坐标系之间的第二旋转矩阵、第二平移位置和第二速度之后，所述方法还包括：
根据所述第一位姿对应的坐标系的第一协方差矩阵、第一雅可比矩阵和预设的第一噪声参数，确定所述地图坐标系对应的第二协方差矩阵；
其中，所述第一噪声参数用于消除所述投影位置的位置误差。
7.根据权利要求6所述的方法，其中，所述第一雅可比矩阵按照如下方式计算：
确定所述第二旋转矩阵对所述第一旋转矩阵的第一子雅可比；
确定所述第二平移位置对所述第一平移位置的第二子雅可比；
确定所述第二速度对所述第一旋转矩阵的第三子雅可比；
确定所述第二速度对所述第一位姿对应的坐标系的第三旋转矩阵的第四子雅可比；
确定所述第二速度对所述第一位姿对应的坐标系的第三速度的第五子雅可比；
根据所述第一子雅可比、所述第二子雅可比、所述第三子雅可比、所述第四子雅可比和所述第五子雅可比确定所述第一雅可比矩阵。
8.根据权利要求1-7任一所述的方法，其中，所述根据所述重投影误差更新所述第一位姿，得到所述图像帧基于视觉地图的地图坐标系的第二位姿，包括：
根据所述重投影误差更新包含所述第一位姿的第一扩展卡尔曼滤波变量，得到包含所述第二位姿的第二扩展卡尔曼滤波变量。
9.根据权利要求8所述的方法，其中，根据所述重投影误差更新包含所述第一位姿的第一扩展卡尔曼滤波变量，得到包含所述第二位姿的第二扩展卡尔曼滤波变量，包括：
确定针对所述重投影误差的卡尔曼增益；
根据卡尔曼增益和所述重投影误差确定卡尔曼滤波变量的增量；
根据所述卡尔曼滤波变量的增量和所述第一扩展卡尔曼滤波变量，确定所述第二扩展卡尔曼滤波变量。
10.根据权利要求9所述的方法，其中，所述确定针对所述重投影误差的卡尔曼增益，包括：
根据第二协方差矩阵、第二雅可比矩阵、预设的第二噪声参数确定所述卡尔曼增益；
其中，所述第二协方差矩阵根据权利要求6或7所述的方法确定；所述第二雅可比矩阵表示所述重投影误差对第二旋转矩阵对应的姿态四元数和第二平移位置的雅可比矩阵；所述第二噪声参数用于消除提取所述关键特征点的测量误差。
11.根据权利要求8-10任一所述的方法，其中，所述根据所述重投影误差更新包含所述第一位姿的第一扩展卡尔曼滤波变量，得到包含所述第二位姿的第二扩展卡尔曼滤波变量之后，所述方法还包括：
更新所述第二扩展卡尔曼滤波变量对应的第二协方差矩阵，得到更新后的第二协方差矩阵。
12.根据权利要求11所述的方法，其中，更新所述第二扩展卡尔曼滤波变量对应的第二协方差矩阵，得到更新后的第二协方差矩阵，包括：
根据卡尔曼增益、所述第二扩展卡尔曼滤波变量对应的第二协方差矩阵、单位矩阵、第二雅可比矩阵、预设的第二噪声参数，计算所述更新后的第二协方差矩阵。
13.根据权利要求1-12任一所述的方法，其中，所述在预建立的视觉地图上确定与所述关键特征点在相匹配的地图特征点，包括：
通过特征匹配，确定所述视觉地图上与所述关键特征点相匹配的地图特征点；
确定地图坐标系到相机坐标系之间的第六旋转矩阵和第六平移位置。
14.根据权利要求13所述的方法，其中，所述确定地图坐标系到相机坐标系之间的第六旋转矩阵和第六平移位置，包括：
根据所述第六旋转矩阵、第五平移位置确定所述第六平移位置，其中，所述第六旋转矩阵与第五旋转矩阵相同。
15.一种基于视觉惯性里程计的位姿确定装置，所述装置包括：
关键点提取模块，用于提取当前接收的图像帧的关键特征点，并根据所述关键特征点确定所述图像帧基于视觉惯性里程计的世界坐标系的第一位姿；
地图特征点确定模块，用于在预建立的视觉地图上确定与所述关键特征点在相匹配的地图特征点，其中，所述视觉地图为预建立的与所述图像帧相对应的视觉地图；
重投影误差确定模块，用于根据所述关键特征点在所述图像帧的原位置和所述地图特征点投影在所述图像帧的投影位置，确定重投影误差；
位姿确定模块，用于根据所述重投影误差更新所述第一位姿，得到所述图像帧基于视觉地图的地图坐标系的第二位姿。
16.根据权利要求15所述的装置，其中，所述视觉惯性里程计包括相机部件和惯性部件；
所述重投影误差确定模块具体用于：
根据所述相机部件对应的相机坐标系和所述惯性部件对应的惯性坐标系之间的第一旋转矩阵和第一平移位置、所述第一位姿对应的坐标系和所述地图坐标系之间的第二旋转矩阵和第二平移位置，确定所述地图特征点投影到相机坐标系的空间位置；
根据所述空间位置和预设的相机内参矩阵，确定所述地图特征点投影在所述图像帧的投影位置；
根据所述原位置和所述投影位置，确定所述重投影误差。
17.根据权利要求15或16所述的装置，其中，所述位姿确定模块具体用于：
根据所述重投影误差更新包含所述第一位姿的第一扩展卡尔曼滤波变量，得到包含所述第二位姿的第二扩展卡尔曼滤波变量。
18.一种电子设备，包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-14中任一项所述的方法。
19.一种存储有计算机指令的非瞬时计算机可读存储介质，其中，所述计算机指令用于使所述计算机执行根据权利要求1-14中任一项所述的方法。
20.一种计算机程序产品，包括计算机程序，所述计算机程序在被处理器执行时实现根据权利要求1-14中任一项所述的方法。
CN202310749517.XA 2023-06-21 2023-06-21 基于视觉惯性里程计的位姿确定方法、装置、电子设备 Pending CN116989775A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310749517.XA CN116989775A (zh) 2023-06-21 2023-06-21 基于视觉惯性里程计的位姿确定方法、装置、电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310749517.XA CN116989775A (zh) 2023-06-21 2023-06-21 基于视觉惯性里程计的位姿确定方法、装置、电子设备

Publications (1)

Publication Number Publication Date
CN116989775A true CN116989775A (zh) 2023-11-03

Family

ID=88523978

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310749517.XA Pending CN116989775A (zh) 2023-06-21 2023-06-21 基于视觉惯性里程计的位姿确定方法、装置、电子设备

Country Status (1)

Country Link
CN (1) CN116989775A (zh)

Similar Documents

Publication Publication Date Title
CN111882607B (zh) 一种适用于增强现实应用的视觉惯导融合位姿估计方法
Wu et al. A Square Root Inverse Filter for Efficient Vision-aided Inertial Navigation on Mobile Devices.
WO2019205865A1 (zh) 相机姿态追踪过程的重定位方法、装置、设备及存储介质
US9071829B2 (en) Method and system for fusing data arising from image sensors and from motion or position sensors
CN111811506A (zh) 视觉/惯性里程计组合导航方法、电子设备及存储介质
CN110660098B (zh) 基于单目视觉的定位方法和装置
WO2020221307A1 (zh) 一种运动物体的追踪方法和装置
CN111609868A (zh) 一种基于改进光流法的视觉惯性里程计方法
CN114013449B (zh) 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆
WO2023087681A1 (zh) 定位初始化方法、装置、计算机可读存储介质和计算机程序产品
CN117232499A (zh) 多传感器融合的点云地图构建方法、装置、设备及介质
WO2023050634A1 (zh) 定位方法及装置、设备、存储介质及计算机程序产品
CN113610702B (zh) 一种建图方法、装置、电子设备及存储介质
CN116989775A (zh) 基于视觉惯性里程计的位姿确定方法、装置、电子设备
CN114440877B (zh) 一种异步多相机视觉惯性里程计定位方法
CN115388882A (zh) 用于深度辅助视觉惯性测距的方法和装置
CN116958198A (zh) 基于视觉惯性里程计的运动轨迹漂移检测方法及装置
CN112037261A (zh) 一种图像动态特征去除方法及装置
CN116448105B (zh) 位姿更新方法、装置、电子设备和存储介质
CN117058430B (zh) 用于视场匹配的方法、装置、电子设备和存储介质
CN115342829A (zh) 基于迭代扩展卡尔曼滤波器的里程计算方法及装置
CN113375667B (zh) 导航方法、装置、设备和存储介质
WO2022172831A1 (ja) 情報処理装置
US20150092985A1 (en) Updating filter parameters of a system
CN116704034A (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