CN117949013A - 系统初始化方法、特征跟踪方法、终端设备以及存储介质 - Google Patents
系统初始化方法、特征跟踪方法、终端设备以及存储介质 Download PDFInfo
- Publication number
- CN117949013A CN117949013A CN202311848992.9A CN202311848992A CN117949013A CN 117949013 A CN117949013 A CN 117949013A CN 202311848992 A CN202311848992 A CN 202311848992A CN 117949013 A CN117949013 A CN 117949013A
- Authority
- CN
- China
- Prior art keywords
- visual
- frame
- image
- initialization
- feature
- 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
- 238000000034 method Methods 0.000 title claims abstract description 61
- 238000011423 initialization method Methods 0.000 title claims abstract description 37
- 230000000007 visual effect Effects 0.000 claims abstract description 209
- 230000010354 integration Effects 0.000 claims abstract description 28
- 230000003287 optical effect Effects 0.000 claims description 21
- 230000009466 transformation Effects 0.000 claims description 7
- 230000004927 fusion Effects 0.000 claims description 3
- 230000008569 process Effects 0.000 description 19
- 230000033001 locomotion Effects 0.000 description 15
- 238000005259 measurement Methods 0.000 description 15
- 238000010586 diagram Methods 0.000 description 9
- 238000005457 optimization Methods 0.000 description 8
- 230000005484 gravity Effects 0.000 description 7
- 230000001133 acceleration Effects 0.000 description 5
- 238000006073 displacement reaction Methods 0.000 description 5
- 239000011159 matrix material Substances 0.000 description 5
- 230000006870 function Effects 0.000 description 4
- 238000007781 pre-processing Methods 0.000 description 4
- 238000012545 processing Methods 0.000 description 4
- 230000003190 augmentative effect Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 230000008878 coupling Effects 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000005070 sampling Methods 0.000 description 2
- 241000287828 Gallus gallus Species 0.000 description 1
- 230000005856 abnormality Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000002996 descriptor matching method Methods 0.000 description 1
- 244000144992 flock Species 0.000 description 1
- 239000011521 glass Substances 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000001131 transforming effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Landscapes
- Image Analysis (AREA)
Abstract
本申请公开了一种基于视觉惯性跟踪定位的系统初始化方法、特征跟踪方法、终端设备以及计算机存储介质，包括：从多帧视觉图像中获取两帧视觉初始化帧，获取视觉初始化点三维坐标；利用视觉初始化点三维坐标以及滑窗内除两帧视觉初始化帧之外的其余视觉图像帧的重投影误差，建立第一求解方程；基于陀螺仪数据的预积分结果，设置其余视觉图像帧的相机角度；利用每一其余视觉图像帧最近的视觉初始化帧的相机位姿，作为位姿初值；通过最小化第一求解方程中的重投影误差，求解首帧以外的其余视觉图像帧的相机位姿。本申请方法使用了紧耦合的视觉图像和陀螺仪信息进行三维重建，一方面能够处理小视差的场景，另一方面能够提升三维重建的鲁棒性和精度。
Description
技术领域
本申请涉及计算机视觉技术领域，特别是涉及一种基于视觉惯性跟踪定位的系统初始化方法、特征跟踪方法、终端设备以及计算机存储介质。
背景技术
视觉惯性跟踪定位系统是计算机视觉、机器人、无人车、三维重建及增强现实/虚拟现实等领域的重要底层技术。视觉惯性跟踪定位系统的快速初始化方法和稳定高精度的特征匹配方案在增强现实和虚拟现实等领域有着重要的实用价值。
视觉惯性跟踪定位系统的初始化有较多的状态量需要估计，例如：尺度、重力方向、运动速度、初始相机位姿、陀螺仪和加速度计的偏置量等等。如何快速且鲁棒地进行初始化关系到视觉惯性跟踪定位系统的易用性和稳定性。现有的视觉惯性跟踪定位系统初始化方法都较为复杂，用户需要受到较专业的技术指导才能正常使用，还存在一定概率估计了错误的状态量，导致视觉惯性跟踪定位系统在使用过程中出现某些异常。
发明内容
本申请提供一种基于视觉惯性跟踪定位的系统初始化方法、特征跟踪方法、终端设备以及计算机存储介质。
本申请采用的一个技术方案是提供一种基于视觉惯性跟踪定位的系统初始化方法，所述系统初始化方法包括：
获取滑窗内的多帧视觉图像及其陀螺仪数据，所述视觉图像和所述陀螺仪数据为所述基于视觉惯性跟踪定位的系统中的图像采集装置和陀螺仪分别感知的数据；
从所述多帧视觉图像中获取两帧视觉初始化帧，利用所述两帧视觉初始化帧获取视觉初始化点三维坐标；
利用所述视觉初始化点三维坐标以及所述滑窗内除所述两帧视觉初始化帧之外的其余视觉图像帧的重投影误差，建立第一求解方程；
基于所述陀螺仪数据的预积分结果，设置所述其余视觉图像帧的相机角度；
利用每一所述其余视觉图像帧最近的视觉初始化帧的相机位姿，作为每一所述其余视觉图像帧的位姿初值；
将所述位姿初值和所述相机角度代入所述第一求解方程，并通过最小化所述第一求解方程中的重投影误差，求解所述滑窗内的多帧视觉图像中首帧以外的所述其余视觉图像帧的相机位姿。
其中，所述求解所述滑窗内的多帧视觉图像中首帧以外的所述其余视觉图像帧的相机位姿之后，所述系统初始化方法还包括：
利用所述滑窗内首帧视觉图像的相机位姿、所述滑窗内首帧视觉图像以外的其余视觉图像帧的误差项，建立第二求解方程；
将所述滑窗内的多帧视觉图像中首帧以外的所述其余视觉图像帧的相机位姿代入第二求解方程，通过最小化所述第二求解方程中的误差项，求解所述滑窗内的多帧视觉图像中首帧以外的所述其余视觉图像帧的优化相机位姿；
其中，所述误差项包括重投影误差以及预积分误差。
其中，所述建立第二求解方程之前，所述系统初始化方法还包括：
获取所述两帧视觉初始化帧的平均视差；
利用所述平均视差以及权重误差范围，获取平衡权重；
按照所述平衡权重将所述第二求解方程中的预积分误差和所述重投影误差进行加权融合，获得所述误差项。
其中，所述利用所述两帧视觉初始化帧获取视觉初始化点三维坐标，包括：
基于所述两帧视觉初始化帧的相对角度变换关系以及所述两帧视觉初始化帧中同一匹配点的二维坐标，确定所述两帧视觉初始化帧的相机位姿；
利用所述两帧视觉初始化帧的相机位姿，获取所述视觉初始化点三维坐标。
其中，所述系统初始化方法还包括：
基于所述两帧视觉初始化帧的惯性数据，获取所述两帧视觉初始化帧的相对角度变换关系。
其中，所述滑窗内的多帧视觉图像为4帧视觉图像。
本申请采用的另一个技术方案是提供一种基于视觉惯性跟踪定位的特征跟踪方法，所述特征跟踪方法应用于通过上述的系统初始化方法初始化后的视觉惯性跟踪定位系统；所述特征跟踪方法包括：
获取当前帧图像的特征点和特征描述符，所述特征描述符用于描述图像像素点的局部特征信息；
基于所述当前帧图像的特征点判断上一帧图像中是否存在符合光流特性的特征点；
若是，则基于所述特征点在所述当前帧图像设置当前帧先验位置和在所述上一帧图像设置上一帧先验位置；
在所述当前帧先验位置和所述上一帧先验位置进行所述特征描述符的匹配；
在所述特征描述符匹配成功的情况下，将所述特征描述符对应的像素点作为跟踪成功点。
其中，所述特征跟踪方法还包括：
在所述特征描述符匹配失败的情况下，将所述特征点对应的像素点作为所述跟踪成功点。
其中，所述基于所述当前帧图像的特征点判断上一帧图像中是否存在符合光流特性的特征点之前，所述特征跟踪方法还包括：
获取所述上一帧图像的第一相机位姿，以及所述上一帧图像到当前帧图像的惯性传感器数据；
按照所述第一相机位姿以及所述惯性传感器数据，获取所述当前帧图像的第二相机位姿；
按照所述第一相机位姿和所述第二相机位姿将所述上一帧图像的跟踪成功点投影到所述当前帧图像；
按照所述上一帧图像的跟踪成功点在所述当前帧图像的投影点所在区域匹配所述特征描述符；
将匹配成功的特征描述符对应的像素点作为所述跟踪成功点。
本申请采用的另一个技术方案是提供一种终端设备，所述终端设备包括存储器以及与所述存储器耦接的处理器；
其中，所述存储器用于存储程序数据，所述处理器用于执行所述程序数据以实现如上述的系统初始化方法，和/或特征跟踪方法。
本申请采用的另一个技术方案是提供一种计算机存储介质，所述计算机存储介质用于存储程序数据，所述程序数据在被计算机执行时，用以实现如上述的系统初始化方法，和/或特征跟踪方法。
本申请的有益效果是：本申请提供的终端设备获取滑窗内的多帧视觉图像及其陀螺仪数据，所述视觉图像和所述陀螺仪数据为所述基于视觉惯性跟踪定位的系统中的图像采集装置和陀螺仪分别感知的数据；从所述多帧视觉图像中获取两帧视觉初始化帧，利用所述两帧视觉初始化帧获取视觉初始化点三维坐标；利用所述视觉初始化点三维坐标以及所述滑窗内除所述两帧视觉初始化帧之外的其余视觉图像帧的重投影误差，建立第一求解方程；基于所述陀螺仪数据的预积分结果，设置所述其余视觉图像帧的相机角度；利用每一所述其余视觉图像帧最近的视觉初始化帧的相机位姿，作为每一所述其余视觉图像帧的位姿初值；将所述位姿初值和所述相机角度代入所述第一求解方程，并通过最小化所述求解方程中的重投影误差，求解所述滑窗内的多帧视觉图像中首帧以外的所述其余视觉图像帧的相机位姿。本申请的系统初始化方法在初始化过程中使用了紧耦合的视觉图像和陀螺仪信息进行三维重建，一方面能够处理小视差的场景，另一方面能够提升三维重建的鲁棒性和精度。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本申请提供的视觉惯性跟踪定位系统的初始化和跟踪整体流程示意图；
图2是本申请提供的视觉惯性跟踪定位的系统初始化方法一实施例的流程示意图；
图3是本申请提供的视觉惯性跟踪定位系统初始化方法的整体流程示意图；
图4是本申请提供的视觉陀螺仪三维重建的流程示意图；
图5是本申请提供的旋转时的2个视图几何结构示意图；
图6是本申请提供的VG-BA的因子示意图；
图7是本申请提供的视觉惯性跟踪定位的特征跟踪方法一实施例的流程示意图；
图8是本申请提供的视觉惯性跟踪定位的特征跟踪方法的效果流程示意图；
图9是本申请提供的终端设备一实施例的结构示意图；
图10是本申请提供的计算机存储介质一实施例的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请的一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
本申请实施例将系统快速初始化和特征混合匹配整合到一个视觉惯性跟踪定位系统中，具体请参阅图1，图1是本申请提供的视觉惯性跟踪定位系统的初始化和跟踪整体流程示意图。
如图1所述，本申请实施例的视觉惯性跟踪定位系统的整体流程主要由系统初始化和特征匹配两个关键模块组成。图1左侧显示了本申请基于视觉惯性紧密耦合的VG-SFM的初始化方案，它可以显著提高初始化的精度和成功率。图1右侧是本申请提出的混合匹配策略，它有效地集成了基于光流和基于描述符的匹配方法，显著提高了VIO的精度。
具体地，本申请实施例提供了一种快速的视觉惯性初始化方法，可以只使用较少帧图像信息即可鲁棒地计算出视觉惯性跟踪定位系统的初始化状态，例如4帧或者其他较少帧数。本申请的实施例具体提出了松耦合初始化工作流，既能够支持小视差运动的初始化，又能够进一步提升现有初始化方案的精度、稳定性和成功率。
请参阅图2和图3，图2是本申请提供的视觉惯性跟踪定位的系统初始化方法一实施例的流程示意图，图3是本申请提供的视觉惯性跟踪定位系统初始化方法的整体流程示意图。
状态初始化：高质量的初始化可以提高滤波器的收敛速度。本申请根据运动状态、运动状态和静态状态采用不同的初始化方法。为了确定当前状态，使用稀疏特征的平均位移和加速度和角速度的标准偏差。如果平均位移和标准差低于特定的阈值，则本申请确定系统处于静止状态；否则，本申请确定系统为运动状态。
在静止状态的情况下，本申请执行静态初始化。这包括将初始旋转设置为零。初始局部重力被估计为两个最近的图像帧之间的加速度计测量值的平均值，并且初始旋转与初始局部重力对齐。初始陀螺仪偏差计算为两个最近的图像帧之间的陀螺仪测量值的平均值。最后，将初始加速度偏差设为零。
在运动状态的情况下，本申请使用新的视惯性觉初始化模式去解决VIO(视觉惯性里程计，visual-inertial odometry)的初始化的问题。在这种情况下，需要通过较少的观察结果解决问题。具体地，本申请使用仅仅4帧图像和相关的IMU(惯性测量单元，InertialMeasurement Unit)数据来解决上述问题。因为当观测图像很少时，传统的使用SFM(运动恢复结构，Structure from Motion)的技术方案的处理效果较差，误差大。
进一步地，对于IMU初始化的问题，大量的参数需要预测和估计，并且视觉的观测是不重要的，虽然IMU存在噪声，但陀螺仪可以提供准确的初始方向，并且不需要场景信息，这对提高视觉SFM的精度有一定的帮助。另一方面，加速度计的状态很难估计。因此，需要在参数稳定后初始化加速度。
在此基础上，本申请设计了一个新的VI初始化通道。如图1所示。核心思想是首先与IMU陀螺仪紧密耦合，然后与加速度计松散耦合，最后一起优化所有参数。该通道由VG-SFM(视觉陀螺仪-运动恢复结构，Visual Gyroscope-Structure from Motion)、VA-Align(视觉加速度计对齐，Visual Accelerometer-Align)和VI-BA(视觉惯性集束优化，VisualInertia-Bundle Adjustment)组成。特别是对于VG-SFM，在整个过程中，应用陀螺仪集成的旋转，极大地提高了SFM的鲁棒性和精度。
如图2所述，本申请所涉及的特征跟踪方法包括以下步骤：
步骤S11：获取滑窗内的多帧视觉图像及其陀螺仪数据。
在本申请实施例中，如图1所示，在系统初始化阶段，终端设备需要获取视觉惯性跟踪定位系统的以下输入：视觉图像、陀螺仪的陀螺仪数据以及加速度计的加速度数据。
进一步地，本申请提出了一个使用少量帧滑窗进行运动初始化的方案，具体地，本申请可以将滑窗大小设置为四帧或其他帧数。采用了滑窗初始化策略，也就是只保留指定帧数的视觉图像及其惯性传感器数据，当超过该帧数时就将最旧的视觉图像及其惯性传感器数据移出滑窗。
其中，视觉图像和陀螺仪数据为基于视觉惯性跟踪定位的系统中的图像采集装置和陀螺仪分别感知的数据。
步骤S12：从多帧视觉图像中获取两帧视觉初始化帧，利用两帧视觉初始化帧获取视觉初始化点三维坐标。
在本申请实施例中，如图3所述，本申请提供的视觉惯性跟踪定位系统初始化方法主要分为三个步骤：视觉陀螺仪三维重建、视觉加速度计对齐和视觉惯性集束优化。
其中，视觉陀螺仪三维重建，VG-SFM是一种视觉和陀螺仪的紧密耦合方法。在时间短、视差小的情况下，视觉测量不能提供稳定的观测，而另一方面，陀螺仪也能提供高精度的旋转信息。因此，本申请充分利用陀螺仪积分得到的旋转信息，将其作为视觉SFM的重要先验和约束，从而得到一个更稳定的SFM解，具体请参阅图4，图4是本申请提供的视觉陀螺仪三维重建的流程示意图。
在本申请实施例中，图4的数据预处理具体为：分别对图像滑窗中的图像数据做特征匹配，对惯导数据计算陀螺仪预积分，在预处理完成后挑选视差最大的两帧作为两帧重建的初始化帧，即两帧关键帧。
具体地，本申请使用视觉数据和惯性数据来初始化系统，当获取图像后，首先对图像特征进行跟踪，进一步地，对陀螺仪的角速度进行积分。在两个视图重建之前，从初始帧中选择两帧作为关键帧。在本申请实施例中，选择具有最大视差的两帧作为关键帧。
在本申请实施例中，图4的二维场景重建具体为：从预处理阶段得到视差最大的两帧，两帧的相对角度变换关系直接使用陀螺仪预积分的结果，将该值固定，再假定两帧的相对运动距离为单位1。那么两帧的位姿重建会退化为一个线性问题，这样就可以使用两点法求解初始两帧的相机位姿。
由于预处理中的特征匹配可能存在错误的匹配关系，所以方法中使用了RANSAC(Random Sample Consensus，随机抽样一致性)方案，从而找到一组最多匹配点满足极线约束的最优位姿。使用求解得到的位姿，将满足极线约束的点全部进行三角化，得到两帧重建的三维点。
具体地，传统的SFM需要5个点进行相对位姿估计，但是在视差较小的情况下准确性较差，但是在VIO的任务中，本申请能够得到陀螺仪的初始旋转，在很小的时间间隔内准确性较高。在本申请实施例中，使用陀螺仪的旋转，而不是视觉上的，进一步通过两点相对位姿进行双视图重建，得到外极性几何关系：
n1＝f1×Rf′1
n2＝f2×Rf′2
t＝n1×n2
其中，如图5所示，图5是本申请提供的旋转时的2个视图几何结构示意图。R为第二帧图像到w第一帧图像的相对旋转，通过陀螺仪的测量值计算，P1和P2是第一帧图像和第二帧图像中的两个三维点，n1，n2是每个外极平面的法线；f1为第一帧图像对特征点1的观测点，f2为第一帧图像对特征点2的的观测点；f′1为第二帧图像对特征点1的观测点，f′2为第二帧图像对特征点2的的观测点。
假设P1和P2不在平面t共面。由于只需要估计2个自由度(无尺度)的平移参数，因此该问题是线性的，可以非常稳健地解决。
而对于二自由度问题，只需要两点来估计t，因此在RANSAC的过程中，使用2-PointRANSAC(两点随机抽样一致性)很容易选择惯性点并得到最佳参数。
在本申请实施例中，在计算出初始姿态后，终端设备继续对三维点进行三角化，并用三维点来求解其他帧。
步骤S13：利用视觉初始化点三维坐标以及滑窗内除所述两帧视觉初始化帧之外的其余视觉图像帧的重投影误差，建立第一求解方程。
在本申请实施例中，图4的VG-PnP(视觉陀螺仪-多点透视成像，VisualGyroscope-Perspective-n-Point)，即视觉陀螺仪PnP具体为：对于未参与两帧重建的滑窗剩余帧，可以通过PnP计算位姿信息。具体方案是使用该帧的陀螺仪数据的预积分结果作为角度先验，即在求解相机位姿的过程中固定相机角度的值，使用已求解过最近一帧的位姿信息作为位姿初值，通过最小化三维点的重投影误差来优化该帧的位姿并得到最优结果。
具体地，当本申请有足够的三维点和相应的二维特征时，本申请执行PnP来求解每一帧的姿态。此外，本申请使用陀螺仪的旋转作为测量，以提高视觉PnP的鲁棒性。本申请称之为VG-PnP，它将视觉和陀螺仪的数据紧密耦合。该问题的定义如下：
其中，CIk(y)为第k帧图像的IMU的预积分项，利用陀螺仪的测量值来约束旋转，其中，I表示IMU，y表示图像像素点。CVk为第k帧图像的视觉重投影误差项，使用3D-2D测量来约束6自由度姿态。V表示视觉。
需要说明的是，在VG-PnP中，认为陀螺仪的积分结果是完全准确的，因此，在VG-PnP的求解方程中，陀螺仪的预积分误差项无需优化，只需要最小化视觉重投影误差项即可，计算各个视觉图像帧的相机位姿。
步骤S14：基于陀螺仪数据的预积分结果，设置其余视觉图像帧的相机角度。
在本申请实施例中，相机角度的设置公式如下：
将各视觉图像帧对应的陀螺仪数据的预积分结果代入上述设置公式，即可得到各视觉图像帧的相机角度。
步骤S15：利用每一其余视觉图像帧最近的视觉初始化帧的相机位姿，作为每一其余视觉图像帧的位姿初值。
步骤S16：将位姿初值和相机角度代入第一求解方程，并通过最小化第一求解方程中的重投影误差，求解滑窗内的多帧视觉图像中首帧以外的其余视觉图像帧的相机位姿。
其中，IMU(惯性测量单元)由加速度计和陀螺仪组成，可以在高帧率下测量器件的旋转速度和线性加速度。但在消费者级惯性测量单元中，测量结果总是存在高斯白噪声和时间间隔偏差。两者的偏差都被建模为由零均值高斯分布驱动的随机噪声和/>而陀螺仪和加速度计测量/>和/>在图像帧k处表示为：
其中，wk为图像帧k的陀螺仪真实状态值，ak为图像帧k的加速度计真实状态值，为陀螺仪偏置值，/>为加速度计偏置值，/>为当前相机的角度，gw为陀螺仪的重力矢量。
预积分IMU测量具有较高的帧率，因此，IMU噪声模型需要先进行预积分过程。其具体过程如下：
其中，Δt为IMU测量值i和i+1之间的时间间隔；bk为获取的第k个图像是的IMU坐标系；i为离散时刻；R为变换矩阵；为预积分的三个状态量，其下标表示第i个IMU测量值，上标表示第k个图像帧；Q是R的四元数形式。
其中，上述Q函数将一个旋转从欧拉角转置为四元数，进一步定义了基于等式的IMU测量值误差：
其中，是IMU预积分在图像帧k+1和图像帧k之间测量值；/>表示世界坐标系相对于第k帧的IMU坐标系的旋转，/>表示第k帧的IMU坐标系在世界坐标系下的位移，/>表示第k帧的IMU坐标系在世界坐标系下的运动速度，Δtk表示第k+1帧与第k帧的时间差，/>表示第k帧IMU坐标系在世界坐标系下的旋转以四元数的形式表示，[]_xyz表示四元数的李代数形式。因此，IMU的惯性预积分误差表示为：
其中，为状态量从第k帧到第k+1帧转移协方差矩阵，CI为表示惯性预积分误差；CI(y)为陀螺仪相关的误差项，目标是约束旋转，y表示图像像素。
视觉测量：本申请实施例所采用的照相机模型为简单的针孔模型。视觉测量是在图像平面上的重投影误差，三维点以逆深度的形式表示。然后，在第i幅图像中的特征l，在第j幅图像中重新观察到的特征的视觉误差定义为：
其中，为特征Xl在第i幅图像观测的特征值；/>为特征Xl在第j幅图像观测的特征值。其中，u为特征点在图像坐标系下的横坐标，v为特征点在图像坐标系下的纵坐标。πC为正向投影，πC -1为反向投影，/>是第i帧图像的相机位姿在世界坐标系下的坐标，/>是第j帧图像的相机位姿在相机坐标系下的坐标。Xl是特征xl的三维坐标。其中，R表示相机在世界坐标系下的角度，p表示相机在世界坐标系下的位置。
进一步地，定义视觉重投影误差的公式为：
其中，V是在滑窗中，即4帧图像中观测过至少两次的特征，(l,j)是第j帧图像中的特征l，是rV(l,j)的协方差矩阵，rV(l,j)是在第i幅图像中的特征l，在第j幅图像中重新观察到的特征的视觉误差，V表示在第j幅图像上可以见到的所有特征点l的集合。
进一步地，在一些可选的实施方式中，图4的VG-BA，即视觉陀螺仪集束优化具体为：在上述过程中认为陀螺仪的积分结果是完全准确的，但实际上陀螺仪存在着时变的偏置量，因此在集束优化过程中，再加入了视觉的重投影误差项的基础上还加入了陀螺仪的预积分误差项。视觉陀螺仪集束优化会同时优化每一帧的位姿信息，陀螺仪的偏置量以及每个三维点的逆深度信息。
具体地，如图6所示，在本申请实施例计算了三维点和相机姿态的初始值之后，本申请实施例的一些可选实现方式中，进一步执行了VG-BA，这是一个视觉和陀螺仪紧密耦合的束调整。将陀螺仪偏差的初始值设为零。VG-BA有两个部分，可以将这两种误差项合并，并进一步优化总成本如下：
其中，CI(y)为惯性预积分误差，CV为视觉重投影误差；Xl,Rk,tk,bg为需要估计的状态值；Xl为第l个三维点，Rk，tk为第k帧图像的相机位姿，bg为陀螺仪的偏置。
图3所示的视觉加速度计对齐为：与一个完整的视觉惯性跟踪定位系统相比，视觉陀螺仪三维重建的结果缺少了尺度、速度和重力方向等信息。因此可以构造一个线性方程组，将尺度、速度和重力方向作为未知量进行线性求解。将求解得到的信息通过一个三维相似变换，从视觉陀螺仪三维重建的世界坐标系，变换到视觉惯性跟踪定位系统的可用的世界坐标系下。
具体地，经过VG-SFM，可以获取每一帧图像的相机位姿。为了将相机位姿与IMU数据对齐，本申请将每一帧图像的相机位姿添加比例参数放大到IMU：
其中，s，即scale为尺度因子，表示缩放比例；表示第k帧的IMU坐标系相对于第0帧的相机坐标系的旋转，/>表示第k帧的相机坐标系相对于第0帧的相机坐标系的旋转，表示相机相对于IMU坐标系的旋转。/>表示第k帧的IMU坐标系在第0帧的相机坐标系下位移，/>表示第k帧的相机坐标系在第0帧的相机坐标系下的位移，/>表示相机在IMU坐标系下的位移。
VA-Align：首先定义视觉惯性对齐的初始状态向量：
其中，为图像帧k帧中比例放大后的相机位姿中的速度，s为视觉SFM和IMU之间的比例因子，/>为图像帧c0帧的重力矢量。
根据上述初始状态向量的定义，本申请对IMU的预积分过程重新改写为：
其中，表示第0帧的相机坐标系相对于第k帧的IMU坐标系的旋转；/>表示第k帧的IMU坐标系相对于第0帧的相机坐标系的旋转；/>表示第k帧的IMU坐标系在第0帧的相机坐标系下位移；/>表示第k帧的IMU坐标系在自身坐标系下的运动速度；Δtk表示第k+1帧与第k帧的时间差。
结合重写后的IMU预积分过程，对视觉惯性跟踪定位系统的初始状态向量进行求解，得到Xinit。
图3所示的视觉惯性集束优化为：该过程会将完整的视觉重投影误差与惯性预积分误差进行联合优化，该步骤与基于滑窗优化的视觉惯性跟踪定位系统流程一致。在此基础上，为了提升极小视差下初始化的稳定性，本方法提出了一个类Sigmoid函数来平衡视觉重投影误差和惯性预积分误差权重。函数如下：
Ctotal＝w(P)*CV+CI
其中Cv表示视觉重投影误差，Ci表示惯性预积分误差，w(P)表示视觉误差的权重，用于平衡视觉重投影误差和惯性预积分误差。这样的权重方案可以在视差极小的情况下降低惯性传感器噪声对系统求解的影响。P为视觉SFM中视差最大的两帧图像的平均视差，wmax和wmin是权重的最大值和最小值。
在一种具体实施方式中，可以设置(wmax,wmin)为(e4,1)。Pmin为集束优化能够正常工作的最小视差范围的阈值，例如可以设置为20像素。
在集束优化的过程中，本申请确定的第一帧的相机位姿和偏航，因为4DoF是不可观测的。
在本申请实施例中，终端设备获取滑窗内的多帧视觉图像及其陀螺仪数据，所述视觉图像和所述陀螺仪数据为所述基于视觉惯性跟踪定位的系统中的图像采集装置和陀螺仪分别感知的数据；从所述多帧视觉图像中获取两帧视觉初始化帧，利用所述两帧视觉初始化帧获取视觉初始化点三维坐标；利用所述视觉初始化点三维坐标以及所述滑窗内除所述两帧视觉初始化帧之外的其余视觉图像帧的重投影误差，建立第一求解方程；基于所述陀螺仪数据的预积分结果，设置所述其余视觉图像帧的相机角度；利用每一所述其余视觉图像帧最近的视觉初始化帧的相机位姿，作为每一所述其余视觉图像帧的位姿初值；将所述位姿初值和所述相机角度代入所述第一求解方程，并通过最小化所述求解方程中的重投影误差，求解所述滑窗内的多帧视觉图像中首帧以外的所述其余视觉图像帧的相机位姿。本申请的系统初始化方法在初始化过程中使用了紧耦合的视觉图像和陀螺仪信息进行三维重建，一方面能够处理小视差的场景，另一方面能够提升三维重建的鲁棒性和精度。
特征匹配方案的选取会直接影响到视觉惯性跟踪定位系统的整体精度，一个好的特征跟踪方案能够显著地提升算法的整体精度和鲁棒性。目前视觉惯性定位跟踪系统的特征匹配方案比较单一：1.基于光流匹配；2.基于高效的二进制特征描述符匹配；3.基于高精度高稳定性的特征描述符匹配。
传统的VIO特征匹配可分为基于光流匹配和基于描述符匹配两种类型。基于光流匹配的方法，如KLT，总是具有连续漂移，这限制了VIO的精度。而基于描述符的方法不能有很长的跟踪轨迹。因此，本申请提出了一种新的特征匹配方法，即紧密耦合光流匹配和描述符匹配方法。特征可以从光流和描述符中进行跟踪。该方案平衡了特征轨迹的长度和精度。
本申请实施例基于上述初始化后的视觉惯性跟踪定位系统提出了一个混合匹配方案，可以将传统的光流方案和特征匹配方案进行深度融合，从而拥有更好的鲁棒性和更高的精度。
为了融合光流匹配和描述符匹配两者的优势，本申请提出了一个混合特征匹配的图像跟踪策略，具体请参阅图7和图8，图7是本申请实施例提供的视觉惯性跟踪定位的特征跟踪方法一实施例的流程示意图，图8是本申请提供的视觉惯性跟踪定位的特征跟踪方法的效果流程示意图。
如图7所述，本申请所涉及的特征跟踪方法包括以下步骤：
步骤S21：获取当前帧图像的特征点和特征描述符。
在本申请实施例中，终端设备对当前帧图像进行ORB特征提取及特征描述符计算。其中，ORB特征点采用FAST(features from accelerated segment test)算法来检测特征点。FAST核心思想就是找出那些鹤立鸡群的点，即拿一个点跟它周围的点比较，如果它和其中大部分的点都不一样就可以认为它是一个特征点。其中，特征描述符用于描述图像像素点的局部特征信息。
终端设备首先使用上一帧到当前帧的惯性传感器数据以及上一帧位姿信息估算当前帧的粗略位姿，然后将已有三维信息的地图点投影到当前帧，使用描述符进行局部区域特征匹配，最后将匹配到的点加入跟踪成功点。
具体地，终端设备获取所述上一帧图像的第一相机位姿，以及所述上一帧图像到当前帧图像的惯性传感器数据；按照所述第一相机位姿以及所述惯性传感器数据，获取所述当前帧图像的第二相机位姿；按照所述第一相机位姿和所述第二相机位姿将所述上一帧图像的跟踪成功点投影到所述当前帧图像；按照所述上一帧图像的跟踪成功点在所述当前帧图像的投影点所在区域匹配所述特征描述符；将匹配成功的特征描述符对应的像素点作为所述跟踪成功点。
步骤S22：基于当前帧图像的特征点判断上一帧图像中是否存在符合光流特性的特征点。
在本申请实施例中，对于上述步骤S21中三维投影匹配没有跟踪上的像素点，在此步骤执行二维先验匹配：
具体地，先通过上一帧与当前帧的光流计算其先验位置，如光流失败则该点跟踪失败，直接返回。光流成功，则进入步骤S23。
步骤S23：基于特征点在当前帧图像设置当前帧先验位置和在上一帧图像设置上一帧先验位置。
步骤S24：在当前帧先验位置和上一帧先验位置进行特征描述符的匹配。
步骤S25：在特征描述符匹配成功的情况下，将特征描述符对应的像素点作为跟踪成功点。
步骤S26：在特征描述符匹配失败的情况下，将特征点对应的像素点作为跟踪成功点。
在本申请实施例中，终端设备在先验位置附近的局部区域内进行描述符匹配，将匹配成功的点加入跟踪成功点；如果描述符匹配失败，则直接将光流匹配成功的点加入跟踪成功点。
进一步地，终端设备还需要根据Ransac(Random Sample Consensus，随机样本一致算法)对所有跟踪成功的点使用8点法计算基础矩阵，找到一个符合最多点分布的基础矩阵，同时将不符合的该分布的点从跟踪结果中剔除。经过Ransac处理后跟踪上的点数少于某个阈值时，则终端设备通过增加特征提取流程中未被跟踪上的新点，使总的特征点数达到该阈值。
本申请提出了一个使用四帧滑窗进行运动初始化方案，同时提升了初始化速度、降低了初始化的计算量、减少用户的运动初始化成本。
本申请在初始化过程中使用了紧耦合的视觉陀螺仪信息进行三维重建，本申请一方面能够处理小视差的场景，一方面能够提升三维重建的鲁棒性和精度。
本申请在极小视差初始化的情况下引入Sigmoid函数来平衡视觉重投影误差和惯性预积分误差的权重，这有助于提升初始化的整体精度。
本申请提出了一个光流+特征描述符的混合特征匹配方案，融合了两种方案的优点，显著提升了视觉惯性跟踪定位系统的精度和鲁棒性。
本申请的终端设备具体可以为包括但不限于：智能手机，虚拟现实头盔，增强现实眼镜以及其他同时具有图像传感器和惯性传感器模组的设备。
以上实施例，仅是对本申请的其中一种常见案例而已，并非对本申请的技术范围做任何限制，故凡是依据本申请方案的实质对以上内容所做的任何细微修改、等同变化或者修饰，均仍属于本申请技术方案的范围内。
请继续参见图8，图8是本申请提供的终端设备一实施例的结构示意图。本申请实施例的终端设备500包括处理器51、存储器52、输入输出设备53以及总线54。
该处理器51、存储器52、输入输出设备53分别与总线54相连，该存储器52中存储有程序数据，处理器51用于执行程序数据以实现上述任意实施例所述的系统初始化方法，和/或特征跟踪方法。
在本申请实施例中，处理器51还可以称为CPU(Central Processing Unit，中央处理单元)。处理器51可能是一种集成电路芯片，具有信号的处理能力。处理器51还可以是通用处理器、数字信号处理器(DSP，Digital Signal Process)、专用集成电路(ASIC，Application Specific Integrated Circuit)、现场可编程门阵列(FPGA，FieldProgrammable Gate Array)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器51也可以是任何常规的处理器等。
本申请还提供一种计算机存储介质，请继续参阅图9，图9是本申请提供的计算机存储介质一实施例的结构示意图，该计算机存储介质600中存储有程序数据61，该程序数据61在被处理器执行时，用以实现上述任意实施例的系统初始化方法，和/或特征跟踪方法。
本申请的实施例以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅为本申请的实施方式，并非因此限制本申请的专利范围，方式利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。
Claims (11)
1.一种基于视觉惯性跟踪定位的系统初始化方法，其特征在于，所述系统初始化方法包括：
获取滑窗内的多帧视觉图像及其陀螺仪数据，所述视觉图像和所述陀螺仪数据为所述基于视觉惯性跟踪定位的系统中的图像采集装置和陀螺仪分别感知的数据；
从所述多帧视觉图像中获取两帧视觉初始化帧，利用所述两帧视觉初始化帧获取视觉初始化点三维坐标；
利用所述视觉初始化点三维坐标以及所述滑窗内除所述两帧视觉初始化帧之外的其余视觉图像帧的重投影误差，建立第一求解方程；
基于所述陀螺仪数据的预积分结果，设置所述其余视觉图像帧的相机角度；
利用每一所述其余视觉图像帧最近的视觉初始化帧的相机位姿，作为每一所述其余视觉图像帧的位姿初值；
将所述位姿初值和所述相机角度代入所述第一求解方程，并通过最小化所述第一求解方程中的重投影误差，求解所述滑窗内的多帧视觉图像中首帧以外的所述其余视觉图像帧的相机位姿。
2.根据权利要求1所述的系统初始化方法，其特征在于，
所述求解所述滑窗内的多帧视觉图像中首帧以外的所述其余视觉图像帧的相机位姿之后，所述系统初始化方法还包括：
利用所述滑窗内首帧视觉图像的相机位姿、所述滑窗内首帧视觉图像以外的其余视觉图像帧的误差项，建立第二求解方程；
将所述滑窗内的多帧视觉图像中首帧以外的所述其余视觉图像帧的相机位姿代入第二求解方程，通过最小化所述第二求解方程中的误差项，求解所述滑窗内的多帧视觉图像中首帧以外的所述其余视觉图像帧的优化相机位姿；
其中，所述误差项包括重投影误差以及预积分误差。
3.根据权利要求2所述的系统初始化方法，其特征在于，
所述建立第二求解方程之前，所述系统初始化方法还包括：
获取所述两帧视觉初始化帧的平均视差；
利用所述平均视差以及权重误差范围，获取平衡权重；
按照所述平衡权重将所述第二求解方程中的预积分误差和重投影误差进行加权融合，获得所述误差项。
4.根据权利要求1所述的系统初始化方法，其特征在于，
所述利用所述两帧视觉初始化帧获取视觉初始化点三维坐标，包括：
基于所述两帧视觉初始化帧的相对角度变换关系以及所述两帧视觉初始化帧中同一匹配点的二维坐标，确定所述两帧视觉初始化帧的相机位姿；
利用所述两帧视觉初始化帧的相机位姿，获取所述视觉初始化点三维坐标。
5.根据权利要求4所述的系统初始化方法，其特征在于，
所述系统初始化方法还包括：
基于所述两帧视觉初始化帧的惯性数据，获取所述两帧视觉初始化帧的相对角度变换关系。
6.根据权利要求1所述的系数初始化方法，其特征在于，
所述滑窗内的多帧视觉图像为4帧视觉图像。
7.一种基于视觉惯性跟踪定位的特征跟踪方法，其特征在于，所述特征跟踪方法应用于通过权利要求1至6任一项所述的系统初始化方法初始化后的视觉惯性跟踪定位系统；所述特征跟踪方法包括：
获取当前帧图像的特征点和特征描述符，所述特征描述符用于描述图像像素点的局部特征信息；
基于所述当前帧图像的特征点判断上一帧图像中是否存在符合光流特性的特征点；
若是，则基于所述特征点在所述当前帧图像设置当前帧先验位置和在所述上一帧图像设置上一帧先验位置；
在所述当前帧先验位置和所述上一帧先验位置进行所述特征描述符的匹配；
在所述特征描述符匹配成功的情况下，将所述特征描述符对应的像素点作为跟踪成功点。
8.根据权利要求7所述的特征跟踪方法，其特征在于，
所述特征跟踪方法还包括：
在所述特征描述符匹配失败的情况下，将所述特征点对应的像素点作为所述跟踪成功点。
9.根据权利要求7或8所述的特征跟踪方法，其特征在于，
所述基于所述当前帧图像的特征点判断上一帧图像中是否存在符合光流特性的特征点之前，所述特征跟踪方法还包括：
获取所述上一帧图像的第一相机位姿，以及所述上一帧图像到当前帧图像的惯性传感器数据；
按照所述第一相机位姿以及所述惯性传感器数据，获取所述当前帧图像的第二相机位姿；
按照所述第一相机位姿和所述第二相机位姿将所述上一帧图像的跟踪成功点投影到所述当前帧图像；
按照所述上一帧图像的跟踪成功点在所述当前帧图像的投影点所在区域匹配所述特征描述符；
将匹配成功的特征描述符对应的像素点作为所述跟踪成功点。
10.一种终端设备，其特征在于，所述终端设备包括存储器以及与所述存储器耦接的处理器；
其中，所述存储器用于存储程序数据，所述处理器用于执行所述程序数据以实现如权利要求1至6任一项所述的系统初始化方法，和/或权利要求7至9任一项所述的特征跟踪方法。
11.一种计算机存储介质，其特征在于，所述计算机存储介质用于存储程序数据，所述程序数据在被计算机执行时，用以实现如权利要求1至6任一项所述的系统初始化方法，和/或权利要求7至9任一项所述的特征跟踪方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311848992.9A CN117949013A (zh) | 2023-12-28 | 2023-12-28 | 系统初始化方法、特征跟踪方法、终端设备以及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311848992.9A CN117949013A (zh) | 2023-12-28 | 2023-12-28 | 系统初始化方法、特征跟踪方法、终端设备以及存储介质 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117949013A true CN117949013A (zh) | 2024-04-30 |
Family
ID=90793662
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311848992.9A Pending CN117949013A (zh) | 2023-12-28 | 2023-12-28 | 系统初始化方法、特征跟踪方法、终端设备以及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117949013A (zh) |
-
2023
- 2023-12-28 CN CN202311848992.9A patent/CN117949013A/zh active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107747941B (zh) | 一种双目视觉定位方法、装置及系统 | |
| US20210190497A1 (en) | Simultaneous location and mapping (slam) using dual event cameras | |
| CN110322500B (zh) | 即时定位与地图构建的优化方法及装置、介质和电子设备 | |
| CN109029433B (zh) | 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法 | |
| CN111156998B (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
| CN112634451B (zh) | 一种融合多传感器的室外大场景三维建图方法 | |
| CN110617814A (zh) | 单目视觉和惯性传感器融合的远距离测距系统及方法 | |
| US20130335529A1 (en) | Camera pose estimation apparatus and method for augmented reality imaging | |
| US20210183100A1 (en) | Data processing method and apparatus | |
| US20220051031A1 (en) | Moving object tracking method and apparatus | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN111354043A (zh) | 一种基于多传感器融合的三维姿态估计方法及装置 | |
| CN111932674A (zh) | 一种线激光视觉惯性系统的优化方法 | |
| CN113012224B (zh) | 定位初始化方法和相关装置、设备、存储介质 | |
| CN111609868A (zh) | 一种基于改进光流法的视觉惯性里程计方法 | |
| CN116205947A (zh) | 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 | |
| CN113052897A (zh) | 定位初始化方法和相关装置、设备、存储介质 | |
| CN112179373A (zh) | 一种视觉里程计的测量方法及视觉里程计 | |
| CN114608554A (zh) | 一种手持slam设备以及机器人即时定位与建图方法 | |
| Xian et al. | Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach | |
| CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
| Huttunen et al. | A monocular camera gyroscope | |
| CN114022556A (zh) | 定位初始化方法、装置和计算机可读存储介质 | |
| Schill et al. | Estimating ego-motion in panoramic image sequences with inertial measurements | |
| Panahandeh et al. | Vision-aided inertial navigation using planar terrain features |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination |