CN116958198A - 基于视觉惯性里程计的运动轨迹漂移检测方法及装置 - Google Patents
基于视觉惯性里程计的运动轨迹漂移检测方法及装置 Download PDFInfo
- Publication number
- CN116958198A CN116958198A CN202310748383.XA CN202310748383A CN116958198A CN 116958198 A CN116958198 A CN 116958198A CN 202310748383 A CN202310748383 A CN 202310748383A CN 116958198 A CN116958198 A CN 116958198A
- Authority
- CN
- China
- Prior art keywords
- pose
- error
- determining
- map
- coordinate system
- 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
Links
- 230000000007 visual effect Effects 0.000 title claims abstract description 96
- 230000033001 locomotion Effects 0.000 title claims abstract description 83
- 238000001514 detection method Methods 0.000 title claims abstract description 29
- 230000001131 transforming effect Effects 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 121
- 230000009466 transformation Effects 0.000 claims description 78
- 238000000034 method Methods 0.000 claims description 62
- 238000013519 translation Methods 0.000 claims description 60
- 230000002159 abnormal effect Effects 0.000 claims description 21
- 238000001914 filtration Methods 0.000 claims description 21
- 238000004590 computer program Methods 0.000 claims description 12
- 238000005259 measurement Methods 0.000 claims description 7
- 238000012795 verification Methods 0.000 claims description 6
- 238000012216 screening Methods 0.000 claims description 4
- 238000012545 processing Methods 0.000 abstract description 8
- 238000013473 artificial intelligence Methods 0.000 abstract description 3
- 238000013135 deep learning Methods 0.000 abstract description 2
- 230000014616 translation Effects 0.000 description 45
- 238000004422 calculation algorithm Methods 0.000 description 8
- 238000004891 communication Methods 0.000 description 8
- 239000013598 vector Substances 0.000 description 8
- 238000012937 correction Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 238000000605 extraction Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 101150071577 chi2 gene Proteins 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 230000007704 transition Effects 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 230000001413 cellular effect Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000001953 sensory effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012876 topography Methods 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
Classifications
-
- 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
- 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/254—Analysis of motion involving subtraction of images
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Multimedia (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
Abstract
本公开提供了一种基于视觉惯性里程计的运动轨迹漂移检测方法及装置，涉及人工智能技术领域，具体为计算机视觉、图像处理、深度学习等技术领域，可应用于智慧城市等场景。具体实现方案为：获取多个图像帧，分别基于视觉惯性里程计的世界坐标系确定多个图像帧对应的多个第一位姿，并基于预建立的视觉地图的地图坐标系确定多个图像帧对应的多个第二位姿；确定多个位姿对，其中，一个位姿对包括一个图像帧对应的第一位姿和第二位姿；针对每个位姿对，将第一位姿由世界坐标系变换到地图坐标系，得到变换后的第一位姿；根据变换后的第一位姿和第二位姿，确定位姿对的第一误差；基于多个位姿对的第一误差，确定多个第一位姿对应的运动轨迹是否发生漂移。
Description
技术领域
本公开涉及人工智能技术领域，具体为计算机视觉、图像处理、深度学习等技术领域，可应用于智慧城市等场景。
背景技术
视觉惯性里程计(Visual-Inertial Odometry，VIO)包括相机部件和惯性部件，相机部件输入图像，惯性部件输入惯性运动数据，用来持续跟踪用户的位姿，通常会利用图像序列中的稀疏特征点，并计算这些稀疏特征点在三维空间中的位置。但是因为VIO输出的位姿定义在局部坐标系(世界坐标系)下，长时间跟踪后会发生运动轨迹漂移，即：用户回到原先的起点后，所估计的位姿和起点不重合，会导致在虚拟现实场景中，虚拟物体不贴合的情况，而当前的方案中无法检测到运动轨迹漂移，导致轨迹漂移无法被解决。
发明内容
本公开提供了一种用于解决上述技术问题中的至少一项的基于视觉惯性里程计的运动轨迹漂移检测方法及装置。
根据本公开的一方面，提供了一种基于视觉惯性里程计的运动轨迹漂移检测方法，所述方法包括：
获取多个图像帧，分别基于视觉惯性里程计的世界坐标系确定所述多个图像帧对应的多个第一位姿，并基于预建立的视觉地图的地图坐标系确定所述多个图像帧对应的多个第二位姿，其中，所述视觉地图为与所述图像帧相匹配的视觉地图；
确定多个位姿对，其中，一个所述位姿对包括一个所述图像帧对应的所述第一位姿和所述第二位姿；
针对每个所述位姿对，将所述第一位姿由所述世界坐标系变换到所述地图坐标系，得到变换后的第一位姿；
针对每个所述位姿对，根据所述变换后的第一位姿和所述第二位姿，确定所述位姿对的第一误差；
基于所述多个位姿对的所述第一误差，确定所述多个第一位姿对应的运动轨迹是否发生漂移。
根据本公开的另一方面，提供了一种基于视觉惯性里程计的运动轨迹漂移检测装置，所述装置包括：
获取模块，用于获取多个图像帧，分别基于视觉惯性里程计的世界坐标系确定所述多个图像帧对应的多个第一位姿，并基于预建立的视觉地图的地图坐标系确定所述多个图像帧对应的多个第二位姿，其中，所述视觉地图为与所述图像帧相匹配的视觉地图；
位姿对模块，用于确定多个位姿对，其中，一个所述位姿对包括一个所述图像帧对应的所述第一位姿和所述第二位姿；
变换模块，用于针对每个所述位姿对，将所述第一位姿由所述世界坐标系变换到所述地图坐标系，得到变换后的第一位姿；
第一误差模块，用于针对每个所述位姿对，根据所述变换后的第一位姿和所述第二位姿，确定所述位姿对的第一误差；
漂移检测模块，用于基于所述多个位姿对的所述第一误差，确定所述多个第一位姿对应的运动轨迹是否发生漂移。
根据本公开的另一方面，提供了一种电子设备，包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执上述方法。
根据本公开的另一方面，提供了一种存储有计算机指令的非瞬时计算机可读存储介质，其中，所述计算机指令用于使所述计算机执行根据上述方法。
根据本公开的另一方面，提供了一种计算机程序产品，包括计算机程序，所述计算机程序在被处理器执行时实现根据上述方法。
应当理解，本部分所描述的内容并非旨在标识本公开的实施例的关键或重要特征，也不用于限制本公开的范围。本公开的其它特征将通过以下的说明书而变得容易理解。
附图说明
附图用于更好地理解本方案，不构成对本公开的限定。其中：
图1是本公开第一实施例提供的一种基于视觉惯性里程计的运动轨迹漂移检测方法的流程示意图；
图2是本公开第二实施例提供的一种基于视觉惯性里程计的运动轨迹漂移检测方法的流程示意图之一；
图3是本公开第二实施例提供的一种基于视觉惯性里程计的运动轨迹漂移检测方法的流程示意图之二；
图4是本公开第二实施例提供的一种基于视觉惯性里程计的运动轨迹漂移检测方法的流程示意图之三；
图5是本公开第三实施例提供的一种基于视觉惯性里程计的运动轨迹漂移检测装置的结构示意图；
图6是用来实现本公开实施例的方法的电子设备的框图。
具体实施方式
以下结合附图对本公开的示范性实施例做出说明，其中包括本公开实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本公开的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
在不冲突的情况下，本公开各实施例及实施例中的各特征可相互组合。
如本文所使用的，术语“和/或”包括一个或多个相关列举条目的任何和所有组合。
本文所使用的术语仅用于描述特定实施例，且不意欲限制本公开。如本文所使用的，单数形式“一个”和“该”也意欲包括复数形式，除非上下文另外清楚指出。
除非另外限定，否则本文所用的所有术语(包括技术和科学术语)的含义与本领域普通技术人员通常理解的含义相同。还将理解，诸如那些在常用字典中限定的那些术语应当被解释为具有与其在相关技术以及本公开的背景下的含义一致的含义，且将不解释为具有理想化或过度形式上的含义，除非本文明确如此限定。
根据本公开的车辆安全资产的文件夹的复制方法可以由终端设备或服务器等电子设备执行，终端设备可以为车载设备、用户设备(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对非线性函数的泰勒展开式进行一阶线性化截断，忽略其余高阶项，从而将非线性问题转化为线性，可以将卡尔曼线性滤波算法应用于非线性系统中。
需要说明的是，本公开包括四个基础坐标系，即相机部件采集的图像帧的相机坐标系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即代表不同坐标系之间的转换，例如，RWC或pCW表示地图坐标系和相机坐标系之间的转换；RWG或pGW表示地图坐标系和世界坐标系之间的转换；RCI或pIC表示相机坐标系和惯性坐标系之间的转换；RIG或pGI表示惯性坐标系到世界地图坐标系之间的转换，例如：第一位姿表示为RIG和pGI，RIG和pGI即为VIO基于世界坐标系G的输出(即未经视觉地图W修正的VIO输出)；第二位姿表示位RIW和pWI，RIW和pWI表示VIO的输出变换到地图坐标系后的第二位姿。
在公开第一实施例中，参见图1，图1示出本公开第一实施例提供的一种基于视觉惯性里程计的运动轨迹漂移检测方法的流程示意图。该方法包括：
S101、获取多个图像帧，分别基于视觉惯性里程计的世界坐标系确定多个图像帧对应的多个第一位姿，并基于预建立的视觉地图的地图坐标系确定多个图像帧对应的多个第二位姿。
其中，视觉地图为与图像帧相匹配的视觉地图。视觉地图为预建立的与图像帧相对应的视觉地图。预先根据装载相机部件的设备的活动区域，生成与活动区域的环境信息相匹配的视觉地图，视觉地图中每个点的坐标都是确定的，从而能够通过位置确定的视觉地图的点与图像帧中的点的对应关系，去确定运动轨迹是否发生漂移。
其中，第一位姿至少包括第一旋转矩阵RIG和第一平移位置pGI；第二位姿至少包括第二旋转矩阵RIW和第二平移位置pWI。
相应的，多个图像帧的第一位姿集合Traj_loc表示为：
多个图像帧的第二位姿集合Traj_vio表示为：
其中，N表示图像帧的数量，N为大于1的整数。当接收到一帧图像帧，将图像帧定位到视觉地图中，并将定位成功的图像帧确定为S101中获取的图像帧，换言之，S101中的多个图像帧为成功定位到视觉地图的图像帧。
进一步地，第一位姿通过以下方式确定：
针对每一帧图像帧，确定图像帧的关键特征点，并根据关键特征点在世界坐标系的位置，得到图像帧对应的第一位姿。
第二位姿通过以下方式确定：
针对每一图像帧，在与该图像帧相匹配的视觉地图上，确定与关键特征点相匹配的地图特征点；
根据地图特征点在地图坐标系的位置，得到图像帧对应的第二位姿。
也就是说，第一位姿是根据图像帧上的关键特征点在世界坐标系下的位置确定的；第二位姿是根据图像帧上的关键特征点在视觉地图上对应的地图特征点在视觉地图上的位置确定的。
S102、确定多个位姿对，其中，一个位姿对包括一个图像帧对应的第一位姿和第二位姿。
将多个图像帧中，每个图像帧的第一位姿和第二位姿确定为一个位姿对，第一位姿是基于VIO的世界坐标系确定的，第二位姿是基于视觉地图的地图坐标系确定的。
S103、针对每个位姿对，将第一位姿由世界坐标系变换到地图坐标系，得到变换后的第一位姿。
针对基于VIO的世界坐标系确定的第一位姿，通过坐标变换将其变换到地图坐标系的表达下，从而统一变化后的第一位姿和第二位姿的坐标系，以便后续进行比较。
S104、针对每个位姿对，根据变换后的第一位姿和第二位姿，确定位姿对的第一误差。
由于S103中统一了第一位姿和第二位姿的坐标系，从而能够基于地图坐标系比较变换后的第一位姿和第二位姿，并得到第一误差。
S105、基于多个位姿对的第一误差，确定多个第一位姿对应的运动轨迹是否发生漂移。
其中，多个图像帧的第一位姿生成基于世界坐标系下的运动轨迹；多个图像帧的第二位姿生成基于地图坐标系下的运动轨迹，将两个运动轨迹对齐，世界坐标系下的运动轨迹的每个第一位姿有相对应的第二位姿，第二位姿的运动轨迹即为标准运动轨迹(也即未发生漂移的运动轨迹)，从而，通过每对相匹配的第一位姿和第二位姿的第一误差，能够对第一位姿的运动轨迹进行检测，以检测第一位姿的运动轨迹是否发生漂移，换言之，由于视觉地图上的每个点的位置的不变的且确定的，因此根据视觉地图得到的第二位姿的运动轨迹为未发生漂移的运动轨迹，基于第二位姿的运动轨迹与第一位姿的运动轨迹的偏差，能够确定第一位姿的运动轨迹是否发生漂移，以实现运动轨迹漂移检测。
在公开第二实施例中，参见图2，图2示出本公开第二实施例提供的一种基于视觉惯性里程计的运动轨迹漂移检测方法的流程示意图。
首先对下述定义的多个旋转矩阵的平移位置进行解释。
第一位姿：包括第一旋转矩阵RIG、第一平移位置pGI、第一速度vGI，图像帧的关键特征点基于世界坐标系下的VIO输出的位姿。
变换后的第一位姿：变换后的第一旋转矩阵RI'W、变换后的第一平移位置pW'I、变换后的第一速度vW'I，将第一位姿经过坐标变换，变换到视觉地图的地图坐标系后的位姿。
第二位姿：包括第二旋转矩阵RIW、第二平移位置pWI、第二速度vWI，与图像帧的关键特征点匹配到地图特征点基于地图坐标系下得到的位姿。
第一变换旋转矩阵RGW和第一变换平移位置pWG，对应地图坐标系W和世界坐标系G之间的变换的旋转矩阵的平移向量。
第一变换旋转矩阵RGW和第一变换平移位置pWG，对应地图坐标系W和世界坐标系G之间的变换的旋转矩阵的平移向量。
第二变换旋转矩阵RCW和第二变换平移位置pWC，对应地图坐标系W和采集图像帧相机部件的相机坐标系C之间的变换的旋转矩阵的平移向量。
第三变换旋转矩阵RCI和第二变换平移位置pCI，对应VIO的惯性部件的惯性坐标系I和采集图像帧相机部件的相机坐标系C之间的变换的旋转矩阵的平移向量。由于惯性部件和相机部件的内参是预先标定的，因此二者之间的坐标变换也是预先设置的，从而第三变换旋转矩阵RCI和第二变换平移位置pCI是预先获知的。
基于上述，该方法包括：
S201、获取多个图像帧。
当接收到一帧图像帧，将图像帧定位到视觉地图中，并将定位成功的图像帧确定为S201中获取的图像帧，换言之，S201中的多个图像帧为成功定位到视觉地图的图像帧。
S202、提取多个图像帧的关键特征点。
提取一个图像帧的关键特征点的方法有很多，例如：采用预训练的特征点提取模型或者，采用旋转不变二进制鲁棒独立基本特征(Oriented FAST and Rotated BRIEF，ORB)算法对输入的图像帧进行特征点提取和描述，特征点提取采用改进的基于加速分割测试的特征(Features from Accelerated Segment Test，FAST)算法实现，特征点描述采用二进制鲁棒独立基本特征(Binary Robust IndependentElementary Features，BRIEF)算法实现进行特征描述，在此不做限定。
S203、在视觉地图上确定与多个关键特征点在相匹配的多个地图特征点，并生成多个关键特征点和多个地图特征点的匹配关系。
在一些示例中，多个关键特征点和多个地图特征点的匹配关系表示为：[fcorr_1，fcorr_2，...，fcorr_N]。
其中，
p1，p2，…pk分别为多个关键特征点在第i帧图像上的位置，分别为多个关键特征点和与多个关键特征点相匹配的地图特征点的定位索引。
S203的目的为将图像帧定位到视觉地图中，具体地，采用图像搜索、特征匹配、位姿求解等步骤，即可以确定相机坐标系在视觉地图坐标系中的定位信息TCW，表示为TCW＝[RCW，pCW]，其中，RCW表示地图坐标系和相机坐标系之间变换的第三变换旋转矩阵，pCW表示地图坐标系的中心在相机坐标系中的位置，通过RCW和pCW可以得到第三变换平移位置：pWC＝-RCW TpCW。
S204、针对每一帧图像帧，确定图像帧的关键特征点，并根据关键特征点在世界坐标系的位置，得到图像帧对应的第一位姿。
根据S202和S203可知，针对多个图像帧提取了多个关键特征点，针对每一图像帧，确定多个关键特征点在属于该图像帧的目标关键特征点，根据目标关键特征点在世界坐标系的位置，得到图像帧对应的第一位姿：第一旋转矩阵RIG、第一平移位置pGI、第一速度vGI。
其中，S204为S101中分别基于视觉惯性里程计的世界坐标系确定多个图像帧对应的多个第一位姿的一种实现方式。
S205、针对每一图像帧，在与图像帧相匹配的视觉地图上，确定与关键特征点相匹配的地图特征点。
再通过上述关键特征点和地图特征点的匹配关系确定出目标关键特征点相匹配的地图特征点。
S206、根据地图特征点在地图坐标系的位置，得到图像帧对应的第二位姿。
确定出与关键特征点匹配到地图特征点后，基于地图坐标系和与第一位姿相同的VIO算法，确定出第二位姿。
其中，S205和S206为S101中基于预建立的视觉地图的地图坐标系确定多个图像帧对应的多个第二位姿的一种实现方式。
S207、确定多个位姿对，其中，一个位姿对包括一个图像帧对应的第一位姿和第二位姿。
将多个图像帧中，每个图像帧的第一位姿和第二位姿确定为一个位姿对，第一位姿是基于VIO的世界坐标系确定的，第二位姿是基于视觉地图的地图坐标系确定的。
由于视觉地图中的各个地图特征点的位置的一定的，建立地图特征点和关键特征点的匹配关系后，相当于给关键特征点提供了一个确定的位置点(即与之对应的地图特征点)，从而根据关键特征点基于VIO输出的图像帧的第一位姿和根据地图特征点基于同样的VIO算法得到的第二位姿的误差大，则表示当前第一位姿的位置发生了漂移。
S208、确定对应世界坐标系和地图坐标系之间的变换的第一变换旋转矩阵和第一变换平移位置；
在一些示例中，S208包括：
子步骤一：根据第一旋转矩阵、图像帧的相机坐标系和地图坐标系之间的第二变换旋转矩阵、相机坐标系和视觉惯性里程计的惯性坐标系之间的第三变换旋转矩阵，确定第一变换旋转矩阵。
具体按照如下公式确定第一变换旋转矩阵RGW：
RGW＝RIG TRCI TRCW (1)
其中，RIG为第一旋转矩阵；RCI为第二变换旋转矩阵；RCW为第三变换旋转矩阵。
子步骤二：根据第一旋转矩阵和第一平移位置、第二变换旋转矩阵和第二变换平移位置、第三变换旋转矩阵和第三变换平移位置，确定第一变换平移位置。
具体按照如下公式确定第一变换平移位置RWG：
pWG＝-RCW TRCIRIGpGI+RCW TpCI+pWC (2)
其中，RIG、pGI为第一旋转矩阵和第一平移位置；RCW、pWC为第二变换旋转矩阵和第二变换平移位置；RCI、pCI为第三变换旋转矩阵和第三变换平移位置。
S209、根据第一旋转矩阵和第一变换旋转矩阵，得到变换后的第一旋转矩阵；
具体按照如下公式确定变换后的第一旋转矩阵RI′W：
RI′W＝RIGRGW (3)
其中，RIG为第一旋转矩阵；RGW为第一变换旋转矩阵。
S2010、根据第一平移位置、第一变换平移位置和第一变换旋转矩阵，得到变换后的第一平移位置。
具体按照如下公式变换后的第一平移位置pW′I：
pW′I＝RGW TpGI+pWG (4)
其中，RGW为第一变换旋转矩阵；pWG为第一变换平移位置；pGI为第一平移位置。
在一些示例中，还包括：根据第一变换旋转矩阵和第一速度，得到变换后的第一速度vW'I。
具体按照如下公式变换后的第一速度vW'I：
vW'I＝RGW T+vGI (4)
其中，RGW为第一变换旋转矩阵；pWG为第一变换平移位置；pGI为第一平移位置。
S208-S2010的目的为将根据图像帧的关键特征点、基于世界坐标系G输出的第一位姿转换到地图坐标系W下，以统一第一位姿和第二位姿的坐标系，从而对二者进行比较。针对多个第一位姿来说，这一步骤相当于将第一位姿的运动轨迹和第二位姿的运动轨迹对齐。
需要说明的是，S208-S2010是S103的一种实现方式，S103还具有其他实现方式，在此不做限定。
S2011、针对每个位姿对，确定变换后的第一位姿的第一平移位置和第二位姿的第二平移位置的欧式距离，以欧式距离作为位姿对的第一误差。
具体地，针对每个位姿对，第一误差即为：pW'I和pWI之间的欧式距离。
需要说明的是，S2011是S104的一种实现方式，S104还具有其他实现方式，在此不做限定。
S2012、将多个第一误差分为多个第一误差组。
具体地，可以采用按第一误差对应的图像帧的时间顺序，均分的方式分为多组。也即按照采集图像帧的时间先后顺序，对图像帧对应的第一误差进行分组。
S2013、针对每个第一误差组，根据第一误差组中的第一误差确定第一误差组的组误差。
在一些示例中，组误差可以为该第一误差组中的平均值，获知，组误差为均方根误差。在组误差为均方根误差的情况下，S2013具体包括：
针对每个第一误差组，根据第一误差组中的第一误差计算误差平均值，并根据误差平均值计算均方根误差err_disrmse。
具体地，可以按照如下方式计算均方根误差err_disrmse。
S2014、根据多个第一误差组的多个组误差，确定运动轨迹是否发生漂移。
在一些示例中，基于组误差为均方根误差，S2014可以包括：
子步骤一、按照多个第一误差组对应的图像帧的时间顺序，依次比较每个第一误差组的均方根误差与预设的第一阈值，并将均方根误差小于第一阈值的第一误差组确定为异常组。
子步骤二、在确定连续K帧图像帧对应的第一误差组为异常组的情况下，确定运动轨迹发生漂移，其中，K为大于1的整数。
按照S201中获取的多个图像帧的时间顺序(即采集时间的先后顺序)，从距离当前最近的图像帧对应的第一误差组成的第一误差组开始，确定第一误差组的均方根误差与第一阈值的大小；通过这种方式确定每个第一误差组的均方根误差的大小。在确定连续多帧图像帧对应的第一误差组的均方根误差小于第一阈值err_distt的情况下，即可判断第一位姿对应的运动轨迹发生漂移。通过这种方式，根据连续多帧的第一位姿和第二位姿的距离误差，确定出均方根，从而根据连续多帧图像帧的状态判断运动轨迹漂移，能够综合一个时间段上的位姿状态准确判断漂移。
需要说明的是，S2012-S2014是S105的一种实现方式，S105还具有其他实现方式，在此不做限定。
在一些示例中，除了以均方根误差作为检测参考外，还可以引入其他误差因素进一步对运动轨迹进行漂移检测。
参见图3，图3示出基于上述S2014之后，本方法还包括：
S301、获取多个特征点对，其中，一个特征点对包括一个基于多个图像帧提取的关键特征点，和一个在视觉地图上与关键特征点相匹配的地图特征点。
其中，基于S203中确定的述多个关键特征点和多个地图特征点的匹配关系，S301包括：
子步骤一：针对多个图像帧的关键特征点进行筛选，去除重复的关键特征点。
多个图像帧中每个图像帧具有多个关键特征点，多个图像帧的关键特征点可能存在重复(即同一位置或相似位置的特征点)，为了减少运算量，统一关键特征点只进行一次计算，因此综合多个图像帧进行筛选，去除其中重复的关键特征点，确定筛选后的彼此具有差异度的关键特征点。
子步骤二：根据筛选后的关键特征点与地图特征点的匹配关系，确定多个特征点对。
基于子步骤一筛选出来的彼此具有差异度的关键特征点，根据S203中确定的关键特征点和地图特征点的匹配关系，确定出筛查出的关键特征点对应的地图特征点，再将每个筛查后的关键特征点和与之对应的地图特征点确定为一个特征点对。
在上述步骤中，接收到图像帧后，将图像帧定位到视觉地图上，从而确定出视觉地图上与图像帧的关键特征点相对应的地图特征点，而地图特征点的位置的确定的且不变的，从而能够给关键特征点提供稳定的参考位置，通过比较关键特征点和与之对应的地图特征点的位置，即能够确定漂移状态。
S302、针对每个特征点对，根据关键特征点的位置和地图特征点的位置确定特征点对的第二误差。
可以根据多种方式，比较特征点对内两个特征点的位置关系，例如：第二误差包括重投影误差和卡方校验(chi2)误差中的至少一个。为了便于说明，以下以第二误差包括重投影误差和chi2误差为例。
在第二误差包括重投影误差的情况下，S302包括：
S3021、针对每个特征点对，根据关键特征点在图像帧的原位置和地图特征点投影在图像帧的投影位置，确定特征点对的重投影误差。
关键特征点在图像帧的原位置，即关键特征点在图像帧的像素位置，图像帧上每个像素点都具有像素坐标信息，因此根据关键特征点的像素坐标信息及可确定像素位置(原位置)；投影位置：先确定地图特征点投影到相机坐标系的空间位置，再根据空间位置和预设的相机部件的相机内参矩阵，确定地图特征点投影在图像帧的投影位置；确定原位置和投影位置后，即可确定二者之间的距离(例如：欧式距离)，以距离作为重投影误差。S3021的目的为：以关键特征点对应的地图特征点，确定其与当前测量得到的关键特征点的距离，以确定漂移量。
在一些示例中，S3021具体包括：
子步骤一、针对每个特征点对，根据第二旋转矩阵和第二平移位置、图像帧对应的相机坐标系和视觉惯性里程计的惯性坐标系之间的第三变换旋转矩阵和第三变换平移位置，确定地图特征点在地图坐标系的空间位置。
具体地，按照下式确定所述空间位置：
其中，为地图特征点投影到相机坐标系的空间位置的三个向量；WPk表示地图特征点在地图坐标系的位置；其中，角标k表示第k图像帧，k为大于等于1的整数，/>表示姿态四元数，根据姿态四元数可表示旋转矩阵，即第一旋转矩阵表示为/>
子步骤二、根据空间位置和预设的相机内参矩阵，确定地图特征点投影在图像帧的投影位置。
具体地，按照下式确定所述投影位置：
其中，特征点i(地图特征点或关键特征点)在第k帧图像上的位置表示为：
其中，为相机内参矩阵，其中，fx、fy为相机内参矩阵的焦距；cx、cy为相机内参的主点，将式(5)代入式(6)进行计算。
子步骤三、根据原位置和投影位置，确定特征点对的重投影误差。
具体地，按照下式确定所述重投影误差ere-pro：
ere-pro＝pk+pk' (7)
其中，pk是关键特征点在图像帧的原位置；pk'是关键特征点对应的地图特征点投影到图像帧上的投影位置。
综合式(6)和(7)可知，重投影误差项可以直接约束EKF更新变量中的和/>即直接约束转换到地图坐标的姿态和位置。
在第二误差还包括重chi2误差的情况下，S302还包括：
S3022、针对每个特征点对，根据重投影误差、卡方参数确定特征点对的卡方校验误差。
echi2＝ere-pro TS-1ere-pro (8)
其中，将式(7)代入式(8)进行计算；S为卡方参数。
在一些示例中，其中，卡方参数根据以下方式获得：
根据第一雅可比矩阵、第一协方差矩阵和预设的第一噪声参数确定。
S＝HXPXXHX T+R (9)
其中，第一雅可比矩阵Hk为重投影误差对第二位姿(具体为和/>)的雅可比矩阵；第一协方差矩阵PXX为第二位姿(具体为/>和/>)的协方差矩阵；第一噪声参数R用于消除提取关键特征点的测量误差。
S303、基于多个特征点对的第二误差，确定运动轨迹是否发生漂移。
基于S3021，S303包括：
S3031、依次比较每个特征点对的重投影误差与预设的第二阈值，并将重投影误差大于第二阈值的特征点对确定为重投影异常点对。
S3032、根据重投影异常点对的数量与特征点对的总数确定重投影误差异常比例，并根据重投影误差异常比例确定运动轨迹是否发生漂移。
重投影误差大于第二阈值则表示误差过大无法通过校验，S3032的目的为确定多个特征点对的重投影误差无法通过校验的比例，若比例大于预设的第四阈值，则表示当前第一位姿的运动轨迹发生漂移。
基于S3022，S303还包括：
S3033、依次比较每个特征点对的卡方校验误差与预设的第三阈值，并将卡方校验误差大于第三阈值的特征点对确定为卡方异常点对。
在一些示例中，第三阈值为卡方校验统计值x2(n)。
其中，n根据重投影误差的向量维度m设置，n＝2m，例如在式(5)中重投影误差的向量维度m为3，则n为6。
S3034、根据卡方异常点对的数量与特征点对的总数确定卡方误差异常比例，并根据卡方误差异常比例确定运动轨迹是否发生漂移。
卡方校验误差大于第三阈值则表示误差过大无法通过校验，S3034的目的为确定多个特征点对的卡方校验误差无法通过校验的比例，若比例大于预设的第五阈值，则表示当前第一位姿的运动轨迹发生漂移。
在一些示例中，可以综合S3032和S3034，就重投影异常点对的数量和卡方异常点对的数量，与特征点对的总数确定最终的异常比例，再基于异常比例确定运动轨迹是否发生漂移。
需要说明的是，对于基于重投影误差的检测和基于chi2误差的检测，在具体实施例中可以只采用其中一种检测，也可以两种都采用，即上述步骤可以根据具体实施例进行省略和组合，在此不做限定。
本公开提供的方法，可以基于三个维度的误差对运动轨迹进行漂移检测，具体地，基于均方根误差、重投影误差、卡方校验误差(或三者中至少一个)对运动轨迹的漂移进行判断，能够准确检测出运动轨迹的漂移。
参见图4，图4示出基于上述S2014或S3034确定出运动轨迹发生漂移后之后，基于检测对运动轨迹进行修正，该方法还包括：
S401、在确定运动轨迹发生漂移的情况下，基于第一误差，对当前接收的图像帧的第一位姿进行修正。
具体地，可以采用多种方式进行修正，例如，在本公开的VIO为基于EKF的VIO的情况下，在基于EKF的框架中，根据扩展卡尔曼滤波变量表示位姿，具体地，需要基于视觉地图的修正更新EKF变量，并且还需要更新EKF变量的协方差矩阵。基于此，S401包括：
步骤一、基于当前接收的图像帧对应的重投影误差，对当前接收的图像帧的第一扩展卡尔曼滤波变量进行修正，其中，第一扩展卡尔曼滤波变量包括第一位姿。
其中，修正前的第一位姿的第一扩展卡尔曼滤波变量按照常规扩展卡尔曼滤波算法确定，后续基于重投影误差修正该第一扩展卡尔曼滤波变量，得到修正后的第一扩展卡尔曼滤波变量。
需要说明的是，修正后的第一扩展卡尔曼滤波变量xekf的定义按照如下：
ximu＝[qIW，bg，vWI，ba，pWI] (10.2)
其中，bg，ba分别表示陀螺仪和加速度计的偏置，其他参数的定义同上述。
并且，修正后第一扩展卡尔曼滤波变量xekf包含过去M帧图像的第二位姿(表示为姿态四元素和平移位置)。
具体地，步骤一包括：
子步骤一、确定针对重投影误差的卡尔曼增益。
在一些示例中，根据第一协方差矩阵、第一雅可比矩阵、预设的第二噪声参数确定卡尔曼增益。
具体地，按照下式确定所述卡尔曼增益Kk：
Kk＝PXXHk T(HkPXXHk T+Rk)-1 (11)
其中，Hk为式(6)中的第一雅可比矩阵；Rk为第二噪声参数；PXX为式(6)中的第一协方差矩阵。
子步骤二、根据卡尔曼增益和重投影误差确定卡尔曼滤波变量的增量。
具体地，按照下式确定所述卡尔曼滤波变量的增量：
δxk＝Kkere-pro (12)
子步骤三、根据卡尔曼滤波变量的增量更新第一扩展卡尔曼滤波变量。
xk＝xk+δxk (13)
其中，等式左侧的xk为更新后的第一扩展卡尔曼滤波变量，右侧的xk为更新前的第一扩展卡尔曼滤波变量，δxk为卡尔曼滤波变量的增量。
S402、对第一扩展卡尔曼滤波变量对应的第一协方差矩阵进行更新，得到更新后的第一协方差矩阵。
根据卡尔曼增益、更新后的第一扩展卡尔曼滤波变量对应的第一协方差矩阵、单位矩阵、第一雅可比矩阵、预设的第二噪声参数，计算更新后的第一协方差矩阵。
具体地，按照下式确定所述更新后的第一协方差矩阵：
P'XX＝(I-KkHk)PXX(1-KkPHk)T+KkRkKk T (14)
其中，公式的各参数含义同上述。
需要说明的是，在一些实现方式中，例如不采用EKF的方式中，可以省略S402，在此不做限定。
在公开第三实施例中，基于与图1相同的原理，图5示出本公开第三实施例提供的基于视觉惯性里程计的运动轨迹漂移检测装置50，该装置包括：
获取模块501，用于获取多个图像帧，分别基于视觉惯性里程计的世界坐标系确定多个图像帧对应的多个第一位姿，并基于预建立的视觉地图的地图坐标系确定多个图像帧对应的多个第二位姿，其中，视觉地图为与图像帧相匹配的视觉地图；
位姿对模块502，用于确定多个位姿对，其中，一个位姿对包括一个图像帧对应的第一位姿和第二位姿；
变换模块503，用于针对每个位姿对，将第一位姿由世界坐标系变换到地图坐标系，得到变换后的第一位姿；
第一误差模块504，用于针对每个位姿对，根据变换后的第一位姿和第二位姿，确定位姿对的第一误差；
漂移检测模块505，用于基于多个位姿对的第一误差，确定多个第一位姿对应的运动轨迹是否发生漂移。
在一些示例中，第一位姿包括：第一旋转矩阵和第一平移位置；
变换模块503具体包括：
变换确定子模块，用于确定对应世界坐标系和地图坐标系之间的变换的第一变换旋转矩阵和第一变换平移位置；
第一变换子模块，用根据第一旋转矩阵和第一变换旋转矩阵，得到变换后的第一旋转矩阵；
第二变换子模块，用于根据第一平移位置、第一变换平移位置和第一变换旋转矩阵，得到变换后的第一平移位置。
在一些示例中，变换确定子模块具体用于：
根据第一旋转矩阵、图像帧的相机坐标系和地图坐标系之间的第二变换旋转矩阵、相机坐标系和视觉惯性里程计的惯性坐标系之间的第三变换旋转矩阵，确定第一变换旋转矩阵；
根据第一旋转矩阵和第一平移位置、第二变换旋转矩阵和第二变换平移位置、第三变换旋转矩阵和第三变换平移位置，确定第一变换平移位置。
在一些示例中，第一位姿包括第一平移位置；第二位姿包括第二平移位置；
第一误差模块504具体用于：
确定第一变换位姿的第一平移位置和第二位姿的第二平移位置的欧式距离，以欧式距离作为位姿对的第一误差。
在一些示例中，漂移检测模块505包括：
分组子模块，用于将多个第一误差分为多个第一误差组；
组误差子模块，用于针对每个第一误差组，根据第一误差组中的第一误差确定第一误差组的组误差；
漂移检测子模块，用于根据多个第一误差组的多个组误差，确定运动轨迹是否发生漂移。
在一些示例中，组误差为均方根误差；
组误差子模块具体用于：
针对每个第一误差组，根据第一误差组中的第一误差计算误差平均值，并根据误差平均值计算均方根误差；
漂移检测子模块具体用于：
按照多个第一误差组对应的图像帧的时间顺序，依次比较每个第一误差组的均方根误差与预设的第一阈值，并将均方根误差小于第一阈值的第一误差组确定为异常组；
在确定连续K帧图像帧对应的第一误差组为异常组的情况下，确定运动轨迹发生漂移，其中，K为大于1的整数。
在一些示例中，该装置还包括：
点对获取模块，用于获取多个特征点对，其中，一个特征点对包括一个基于多个图像帧提取的关键特征点，和一个在视觉地图上与关键特征点相匹配的地图特征点；
第二误差模块，用于针对每个特征点对，根据关键特征点的位置和地图特征点的位置确定特征点对的第二误差；
漂移检测模块，用于基于多个特征点对的第二误差，确定运动轨迹是否发生漂移。
在一些示例中，第二误差包括：重投影误差；
第二误差模块具体用于：
针对每个特征点对，根据关键特征点在图像帧的原位置和地图特征点投影在图像帧的投影位置，确定特征点对的重投影误差。
在一些示例中，第二位姿包括：第二旋转矩阵和第二平移位置；
第二误差模块具体用于：
针对每个特征点对，根据第二旋转矩阵和第二平移位置、图像帧对应的相机坐标系和视觉惯性里程计的惯性坐标系之间的第三变换旋转矩阵和第三变换平移位置，确定地图特征点投影到相机坐标系的空间位置；
根据空间位置和预设的相机内参矩阵，确定地图特征点投影在图像帧的投影位置；
根据原位置和投影位置，确定特征点对的重投影误差。
在一些示例中，漂移检测模块具体用于：
依次比较每个特征点对的重投影误差与预设的第二阈值，并将重投影误差大于第二阈值的特征点对确定为重投影异常点对；
根据重投影异常点对的数量与特征点对的总数确定重投影误差异常比例，并根据重投影误差异常比例确定运动轨迹是否发生漂移。
在一些示例中，漂移检测子模块具体用于：
针对每个特征点对，根据重投影误差、卡方参数确定特征点对的卡方校验误差。
在一些示例中，卡方参数根据以下方式获得：
根据第一雅可比矩阵、第一协方差矩阵和预设的第一噪声参数确定；
其中，第一雅可比矩阵为重投影误差对第二位姿的雅可比矩阵；第一协方差矩阵为第二位姿的协方差矩阵；第一噪声参数用于消除提取关键特征点的测量误差。
在一些示例中，漂移检测模块还用于：
依次比较每个特征点对的卡方校验误差与预设的第三阈值，并将卡方校验误差大于第三阈值的特征点对确定为卡方异常点对；
根据卡方异常点对的数量与特征点对的总数确定卡方误差异常比例，并根据卡方误差异常比例确定运动轨迹是否发生漂移。
在一些示例中在，该装置还包括：
提取模块，用于提取多个图像帧的关键特征点；
匹配模块，用于在视觉地图上确定与多个关键特征点在相匹配的多个地图特征点，并生成多个关键特征点和多个地图特征点的匹配关系。
在一些示例中，点对获取模块具体用于：
针对多个图像帧的关键特征点进行筛选，去除重复的关键特征点；
根据筛选后的关键特征点与地图特征点的匹配关系，确定多个特征点对。
在一些示例中，获取模块具体用于：
针对每一帧图像帧，确定图像帧的关键特征点，并根据关键特征点在世界坐标系的位置，得到图像帧对应的第一位姿。
在一些示例中，获取模块具体用于：
针对每一图像帧，在与图像帧相匹配的视觉地图上，确定与关键特征点相匹配的地图特征点；
根据地图特征地图坐标系的位置，得到图像帧对应的第二位姿。
在一些示例中，该装置还包括：
修正模块，用于在确定运动轨迹发生漂移的情况下，基于第一误差，对当前接收的图像帧的第一位姿进行修正。
在一些示例中，第一误差包括重投影误差；修正模块具体用于：
基于当前接收的图像帧对应的重投影误差，对当前接收的图像帧的第一扩展卡尔曼滤波变量进行修正，其中，第一扩展卡尔曼滤波变量包括第一位姿。
在一些示例中，修正模块具体用于：
确定针对重投影误差的卡尔曼增益；
根据卡尔曼增益和重投影误差确定卡尔曼滤波变量的增量；
根据卡尔曼滤波变量的增量更新第一扩展卡尔曼滤波变量。
在一些示例中，该装置还包括：
协方差模块，用于针对更新后的第一扩展卡尔曼滤波变量对应的第一协方差矩阵，得到更新后的第一协方差矩阵。
本公开的技术方案中，所涉及的用户个人信息的获取，存储和应用等，均符合相关法律法规的规定，且不违背公序良俗。
根据本公开的实施例，本公开还提供了一种电子设备、一种可读存储介质和一种计算机程序产品。
图6示出了可以用来实施本公开的实施例的示例电子设备600的示意性框图。电子设备旨在表示各种形式的数字计算机，诸如，膝上型计算机、台式计算机、工作台、个人数字助理、服务器、刀片式服务器、大型计算机、和其它适合的计算机。电子设备还可以表示各种形式的移动装置，诸如，个人数字处理、蜂窝电话、智能电话、可穿戴设备和其它类似的计算装置。本文所示的部件、它们的连接和关系、以及它们的功能仅仅作为示例，并且不意在限制本文中描述的和/或者要求的本公开的实现。
如图6所示，设备600包括计算单元601，其可以根据存储在只读存储器(ROM)602中的计算机程序或者从存储单元603加载到随机访问存储器(RAM)603中的计算机程序，来执行各种适当的动作和处理。在RAM 603中，还可存储设备600操作所需的各种程序和数据。计算单元601、ROM 602以及RAM 602通过总线604彼此相连。输入/输出(I/O)接口605也连接至总线604。
设备600中的多个部件连接至I/O接口605，包括：输入单元606，例如键盘、鼠标等；输出单元607，例如各种类型的显示器、扬声器等；存储单元608，例如磁盘、光盘等；以及通信单元609，例如网卡、调制解调器、无线通信收发机等。通信单元609允许设备600通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
计算单元601可以是各种具有处理和计算能力的通用和/或专用处理组件。计算单元601的一些示例包括但不限于中央处理单元(CPU)、图形处理单元(GPU)、各种专用的人工智能(AI)计算芯片、各种运行机器学习模型算法的计算单元、数字信号处理器(DSP)、以及任何适当的处理器、控制器、微控制器等。计算单元601执行上文所描述的各个方法和处理，例如基于视觉惯性里程计的运动轨迹漂移检测方法。例如，在一些实施例中，基于视觉惯性里程计的位姿确定方法可被实现为计算机软件程序，其被有形地包含于机器可读介质，例如存储单元608。在一些实施例中，计算机程序的部分或者全部可以经由ROM 602和/或通信单元609而被载入和/或安装到设备600上。当计算机程序加载到RAM 603并由计算单元601执行时，可以执行上文描述的基于视觉惯性里程计的位姿确定方法的一个或多个步骤。备选地，在其他实施例中，计算单元601可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行基于视觉惯性里程计的位姿确定方法。
本文中以上描述的系统和技术的各种实施方式可以在数字电子电路系统、集成电路系统、现场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、负载可编程逻辑设备(CPLD)、计算机硬件、固件、软件、和/或它们的组合中实现。这些各种实施方式可以包括：实施在一个或者多个计算机程序中，该一个或者多个计算机程序可在包括至少一个可编程处理器的可编程系统上执行和/或解释，该可编程处理器可以是专用或者通用可编程处理器，可以从存储系统、至少一个输入装置、和至少一个输出装置接收数据和指令，并且将数据和指令传输至该存储系统、该至少一个输入装置、和该至少一个输出装置。
用于实施本公开的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
在本公开的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
为了提供与用户的交互，可以在计算机上实施此处描述的系统和技术，该计算机具有：用于向用户显示信息的显示装置(例如，CRT(阴极射线管)或者LCD(液晶显示器)监视器)；以及键盘和指向装置(例如，鼠标或者轨迹球)，用户可以通过该键盘和该指向装置来将输入提供给计算机。其它种类的装置还可以用于提供与用户的交互；例如，提供给用户的反馈可以是任何形式的传感反馈(例如，视觉反馈、听觉反馈、或者触觉反馈)；并且可以用任何形式(包括声输入、语音输入或者、触觉输入)来接收来自用户的输入。
可以将此处描述的系统和技术实施在包括后台部件的计算系统(例如，作为数据服务器)、或者包括中间件部件的计算系统(例如，应用服务器)、或者包括前端部件的计算系统(例如，具有图形用户界面或者网络浏览器的用户计算机，用户可以通过该图形用户界面或者该网络浏览器来与此处描述的系统和技术的实施方式交互)、或者包括这种后台部件、中间件部件、或者前端部件的任何组合的计算系统中。可以通过任何形式或者介质的数字数据通信(例如，通信网络)来将系统的部件相互连接。通信网络的示例包括：局域网(LAN)、广域网(WAN)和互联网。
计算机系统可以包括客户端和服务器。客户端和服务器一般远离彼此并且通常通过通信网络进行交互。通过在相应的计算机上运行并且彼此具有客户端-服务器关系的计算机程序来产生客户端和服务器的关系。服务器可以是云服务器，也可以为分布式系统的服务器，或者是结合了区块链的服务器。
应该理解，可以使用上面所示的各种形式的流程，重新排序、增加或删除步骤。例如，本公开中记载的各步骤可以并行地执行也可以顺序地执行也可以不同的次序执行，只要能够实现本公开公开的技术方案所期望的结果，本文在此不进行限制。
上述具体实施方式，并不构成对本公开保护范围的限制。本领域技术人员应该明白的是，根据设计要求和其他因素，可以进行各种修改、组合、子组合和替代。任何在本公开的精神和原则之内所作的修改、等同替换和改进等，均应包含在本公开保护范围之内。
Claims (25)
1.一种基于视觉惯性里程计的运动轨迹漂移检测方法，所述方法包括：
获取多个图像帧，分别基于视觉惯性里程计的世界坐标系确定所述多个图像帧对应的多个第一位姿，并基于预建立的视觉地图的地图坐标系确定所述多个图像帧对应的多个第二位姿，其中，所述视觉地图为与所述图像帧相匹配的视觉地图；
确定多个位姿对，其中，一个所述位姿对包括一个所述图像帧对应的所述第一位姿和所述第二位姿；
针对每个所述位姿对，将所述第一位姿由所述世界坐标系变换到所述地图坐标系，得到变换后的第一位姿；
针对每个所述位姿对，根据所述变换后的第一位姿和所述第二位姿，确定所述位姿对的第一误差；
基于所述多个位姿对的所述第一误差，确定所述多个第一位姿对应的运动轨迹是否发生漂移。
2.根据权利要求1所述的方法，其中，所述第一位姿包括：第一旋转矩阵和第一平移位置；
所述针对每个所述位姿对，将所述第一位姿由所述世界坐标系变换到所述地图坐标系，得到变换后的第一位姿，包括：
确定对应所述世界坐标系和所述地图坐标系之间的变换的第一变换旋转矩阵和第一变换平移位置；
根据所述第一旋转矩阵和所述第一变换旋转矩阵，得到变换后的所述第一旋转矩阵；
根据所述第一平移位置、所述第一变换平移位置和所述第一变换旋转矩阵，得到变换后的所述第一平移位置。
3.根据权利要求2所述的方法，其中，所述确定对应所述世界坐标系和所述地图坐标系之间的变换的第一变换旋转矩阵和第一变换平移位置，包括：
根据所述第一旋转矩阵、所述图像帧的相机坐标系和所述地图坐标系之间的第二变换旋转矩阵、所述相机坐标系和所述视觉惯性里程计的惯性坐标系之间的第三变换旋转矩阵，确定所述第一变换旋转矩阵；
根据所述第一旋转矩阵和所述第一平移位置、所述第二变换旋转矩阵和第二变换平移位置、所述第三变换旋转矩阵和第三变换平移位置，确定所述第一变换平移位置。
4.根据权利要求1-3任一所述的方法，其中，所述第一位姿包括第一平移位置；所述第二位姿包括第二平移位置；
所述针对每个所述位姿对，根据所述变换后的第一位姿和所述第二位姿，确定所述位姿对的第一误差，包括：
确定所述变换后的第一位姿的所述第一平移位置和所述第二位姿的所述第二平移位置的欧式距离，以所述欧式距离作为所述位姿对的所述第一误差。
5.根据权利要求4所述的方法，其中，所述基于所述多个位姿对的所述第一误差，确定所述多个第一位姿对应的运动轨迹是否发生漂移，包括：
将多个所述第一误差分为多个第一误差组；
针对每个第一误差组，根据所述第一误差组中的第一误差确定所述第一误差组的组误差；
根据所述多个第一误差组的多个所述组误差，确定所述运动轨迹是否发生漂移。
6.根据权利要求5所述的方法，其中，所述组误差为均方根误差；
所述针对每个第一误差组，根据所述第一误差组中的第一误差确定所述第一误差组的组误差，包括：
针对每个第一误差组，根据所述第一误差组中的第一误差计算误差平均值，并根据所述误差平均值计算均方根误差；
所述根据所述多个第一误差组的多个所述组误差，确定所述运动轨迹是否发生漂移，包括：
按照多个第一误差组对应的图像帧的时间顺序，依次比较每个所述第一误差组的所述均方根误差与预设的第一阈值，并将所述均方根误差小于所述第一阈值的第一误差组确定为异常组；
在确定连续K帧图像帧对应的所述第一误差组为异常组的情况下，确定所述运动轨迹发生漂移，其中，K为大于1的整数。
7.根据权利要求1-6任一所述的方法，其中，所述基于所述多个位姿对的所述第一误差，确定所述多个位姿对应的运动轨迹是否发生漂移之后，所述方法还包括：
获取多个特征点对，其中，一个特征点对包括一个基于所述多个图像帧提取的关键特征点，和一个在所述视觉地图上与所述关键特征点相匹配的地图特征点；
针对每个特征点对，根据所述关键特征点的位置和所述地图特征点的位置确定所述特征点对的第二误差；
基于所述多个特征点对的所述第二误差，确定所述运动轨迹是否发生漂移。
8.根据权利要求7所述的方法，其中，所述第二误差包括：重投影误差；
所述针对每个特征点对，根据所述关键特征点的位置和所述地图特征点的位置确定所述特征点对的第二误差，包括：
针对每个所述特征点对，根据所述关键特征点在所述图像帧的原位置和所述地图特征点投影在所述图像帧的投影位置，确定所述特征点对的重投影误差。
9.根据权利要求8所述的方法，其中，所述第二位姿包括：第二旋转矩阵和第二平移位置；
所述针对每个所述特征点对，根据所述关键特征点在所述图像帧的原位置和所述地图特征点投影在所述图像帧的投影位置，确定所述特征点对的重投影误差，包括：
针对每个所述特征点对，根据所述第二旋转矩阵和所述第二平移位置、所述图像帧对应的相机坐标系和所述视觉惯性里程计的惯性坐标系之间的第三变换旋转矩阵和第三变换平移位置，确定所述地图特征点投影到相机坐标系的空间位置；
根据所述空间位置和预设的相机内参矩阵，确定所述地图特征点投影在所述图像帧的投影位置；
根据所述原位置和所述投影位置，确定所述特征点对的所述重投影误差。
10.根据权利要求8或9所述的方法，其中，所述基于所述多个特征点对的所述第二误差，确定所述运动轨迹是否发生漂移，包括：
依次比较每个所述特征点对的所述重投影误差与预设的第二阈值，并将所述重投影误差大于所述第二阈值的特征点对确定为重投影异常点对；
根据所述重投影异常点对的数量与所述特征点对的总数确定重投影误差异常比例，并根据所述重投影误差异常比例确定所述运动轨迹是否发生漂移。
11.根据权利要求10所述的方法，其中，所述第二误差还包括：卡方校验误差；
所述针对每个所述特征点对，根据所述关键特征点在所述图像帧的原位置和所述地图特征点投影在所述图像帧的投影位置，确定所述特征点对的重投影误差之后，所述针对每个特征点对，根据所述关键特征点的位置和所述地图特征点的位置确定所述特征点对的第二误差，还包括：
针对每个特征点对，根据所述重投影误差、卡方参数确定所述特征点对的卡方校验误差。
12.根据权利要求11所述的方法，其中，所述卡方参数根据以下方式获得：
根据第一雅可比矩阵、第一协方差矩阵和预设的第一噪声参数确定；
其中，所述第一雅可比矩阵为所述重投影误差对所述第二位姿的雅可比矩阵；所述第一协方差矩阵为所述第二位姿的协方差矩阵；所述第一噪声参数用于消除提取所述关键特征点的测量误差。
13.根据权利要求11或12所述的方法，其中，所述基于所述多个特征点对的所述第二误差，确定所述运动轨迹是否发生漂移，还包括：
依次比较每个所述特征点对的所述卡方校验误差与预设的第三阈值，并将所述卡方校验误差大于所述第三阈值的特征点对确定为卡方异常点对；
根据所述卡方异常点对的数量与所述特征点对的总数确定卡方误差异常比例，并根据所述卡方误差异常比例确定所述运动轨迹是否发生漂移。
14.根据权利要求7-13任一所述的方法，其中，所述获取多个图像帧之后，所述分别基于视觉惯性里程计的世界坐标系确定所述多个图像帧对应的多个第一位姿，并基于预建立的视觉地图的地图坐标系确定所述多个图像帧对应的多个第二位姿之前，所述方法还包括：
提取所述多个图像帧的关键特征点；
在所述视觉地图上确定与所述多个关键特征点在相匹配的多个地图特征点，并生成所述多个关键特征点和所述多个地图特征点的匹配关系。
15.根据权利要求14所述的方法，其中，所述获取多个特征点对，包括：
针对所述多个图像帧的关键特征点进行筛选，去除重复的所述关键特征点；
根据筛选后的所述关键特征点与所述地图特征点的匹配关系，确定多个特征点对。
16.根据权利要求1-15任一所述的方法，其中，所述分别基于视觉惯性里程计的世界坐标系确定所述多个图像帧对应的多个第一位姿，包括：
针对每一帧所述图像帧，确定所述图像帧的关键特征点，并根据所述关键特征点在所述世界坐标系的位置，得到所述图像帧对应的所述第一位姿。
17.根据权利要求1-16任一所述的方法，其中，所述基于预建立的视觉地图的地图坐标系确定所述多个图像帧对应的多个第二位姿，包括：
针对每一所述图像帧，在与所述图像帧相匹配的所述视觉地图上，确定与所述图像帧的关键特征点相匹配的地图特征点；
根据所述地图特征点在所述地图坐标系的位置，得到所述图像帧对应的所述第二位姿。
18.根据权利要求1-17任一所述的方法，其中，所述基于所述多个位姿对的所述第一误差，确定所述多个第一位姿对应的运动轨迹是否发生漂移之后，所述方法还包括：
在确定所述运动轨迹发生漂移的情况下，基于所述第一误差，对当前接收的图像帧的所述第一位姿进行修正。
19.根据权利要求18所述的方法，其中，所述第一误差包括重投影误差，所述重投影误差根据权利要求8或9所述的方法得到；
所述在确定所述运动轨迹发生漂移的情况下，基于所述第一误差，对当前接收的图像帧的所述第一位姿进行修正，包括：
基于当前接收的所述图像帧对应的重投影误差，对所述当前接收的图像帧的第一扩展卡尔曼滤波变量进行修正，其中，所述第一扩展卡尔曼滤波变量包括所述第一位姿。
20.根据权利要求19所述的方法，其中，所述基于当前接收的所述图像帧对应的重投影误差，对所述当前接收的图像帧的第一扩展卡尔曼滤波变量进行修正，包括：
确定针对所述重投影误差的卡尔曼增益；
根据卡尔曼增益和所述重投影误差确定卡尔曼滤波变量的增量；
根据所述卡尔曼滤波变量的增量更新所述第一扩展卡尔曼滤波变量。
21.根据权利要求20所述的方法，其中，所述基于当前接收的所述图像帧对应的重投影误差，对所述当前接收的图像帧的第一扩展卡尔曼滤波变量进行修正之后，所述方法还包括：
对所述第一扩展卡尔曼滤波变量对应的第一协方差矩阵进行更新，得到更新后的第一协方差矩阵。
22.一种基于视觉惯性里程计的运动轨迹漂移检测装置，所述装置包括：
获取模块，用于获取多个图像帧，分别基于视觉惯性里程计的世界坐标系确定所述多个图像帧对应的多个第一位姿，并基于预建立的视觉地图的地图坐标系确定所述多个图像帧对应的多个第二位姿，其中，所述视觉地图为与所述图像帧相匹配的视觉地图；
位姿对模块，用于确定多个位姿对，其中，一个所述位姿对包括一个所述图像帧对应的所述第一位姿和所述第二位姿；
变换模块，用于针对每个所述位姿对，将所述第一位姿由所述世界坐标系变换到所述地图坐标系，得到变换后的第一位姿；
第一误差模块，用于针对每个所述位姿对，根据所述变换后的第一位姿和所述第二位姿，确定所述位姿对的第一误差；
漂移检测模块，用于基于所述多个位姿对的所述第一误差，确定所述多个第一位姿对应的运动轨迹是否发生漂移。
23.一种电子设备，包括：
至少一个处理器；以及
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-21中任一项所述的方法。
24.一种存储有计算机指令的非瞬时计算机可读存储介质，其中，所述计算机指令用于使所述计算机执行根据权利要求1-21中任一项所述的方法。
25.一种计算机程序产品，包括计算机程序，所述计算机程序在被处理器执行时实现根据权利要求1-21中任一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310748383.XA CN116958198A (zh) | 2023-06-21 | 2023-06-21 | 基于视觉惯性里程计的运动轨迹漂移检测方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310748383.XA CN116958198A (zh) | 2023-06-21 | 2023-06-21 | 基于视觉惯性里程计的运动轨迹漂移检测方法及装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116958198A true CN116958198A (zh) | 2023-10-27 |
Family
ID=88453888
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310748383.XA Pending CN116958198A (zh) | 2023-06-21 | 2023-06-21 | 基于视觉惯性里程计的运动轨迹漂移检测方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116958198A (zh) |
-
2023
- 2023-06-21 CN CN202310748383.XA patent/CN116958198A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110322500B (zh) | 即时定位与地图构建的优化方法及装置、介质和电子设备 | |
| US11994392B2 (en) | Square-root multi-state constraint Kalman filter for vision-aided inertial navigation system | |
| US20210190497A1 (en) | Simultaneous location and mapping (slam) using dual event cameras | |
| CN111561923B (zh) | 基于多传感器融合的slam制图方法、系统 | |
| US9243916B2 (en) | Observability-constrained vision-aided inertial navigation | |
| US9709404B2 (en) | Iterative Kalman Smoother for robust 3D localization for vision-aided inertial navigation | |
| CN111882607B (zh) | 一种适用于增强现实应用的视觉惯导融合位姿估计方法 | |
| Zheng et al. | Trifo-VIO: Robust and efficient stereo visual inertial odometry using points and lines | |
| CN110660098B (zh) | 基于单目视觉的定位方法和装置 | |
| US20210183100A1 (en) | Data processing method and apparatus | |
| CN110349212B (zh) | 即时定位与地图构建的优化方法及装置、介质和电子设备 | |
| US20220051031A1 (en) | Moving object tracking method and apparatus | |
| CN113066127B (zh) | 一种在线标定设备参数的视觉惯性里程计方法和系统 | |
| CN113029134B (zh) | 视觉惯性系统的初始化方法、装置、电子设备及存储介质 | |
| CN104848861A (zh) | 一种基于图像消失点识别技术的移动设备姿态测量方法 | |
| CN110645976B (zh) | 一种移动机器人的姿态估计方法及终端设备 | |
| CN116958198A (zh) | 基于视觉惯性里程计的运动轨迹漂移检测方法及装置 | |
| KR20220158628A (ko) | 깊이 보조 시각적 관성 주행 거리 측정을 위한 방법 및 장치 | |
| CN115727871A (zh) | 一种轨迹质量检测方法、装置、电子设备和存储介质 | |
| Huai | Collaborative slam with crowdsourced data | |
| CN116989775A (zh) | 基于视觉惯性里程计的位姿确定方法、装置、电子设备 | |
| CN116448105B (zh) | 位姿更新方法、装置、电子设备和存储介质 | |
| Drews et al. | Sequential pose estimation using linearized rotation matrices | |
| CN117058430B (zh) | 用于视场匹配的方法、装置、电子设备和存储介质 | |
| US20240029350A1 (en) | Computing apparatus and model generation method |
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 |