CN111882607A - 一种适用于增强现实应用的视觉惯导融合位姿估计方法 - Google Patents
一种适用于增强现实应用的视觉惯导融合位姿估计方法 Download PDFInfo
- Publication number
- CN111882607A CN111882607A CN202010675822.5A CN202010675822A CN111882607A CN 111882607 A CN111882607 A CN 111882607A CN 202010675822 A CN202010675822 A CN 202010675822A CN 111882607 A CN111882607 A CN 111882607A
- Authority
- CN
- China
- Prior art keywords
- image
- imu
- pose
- augmented reality
- imu data
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 53
- 230000000007 visual effect Effects 0.000 title claims abstract description 47
- 230000004927 fusion Effects 0.000 title claims abstract description 24
- 230000003190 augmentative effect Effects 0.000 title claims abstract description 22
- 238000005457 optimization Methods 0.000 claims abstract description 23
- 230000033001 locomotion Effects 0.000 claims abstract description 16
- 238000007781 pre-processing Methods 0.000 claims abstract description 15
- 238000001514 detection method Methods 0.000 claims abstract description 9
- 230000001133 acceleration Effects 0.000 claims description 23
- 230000010354 integration Effects 0.000 claims description 18
- 239000011159 matrix material Substances 0.000 claims description 18
- 238000012545 processing Methods 0.000 claims description 12
- 238000006073 displacement reaction Methods 0.000 claims description 8
- 239000013078 crystal Substances 0.000 claims description 6
- 238000010606 normalization Methods 0.000 claims description 6
- 238000012549 training Methods 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 5
- 238000001914 filtration Methods 0.000 claims description 4
- 238000010276 construction Methods 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 3
- 238000012423 maintenance Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 239000000969 carrier Substances 0.000 claims description 2
- 238000000354 decomposition reaction Methods 0.000 claims description 2
- 239000013598 vector Substances 0.000 description 6
- 238000004364 calculation method Methods 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 4
- 238000013507 mapping Methods 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 238000009825 accumulation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 239000002360 explosive Substances 0.000 description 1
- 239000011521 glass Substances 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
Images
Classifications
-
- 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
- G06T7/74—Determining position or orientation of objects or cameras using feature-based methods involving reference images or patches
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/23—Clustering techniques
- G06F18/232—Non-hierarchical techniques
- G06F18/2321—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions
- G06F18/23213—Non-hierarchical techniques using statistics or function optimisation, e.g. modelling of probability density functions with fixed number of clusters, e.g. K-means clustering
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F18/00—Pattern recognition
- G06F18/20—Analysing
- G06F18/25—Fusion techniques
- G06F18/253—Fusion techniques of extracted features
-
- 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
- G06T7/248—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments involving reference images or patches
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20081—Training; Learning
Landscapes
- Engineering & Computer Science (AREA)
- Theoretical Computer Science (AREA)
- Data Mining & Analysis (AREA)
- Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- General Physics & Mathematics (AREA)
- Bioinformatics & Cheminformatics (AREA)
- Evolutionary Biology (AREA)
- Evolutionary Computation (AREA)
- Bioinformatics & Computational Biology (AREA)
- General Engineering & Computer Science (AREA)
- Artificial Intelligence (AREA)
- Life Sciences & Earth Sciences (AREA)
- Probability & Statistics with Applications (AREA)
- Multimedia (AREA)
- Navigation (AREA)
Abstract
本发明属于增强现实技术领域,具体涉及一种适用于增强现实应用的视觉惯导融合位姿估计方法。本发明包括如下步骤:步骤1、分别从图像传感器和IMU中采集图像和IMU数据;步骤2、图像和IMU数据的鲁棒预处理;步骤3、***鲁棒初始化;步骤4、启动位姿估计模块,持续不断地对***位姿进行估计并向外输出***最新的位姿;步骤5、启动回环检测和位姿图优化模块,基于点特征和线特征描述子构建图像数据库,将到来的最新关键帧图像和数据库中的图像进行对比,如果相似度大于某个阈值,则认为出现了回环,利用检测到的回环信息消除最新关键帧的累积误差。本发明能够降低位姿估计方法的累积误差并对敏捷运动的情况下保持良好的位姿估计精度和鲁棒性。
Description
技术领域
本发明属于增强现实技术领域,具体涉及一种适用于增强现实应用的视觉惯导融合位姿估计方法。
背景技术
目前,增强现实(AR)技术在世界范围内正呈现爆发式的增长态势,同时定位与建图(SLAM)技术能够为AR眼镜或者移动端AR提供鲁棒和精确的位姿估计,解决了AR中几何一致性的关键问题,因而成了AR***当中的关键技术。SLAM技术提供的鲁棒精确的位姿估计确保了AR***为用户所带来的沉浸式体验。
低精度的消费级MEMS IMU在移动端的快速普及以及诸如手机等移动端计算能力的提升为视觉和IMU惯导融合位姿估计算法在移动端上的应用提供了可能。视觉和IMU两种传感器之间的天然互补性为视觉惯导融合的位姿估计算法拥有更好的鲁棒性提供了理论依据。然而目前的视觉惯导融合位姿估计方法多是针对移动机器人的定位与建图问题而设计的。在AR应用场景中,目前的方法面临着对诸如快速运动、大角度旋转等敏捷运动不够鲁棒以及运算量偏大等诸多挑战。由此导致跟踪失败或者因为累积误差无法消除导致3D虚拟物体产生漂移,严重影响了AR应用的用户体验。
发明内容
本发明的目的在于,针对位姿估计算法对敏捷运动不够鲁棒以及累积误差不易消除的缺陷,提出了一种新型的适用于增强现实应用的视觉惯导融合位姿估计方法,该方法能够降低位姿估计方法的累积误差并对敏捷运动的情况下保持良好的位姿估计精度和鲁棒性。
本发明采用的技术方案:
一种适用于增强现实应用的视觉惯导融合位姿估计方法,具体包括如下步骤:步骤1、分别从图像传感器和IMU中采集图像和IMU数据;步骤2、图像和IMU数据的鲁棒预处理;步骤3、***鲁棒初始化;步骤4、启动位姿估计模块,持续不断地对***位姿进行估计并向外输出***最新的位姿;步骤5、启动回环检测和位姿图优化模块,基于点特征和线特征描述子构建图像数据库,将到来的最新关键帧图像和数据库中的图像进行对比,如果相似度大于某个阈值,则认为出现了回环,利用检测到的回环信息消除最新关键帧的累积误差。
所述步骤1中,所述图像传感器和IMU集成在一起构成视觉惯性模组,并开放相关的API,通过调用API即可采集得到图像和IMU数据。
在视觉惯性模组中集成一个晶振,图像和IMU数据的时间戳均由同一个晶振产生,以此降低图像和IMU数据之间的时延,保证图像和IMU数据的时间戳在硬件上的同步。
所述步骤2包括图像的鲁棒预处理和IMU数据的鲁棒预处理,
其中,图像的鲁棒预处理包括如下步骤:
步骤2.1、检测IMU数据中的加速度值,如果加速度值超过了一定的阈值,则认为发生了敏捷运动,在进一步处理之前需要使用维纳滤波对图像进行去模糊处理;如果加速值没有超过阈值,则无需进行去模糊的处理;
步骤2.2、对图像调用LSD线段提取算法,提取图像中所包含的线段;提取完线段之后,再在图中非线段处提取特征点;
IMU数据的鲁棒预处理包括如下步骤:
步骤2.3、确定IMU数据为载体加速度值和角速度值,根据图像和IMU数据所带的时间戳对图像和IMU数据进行时间戳对齐;
其中:为IMU中加速度计的输出测量值;为IMU中陀螺仪的输出测量值;at为载体的实际加速度;ωt为载体的实际角速度;为载体的实际加速度偏置;为载体的实际角速度偏置;na为载体的实际加速度噪声;nω为载体的实际角速度噪声;表示t时刻的body坐标系相较于tk时刻的旋转,这个可以通过对角速度的积分得到;
对步骤2.4当中的连续型积分公式,采用中值积分进行离散化处理,对于第i+1个IMU数据ai+1,ωi+1,中值积分的公式如下:
其中
因为[tk tk+1]时间段内的IMU输出是不会变得,所以状态增量也是不会变的,当tk时刻状态发生变化之后,只要在变化后的状态之上加上状态的增量即可快速得到tk+1时刻的状态,从而避免了重新对IMU数据进行积分,提升了算法计算效率。
利用公式
Pk+1=FPkFT+VQVT (6)
进行预积分量协方差的递推,其中Pk是tk时刻的协方差矩阵,F为误差项对状态增量的一阶导,V为误差项对噪声项的一阶导而Q为噪声项的协方差矩阵,这样,最新时刻的状态增量的协方差矩阵Pk+1得到更新和维护。
所述步骤3包括求解滑窗内所有帧的位姿、所有3D点的位置、所有线段的相关参数、重力方向、尺度因子、陀螺仪的bias以及每一图像帧所对应的速度。
所述步骤4包括如下步骤:在位姿估计模块中,***维护的核心数据结构是大小为10的滑动窗口;对于滑动窗口中包含的每一个关键帧都含有一个总的带估计变量X,变量X包含了所有需要估计的具体变量,具体地有其中x0到xn表示n+1个载体的状态,所谓状态是指在预积分中提到的位置、速度和方向,表示相机坐标系和IMU下的body系之间的外参数,λ表示特征点的逆深度,而u,w则表示线段的参数;
根据滑窗中所维护的数据结构,构建IMU残差项、特征点残差项、线特征残差项以及滑动窗口滑动时边缘化所产生的先验项;在构建完所有的残差项之后,将这些残差项一起构成联合优化函数,通过最小化联合优化函数得到状态的最优估计。
具体地步骤如下:
构建IMU残差项:
根据滑动窗口中两个图像帧之间的预积分量,构建IMU残差函数如下:
特征点残差项:假设第l个特征点首次在第i个图像帧中被观测到,转换到当前的第j个图像帧所对应的相机坐标系下,则定义视觉重投影误差为:
其中,是第l个路标点在第j个相机归一化平面中的坐标,它由实际观测数据经过反向投影得到,是测量值, 是第l个路标点在第j个相机归一化平面中可能的坐标,是一个估计值,是正切平面上的任意两个正交基,目的是为了将视觉残差投影到正切平面上,这样,每两帧之间的每一个共视特征点都可以为两帧之间提供一个视觉残差项,对两帧之间的相对位姿产生一个约束;
线特征残差项与特征点构成重投影残差类似,每一条空间直线可以构成如下的重投影误差函数:
其中,elk,j=d(zk,j,KHcwLw,j)表示空间直线的重投影误差,Lw,j为空间直线的普吕克坐标,ρ为鲁棒性核函数;
当滑动窗口开始滑动时,对滑窗中的状态进行边缘化,由边缘化可以产生先验残差;
通过联合前面几个残差函数构成联合优化项,对于该联合优化项进行优化,随后,将***最新的位姿向外输出;***都会往外输出一个最新位姿,如此不断往复,持续对***位姿进行估计并输出。
所述步骤5包括如下步骤:
步骤5.1、使用大量的特征描述子进行训练,通常采用离线的方式构建视觉词典;
步骤5.2、利用KMeans++算法进行聚类,在进行训练之前指定词典数的深度L和每层的节点数K;
步骤5.3、在建立好视觉词典后,对输入图像和历史图像之间的相似性进行对比。
与现有技术相比,本发明的有益效果在于:
(1)本发明提供一种适用于增强现实应用的视觉惯导融合位姿估计方法,在视觉前端部分构建了一个融合了点特征和线特征的视觉跟踪模块,即使是在特征点缺失的场景中也能够因为有线特征的存在而稳定工作,为后端优化持续提供准确的位姿初值,大大提升位姿估计方法的鲁棒性。
(2)如果出现敏捷运动,从相机采集到的图像可能会产生运动模糊的情况。当运动模糊产生时,视觉前端失效,后端仅靠IMU数据进行位姿估计,此时,因为IMU读数漂移的特性将不可避免地产生累积误差;为了应对敏捷运动过程中造成的运动模糊,本方法通过检测IMU中加速度计的瞬时输出,如果输出值大于某个阈值则认为出现了敏捷运动,那么在视觉前端部分将对采集到的图像使用维纳滤波进行去模糊的处理,去模糊后得到更加清晰的图像这将降低运动模糊对前端视觉跟踪的影响。
(3)对于采集到的IMU读数,本方法采用IMU预积分的方式进行处理,这样做的好处在于当滑动窗口中的带估计状态进了一次非线性优化之后可以避免IMU数据的重传,节省了大量的计算时间。
(4)为了进一步减少累积误差并提升***的鲁棒性,该方法构建了一个基于点描述子和线描述子和混合词袋模型,在混合词袋模型的基础上构建相应的关键帧的检索和匹配模块,基于该模块实现的回环检测能够对两帧图像中所包含的点特征和线特征进行检索和匹配,这种融合了点特征和线特征的检索与匹配能够提升在弱纹理场景下的回环检测和重定位的成功率提升***鲁棒性,降低累积误差。
附图说明
图1为本位姿估计方法的主要功能模块;
图2为本位姿估计方法的处理流程。
具体实施方式
下面结合附图和具体实施例对本发明提供的一种适用于增强现实应用的视觉惯导融合位姿估计方法作进一步详细说明。
如图1所示,一种视觉惯导融合位姿估计***,包括视觉前端跟踪模块、位姿估计模块、回环检测和位姿图优化模块;其中:视觉前端跟踪模块,主要功能是接收图像和IMU数据并进行鲁棒性预处理;位姿估计模块,在***进行非线性优化状态之前,需要对***进行鲁棒性初始化,获取环境的初始局部地图;回环检测和位姿图优化模块,能够提升***的鲁棒性和估计精度。
如图2所示,本发明提供的一种适用于增强现实应用的视觉惯导融合位姿估计方法,
具体包括如下步骤:
步骤1、分别从图像传感器和IMU中采集图像和IMU数据
图像传感器和IMU集成在一起构成视觉惯性模组,比如将左右两个摄像头和一个IMU器件集成在一个电路板上构成一个视觉惯性模组;并开放相关的API用于读取图像和IMU数据,在使用时通过调用API即可采集得到图像和IMU数据。
值得注意的是,视觉惯导融合位姿估计算法要求图像和IMU数据的时延尽可能低,最好是没有时延。因此,在视觉惯性模组中集成一个晶振,图像和IMU数据的时间戳均由同一个晶振产生,以此来尽可能降低图像和IMU数据之间的时延,这就保证了图像和IMU数据的时间戳在硬件上的同步。经过时间同步的图像和IMU数据能够在后续的数据处理过程中降低累积误差的产生。
步骤2、图像和IMU数据的鲁棒预处理
图像的鲁棒预处理:
在获取一帧图像之后,首先检测IMU数据中的加速度值,加速度值反映了视觉惯性模组此时的瞬时运动状态。如果加速度值超过了一定的阈值,则认为发生了敏捷运动,在进一步处理之前需要使用维纳滤波对图像进行去模糊处理,以防止敏捷运动造成了图像模糊影响后续的位姿估计精度;如果加速值没有超过阈值,则无需进行去模糊的处理。
随后,先是对图像调用LSD线段提取算法,提取图像中所包含的线段;提取完线段之后,再在图中非线段处提取特征点。
为了限制计算量需要对所提取的线段和特征点的数量进行限定,限定的策略灵活多样,比如限定线段和特征点的数量为N,优先提取L个线段之后再提取P个特征点并保持N=L+P的数量关系。
IMU数据鲁棒预处理:
对于六轴IMU而言,它的输出数据为载体加速度值和角速度值。由于IMU数据和图像是硬件同步的,因此可以根据图像和IMU数据所带的时间戳对图像和IMU数据进行时间戳对齐。
对于IMU的输出数据建模如下:
其中:为IMU中加速度计的输出测量值;为IMU中陀螺仪的输出测量值;at为载体的实际加速度;ωt为载体的实际角速度;为载体的实际加速度偏置;为载体的实际角速度偏置;na为载体的实际加速度噪声;nω为载体的实际角速度噪声;
上述公式表明,IMU中加速度计和陀螺仪的输出的测量值和分别包含了载体的实际加速度和角速度at和ωt以及相应的偏置和噪声na和nω。假设有两个图像帧为bk和bk+1并且它们对应的时间段为[tk tk+1],根据时间段[tk tk+1]内的IMU数据可以得到载体在tk+1时刻的最新状态,这些状态包括***的位移、速度和方向,分别用来表示,其表达式如下所示:
其中右上角的w表示这些状态是在世界坐标系也就是惯性坐标系下的状态,
其中
可以看到上述积分公式中,tk+1时刻的载体状态依赖于tk时刻的状态而在基于滑动窗口的位姿估计算法中,tk时刻的状态为待优化的变量,一旦tk时刻的状态经过优化,值发生变化之后,需要在值发生变化之后的基础上将[tk tk+1]时间段内的IMU数据重新积分来得到tk+1时刻的状态,这是非常耗时的;不仅如此,公式当中还含有未知的量这是t时刻从IMU的body坐标系往世界坐标系w变换的旋转矩阵,这是因为IMU输出的数据是在body系下而不是在世界坐标系的。我们要得到的是载体在世界坐标系w下的状态,而IMU器件的实际输出是在其本身的body坐标系下的。旋转矩阵是沟通两个坐标系下的量的桥梁,body坐标系下的量乘上body坐标系往世界坐标系w的旋转矩阵就可以将其转换成世界坐标系w下的量。
因为IMU器件输出的是采样后的离散值,因此对于上述的连续型积分公式,在实际应用中需要进行离散化处理。比如采用中值积分,对于第i+1个IMU数据ai+1,ωi+1,中值积分的公式如下:
其中
将[tk tk+1]时刻内的所有IMU数据都按上述公式进行处理后,可以得到时刻tk和tk+1之间的位移、速度和方向的增量其中表示四元数的乘法。a′i和ω′i分别表示第i个IMU数据ai,ωi和第i+1个IMU数据ai+1,ωi+1的中值(平均值)。
因为[tk tk+1]时间段内的IMU输出是不会变得,所以状态增量也是不会变的,当tk时刻状态发生变化之后,只要在变化后的状态之上加上(对于方向而言是乘上)状态的增量即可快速得到tk+1时刻的状态,从而避免了重新对IMU数据进行积分,提升了算法计算效率。
这里采用的是误差随时间变化的递推方程的形式,利用公式Pk+1=FPkFT+VQVT进行预积分量协方差的递推,其中Pk是tk时刻的协方差矩阵,F为误差项对状态增量的一阶导,V为误差项对噪声项的一阶导而Q为噪声项的协方差矩阵,这样,最新时刻的状态增量的协方差矩阵Pk+1可以得到更新和维护。
步骤3、***鲁棒初始化
***鲁棒初始化包含了求解滑窗内所有帧的位姿、所有3D点的位置、所有线段的相关参数、重力方向、尺度因子、陀螺仪的bias以及每一图像帧所对应的速度等。
设定滑动窗口大小为10,对于依次到来的前十帧图像,***不做任何处理,直到滑动窗口被塞满了十帧图像,此时,开始进行***的鲁棒初始化。
首先利用纯SfM方法求解滑窗内所有帧的位姿、3D点的位置和图像中线段的相关参数,在求解到了滑窗中所有帧的位姿之后,通过将纯视觉SfM的结果与IMU预积分结果进行对齐可以对陀螺仪的bias作出一个较为准确的估计;随后,通过最小化预积分增量也即***位移和速度之间的增量和预测值之间的误差,最小化该误差可以得到每一帧图像对应的速度、重力向量和尺度因子的估计值;最后对重力向量进行微调;至此,整个***的鲁棒初始化就完成了。
步骤4、启动位姿估计模块
在***完成了初始化步骤之后,就进入位姿估计模块。***在位姿估计模块要做的便是持续不断地对***位姿进行估计并向外输出***最新的位姿。在位姿估计模块中,***维护的核心数据结构是大小为10的滑动窗口;对于滑动窗口中包含的每一个关键帧都含有一个总的带估计变量X,变量X包含了所有需要估计的具体变量,具体地有其中x0到xn表示n+1个载体的状态,所谓状态是指在预积分中提到的位置、速度和方向,表示相机坐标系和IMU下的body系之间的外参数,λ表示特征点的逆深度,而u,w则表示线段的参数。
根据滑窗中所维护的数据结构,可以构建IMU残差项、特征点残差项、线特征残差项以及滑动窗口滑动时边缘化所产生的先验项;在构建完所有的残差项之后,将这些残差项一起构成联合优化函数,通过最小化联合优化函数得到状态的最优估计。
具体地步骤如下:
(1)根据滑动窗口中两个图像帧之间的预积分量,构建IMU残差函数如下:
对于滑动窗口中的每两个图像帧之间都可以构建一个IMU残差函数,该残差函数对两帧之间的相对位姿构成了一个约束。
(2)对于滑窗中两帧之间的特征点,它们可以构成重投影误差:假设第l个特征点首次在第i个图像帧中被观测到,转换到当前的第j个图像帧所对应的相机坐标系下,则可以定义视觉重投影误差为:
其中,是第l个路标点在第j个相机归一化平面中的坐标,它由实际观测数据经过反向投影得到,是测量值, 是第l个路标点在第j个相机归一化平面中可能的坐标,是一个估计值,是正切平面上的任意两个正交基,目的是为了将视觉残差投影到正切平面上,这样,每两帧之间的每一个共视特征点都可以为两帧之间提供一个视觉残差项,对两帧之间的相对位姿产生一个约束。
(3)与(2)中特征点构成重投影残差类似,每一条空间直线可以构成如下的重投影误差函数:
其中,elk,j=d(zk,j,KHcwLw,j)表示空间直线的重投影误差,Lw,j为空间直线的普吕克坐标,ρ为鲁棒性核函数。
(4)当滑动窗口开始滑动时,为了尽可能少地减少约束信息的丢失,需要对滑窗中的状态进行边缘化,由边缘化可以产生先验残差。
(5)通过联合前面几个残差函数构成联合优化项,对于该联合优化项,使用诸如G-M或者L-M之类的非线性优化方法进行优化,通过最小化该联合优化函数即可得到待估计状态X的最大后验估计,随后,将***最新的位姿向外输出。
***每当接收到一幅图像帧都会进行一次上述从(1)到(5)的整个过程,这个过程称为局部BA,也就是说每经过一次局部BA,***都会往外输出一个最新位姿,如此不断往复,持续对***位姿进行估计并输出。
步骤5、启动回环检测和位姿图优化模块
随着***运行时间的增加,微小的误差不断累积将造成较大的误差导致***输出的位姿不再准确;因此,需要有回环检测模块来消除累积误差,该模块的主要功能是基于点特征和线特征描述子构建图像数据库,将到来的最新关键帧图像和数据库中的图像进行对比,如果相似度大于某个阈值,则认为出现了回环,利用检测到的回环信息消除最新关键帧的累积误差。
具体地,在本申请实施例中构建了一个点线综合的视觉词典用于回环检测。
(1)首先使用大量的特征描述子进行训练,通常采用离线的方式构建视觉词典。
(2)然后利用KMeans++算法进行聚类,在进行训练之前指定词典数的深度L和每层的节点数K。本实例中采用的是ORB特征点的描述子和LBD直线特征描述子。这两种描述子都是256位的二进制描述子。为了区别点和线特征的描述子,在描述子中添加标志位,标志位为0表示ORB点特征描述子,标志位为1表示LBD线特征描述子。
(3)在建立好视觉词典后,就可以对输入图像和历史图像之间的相似性进行对比了。在本实例中,使用TF-IDF对不同词汇的重要程度进行加权判断。其中IDF称为逆文档频率,表示某词汇在词典中出现频率的高低,如果特征出现的频率越低,则分类时该特征的区分度就更高。TF称为词频表示某词汇在一个图像中出现的频率,如果出现的频率越高,则它的区分度就越高。对于词典中的某个词汇wi,IDF由下面的式子计算得到:
其中N表示数据集中所有特征的总数,Ni表示输出词汇wi的特征数量。对于图像中某个词汇wi,TF由下面的式子计算得出:
其中n表示图像中所有特征的总数,ni表示图像中属于词汇wi的特征数量。最后词汇wi的权重等于TF和IDF的乘积:
ηi=TFi×IDFi
(1)当一帧图像到来时,将图像离散成词包向量,词包向量的形式为:
v={(w1,η1),(w2,η2),…,(wk,ηk)}
(2)对于任意两个词包向量va和vb,使用L1范数衡量它们之间的相似性:
以上所述,仅为本发明较佳的具体实施方式,但本发明的保护范围并不局限于此,任何熟悉本领域的技术人员在本发明揭露的技术范围内,根据本发明的技术方案及其发明构思加以等同替换或改变,都应该涵盖在本发明的保护范围之内。
Claims (10)
1.一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于,具体包括如下步骤:
步骤1、分别从图像传感器和IMU中采集图像和IMU数据;
步骤2、图像和IMU数据的鲁棒预处理;
步骤3、***鲁棒初始化;
步骤4、启动位姿估计模块,持续不断地对***位姿进行估计并向外输出***最新的位姿;
步骤5、启动回环检测和位姿图优化模块,基于点特征和线特征描述子构建图像数据库,将到来的最新关键帧图像和数据库中的图像进行对比,如果相似度大于某个阈值,则认为出现了回环,利用检测到的回环信息消除最新关键帧的累积误差。
2.根据权利要求1所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:所述步骤1中,所述图像传感器和IMU集成在一起构成视觉惯性模组,并开放相关的API,通过调用API即可采集得到图像和IMU数据。
3.根据权利要求2所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:在视觉惯性模组中集成一个晶振,图像和IMU数据的时间戳均由同一个晶振产生,以此降低图像和IMU数据之间的时延,保证图像和IMU数据的时间戳在硬件上的同步。
4.根据权利要求3所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:所述步骤2包括图像的鲁棒预处理和IMU数据的鲁棒预处理,
其中,图像的鲁棒预处理包括如下步骤:
步骤2.1、检测IMU数据中的加速度值,如果加速度值超过了一定的阈值,则认为发生了敏捷运动,在进一步处理之前需要使用维纳滤波对图像进行去模糊处理;如果加速值没有超过阈值,则无需进行去模糊的处理;
步骤2.2、对图像调用LSD线段提取算法,提取图像中所包含的线段;提取完线段之后,再在图中非线段处提取特征点;
IMU数据的鲁棒预处理包括如下步骤:
步骤2.3、确定IMU数据为载体加速度值和角速度值,根据图像和IMU数据所带的时间戳对图像和IMU数据进行时间戳对齐;
步骤2.4、IMU数据预积分,确定tk时刻和tk+1时刻内,位移、速度和方向的增量,分别为
其中:为IMU中加速度计的输出测量值;为IMU中陀螺仪的输出测量值;at为载体的实际加速度;ωt为载体的实际角速度;为载体的实际加速度偏置;为载体的实际角速度偏置;na为载体的实际加速度噪声;nω为载体的实际角速度噪声;表示t时刻的body坐标系相较于tk时刻的旋转,这个可以通过对角速度的积分得到;
对步骤2.4当中的连续型积分公式,采用中值积分进行离散化处理,对于第i+1个IMU数据ai+1,ωi+1,中值积分的公式如下:
其中
6.根据权利要求5所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:利用公式
Pk+1=FPkFT+VQVT (6)
进行预积分量协方差的递推,其中Pk是tk时刻的协方差矩阵,F为误差项对状态增量的一阶导,V为误差项对噪声项的一阶导而Q为噪声项的协方差矩阵,这样,最新时刻的状态增量的协方差矩阵Pk+1得到更新和维护。
7.根据权利要求1所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:所述步骤3包括求解滑窗内所有帧的位姿、所有3D点的位置、所有线段的相关参数、重力方向、尺度因子、陀螺仪的bias以及每一图像帧所对应的速度。
8.根据权利要求1所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:所述步骤4包括如下步骤:在位姿估计模块中,***维护的核心数据结构是大小为10的滑动窗口;对于滑动窗口中包含的每一个关键帧都含有一个总的带估计变量X,变量X包含了所有需要估计的具体变量,具体地有其中x0到xn表示n+1个载体的状态,所谓状态是指在预积分中提到的位置、速度和方向,表示相机坐标系和IMU下的body系之间的外参数,λ表示特征点的逆深度,而u,w则表示线段的参数;
根据滑窗中所维护的数据结构,构建IMU残差项、特征点残差项、线特征残差项以及滑动窗口滑动时边缘化所产生的先验项;在构建完所有的残差项之后,将这些残差项一起构成联合优化函数,通过最小化联合优化函数得到状态的最优估计。
9.根据权利要求8所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:具体地步骤如下:
构建IMU残差项:
根据滑动窗口中两个图像帧之间的预积分量,构建IMU残差函数如下:
特征点残差项:假设第l个特征点首次在第i个图像帧中被观测到,转换到当前的第j个图像帧所对应的相机坐标系下,则定义视觉重投影误差为:
其中,是第l个路标点在第j个相机归一化平面中的坐标,它由实际观测数据经过反向投影得到,是测量值,是第l个路标点在第j个相机归一化平面中可能的坐标,是一个估计值,是正切平面上的任意两个正交基,目的是为了将视觉残差投影到正切平面上,这样,每两帧之间的每一个共视特征点都可以为两帧之间提供一个视觉残差项,对两帧之间的相对位姿产生一个约束;
线特征残差项与特征点构成重投影残差类似,每一条空间直线可以构成如下的重投影误差函数:
其中,elk,j=d(zk,j,KHcwLw,j)表示空间直线的重投影误差,Lw,j为空间直线的普吕克坐标,ρ为鲁棒性核函数;
当滑动窗口开始滑动时,对滑窗中的状态进行边缘化,由边缘化可以产生先验残差;
通过联合前面几个残差函数构成联合优化项,对于该联合优化项进行优化,随后,将***最新的位姿向外输出;***都会往外输出一个最新位姿,如此不断往复,持续对***位姿进行估计并输出。
10.根据权利要求1所述的一种适用于增强现实应用的视觉惯导融合位姿估计方法,其特征在于:所述步骤5包括如下步骤:
步骤5.1、使用大量的特征描述子进行训练,通常采用离线的方式构建视觉词典;
步骤5.2、利用KMeans++算法进行聚类,在进行训练之前指定词典数的深度L和每层的节点数K;
步骤5.3、在建立好视觉词典后,对输入图像和历史图像之间的相似性进行对比。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010675822.5A CN111882607B (zh) | 2020-07-14 | 2020-07-14 | 一种适用于增强现实应用的视觉惯导融合位姿估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010675822.5A CN111882607B (zh) | 2020-07-14 | 2020-07-14 | 一种适用于增强现实应用的视觉惯导融合位姿估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111882607A true CN111882607A (zh) | 2020-11-03 |
CN111882607B CN111882607B (zh) | 2021-05-04 |
Family
ID=73150141
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010675822.5A Active CN111882607B (zh) | 2020-07-14 | 2020-07-14 | 一种适用于增强现实应用的视觉惯导融合位姿估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111882607B (zh) |
Cited By (14)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112525197A (zh) * | 2020-11-23 | 2021-03-19 | 中国科学院空天信息创新研究院 | 基于图优化算法超宽带惯性导航融合位姿估计方法 |
CN112880687A (zh) * | 2021-01-21 | 2021-06-01 | 深圳市普渡科技有限公司 | 一种室内定位方法、装置、设备和计算机可读存储介质 |
CN113052855A (zh) * | 2021-02-26 | 2021-06-29 | 苏州迈思捷智能科技有限公司 | 一种基于视觉-imu-轮速计融合的语义slam方法 |
CN113074726A (zh) * | 2021-03-16 | 2021-07-06 | 深圳市慧鲤科技有限公司 | 位姿确定方法及装置、电子设备和存储介质 |
CN113091738A (zh) * | 2021-04-09 | 2021-07-09 | 安徽工程大学 | 基于视觉惯导融合的移动机器人地图构建方法及相关设备 |
CN113503872A (zh) * | 2021-06-03 | 2021-10-15 | 浙江工业大学 | 一种基于相机与消费级imu融合的低速无人车定位方法 |
CN113532431A (zh) * | 2021-07-15 | 2021-10-22 | 贵州电网有限责任公司 | 一种用于电力巡检与作业的视觉惯性slam方法 |
CN114114369A (zh) * | 2022-01-27 | 2022-03-01 | 智道网联科技(北京)有限公司 | 自动驾驶车辆定位方法和装置、电子设备和存储介质 |
CN114137955A (zh) * | 2021-10-26 | 2022-03-04 | 中国人民解放军军事科学院国防科技创新研究院 | 基于改进市场法的多机器人快速协同建图方法 |
CN114638897A (zh) * | 2022-05-18 | 2022-06-17 | 魔视智能科技(武汉)有限公司 | 基于无重叠视域的多相机***的初始化方法、***及装置 |
CN114842173A (zh) * | 2022-04-15 | 2022-08-02 | 北华航天工业学院 | 一种增强现实***及其控制方法 |
CN116205947A (zh) * | 2023-01-03 | 2023-06-02 | 哈尔滨工业大学 | 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 |
WO2024061238A1 (zh) * | 2022-09-21 | 2024-03-28 | 海信电子科技(深圳)有限公司 | 一种估计手柄位姿的方法及虚拟显示设备 |
CN118052963A (zh) * | 2024-04-16 | 2024-05-17 | 山东金东数字创意股份有限公司 | 一种降低xr长时间误差积累的方法、介质及*** |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108805149A (zh) * | 2017-05-05 | 2018-11-13 | 中兴通讯股份有限公司 | 一种视觉同步定位与地图构建的回环检测方法及装置 |
CN109465832A (zh) * | 2018-12-18 | 2019-03-15 | 哈尔滨工业大学(深圳) | 高精度视觉和imu紧融合定位方法与*** |
CN109934862A (zh) * | 2019-02-22 | 2019-06-25 | 上海大学 | 一种点线特征结合的双目视觉slam方法 |
CN110125928A (zh) * | 2019-03-27 | 2019-08-16 | 浙江工业大学 | 一种基于前后帧进行特征匹配的双目惯导slam*** |
CN110567453A (zh) * | 2019-08-21 | 2019-12-13 | 北京理工大学 | 仿生眼多通道imu与相机硬件时间同步方法和装置 |
WO2020076952A1 (en) * | 2018-10-09 | 2020-04-16 | Google Llc | Placing augmented reality objects in an image based on device position |
CN111156984A (zh) * | 2019-12-18 | 2020-05-15 | 东南大学 | 一种面向动态场景的单目视觉惯性slam方法 |
-
2020
- 2020-07-14 CN CN202010675822.5A patent/CN111882607B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108805149A (zh) * | 2017-05-05 | 2018-11-13 | 中兴通讯股份有限公司 | 一种视觉同步定位与地图构建的回环检测方法及装置 |
WO2020076952A1 (en) * | 2018-10-09 | 2020-04-16 | Google Llc | Placing augmented reality objects in an image based on device position |
CN109465832A (zh) * | 2018-12-18 | 2019-03-15 | 哈尔滨工业大学(深圳) | 高精度视觉和imu紧融合定位方法与*** |
CN109934862A (zh) * | 2019-02-22 | 2019-06-25 | 上海大学 | 一种点线特征结合的双目视觉slam方法 |
CN110125928A (zh) * | 2019-03-27 | 2019-08-16 | 浙江工业大学 | 一种基于前后帧进行特征匹配的双目惯导slam*** |
CN110567453A (zh) * | 2019-08-21 | 2019-12-13 | 北京理工大学 | 仿生眼多通道imu与相机硬件时间同步方法和装置 |
CN111156984A (zh) * | 2019-12-18 | 2020-05-15 | 东南大学 | 一种面向动态场景的单目视觉惯性slam方法 |
Non-Patent Citations (4)
Title |
---|
TONG QIN等: "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator", 《IEEE TRANSACTIONS ON ROBOTICS》 * |
XIANGLONG KONG: "Tightly-Coupled Stereo Visual-Inertial Navigation Using Point and Line Features", 《SENSORS》 * |
梁志伟等: "基于视觉词典的单目视觉闭环检测算法", 《模式识别与人工智能》 * |
皮金柱: "基于单目视觉融合惯导的定位技术研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 * |
Cited By (20)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112525197B (zh) * | 2020-11-23 | 2022-10-28 | 中国科学院空天信息创新研究院 | 基于图优化算法超宽带惯性导航融合位姿估计方法 |
CN112525197A (zh) * | 2020-11-23 | 2021-03-19 | 中国科学院空天信息创新研究院 | 基于图优化算法超宽带惯性导航融合位姿估计方法 |
CN112880687A (zh) * | 2021-01-21 | 2021-06-01 | 深圳市普渡科技有限公司 | 一种室内定位方法、装置、设备和计算机可读存储介质 |
CN112880687B (zh) * | 2021-01-21 | 2024-05-17 | 深圳市普渡科技有限公司 | 一种室内定位方法、装置、设备和计算机可读存储介质 |
CN113052855A (zh) * | 2021-02-26 | 2021-06-29 | 苏州迈思捷智能科技有限公司 | 一种基于视觉-imu-轮速计融合的语义slam方法 |
CN113074726A (zh) * | 2021-03-16 | 2021-07-06 | 深圳市慧鲤科技有限公司 | 位姿确定方法及装置、电子设备和存储介质 |
CN113091738A (zh) * | 2021-04-09 | 2021-07-09 | 安徽工程大学 | 基于视觉惯导融合的移动机器人地图构建方法及相关设备 |
CN113503872B (zh) * | 2021-06-03 | 2024-04-12 | 浙江工业大学 | 一种基于相机与消费级imu融合的低速无人车定位方法 |
CN113503872A (zh) * | 2021-06-03 | 2021-10-15 | 浙江工业大学 | 一种基于相机与消费级imu融合的低速无人车定位方法 |
CN113532431A (zh) * | 2021-07-15 | 2021-10-22 | 贵州电网有限责任公司 | 一种用于电力巡检与作业的视觉惯性slam方法 |
CN114137955A (zh) * | 2021-10-26 | 2022-03-04 | 中国人民解放军军事科学院国防科技创新研究院 | 基于改进市场法的多机器人快速协同建图方法 |
CN114114369A (zh) * | 2022-01-27 | 2022-03-01 | 智道网联科技(北京)有限公司 | 自动驾驶车辆定位方法和装置、电子设备和存储介质 |
CN114842173A (zh) * | 2022-04-15 | 2022-08-02 | 北华航天工业学院 | 一种增强现实***及其控制方法 |
CN114842173B (zh) * | 2022-04-15 | 2023-08-29 | 北华航天工业学院 | 一种增强现实***及其控制方法 |
CN114638897A (zh) * | 2022-05-18 | 2022-06-17 | 魔视智能科技(武汉)有限公司 | 基于无重叠视域的多相机***的初始化方法、***及装置 |
WO2024061238A1 (zh) * | 2022-09-21 | 2024-03-28 | 海信电子科技(深圳)有限公司 | 一种估计手柄位姿的方法及虚拟显示设备 |
CN116205947A (zh) * | 2023-01-03 | 2023-06-02 | 哈尔滨工业大学 | 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 |
CN116205947B (zh) * | 2023-01-03 | 2024-06-07 | 哈尔滨工业大学 | 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 |
CN118052963A (zh) * | 2024-04-16 | 2024-05-17 | 山东金东数字创意股份有限公司 | 一种降低xr长时间误差积累的方法、介质及*** |
CN118052963B (zh) * | 2024-04-16 | 2024-06-25 | 山东金东数字创意股份有限公司 | 一种降低xr长时间误差积累的方法、介质及*** |
Also Published As
Publication number | Publication date |
---|---|
CN111882607B (zh) | 2021-05-04 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111882607B (zh) | 一种适用于增强现实应用的视觉惯导融合位姿估计方法 | |
US11668571B2 (en) | Simultaneous localization and mapping (SLAM) using dual event cameras | |
CN111156984B (zh) | 一种面向动态场景的单目视觉惯性slam方法 | |
CN111275763B (zh) | 闭环检测***、多传感器融合slam***及机器人 | |
CN109307508B (zh) | 一种基于多关键帧的全景惯导slam方法 | |
CN109993113B (zh) | 一种基于rgb-d和imu信息融合的位姿估计方法 | |
CN112634451B (zh) | 一种融合多传感器的室外大场景三维建图方法 | |
Li et al. | Real-time motion tracking on a cellphone using inertial sensing and a rolling-shutter camera | |
CN110702107A (zh) | 一种单目视觉惯性组合的定位导航方法 | |
CN111258313A (zh) | 多传感器融合slam***及机器人 | |
CN112649016A (zh) | 一种基于点线初始化的视觉惯性里程计方法 | |
US20220051031A1 (en) | Moving object tracking method and apparatus | |
CN110660098B (zh) | 基于单目视觉的定位方法和装置 | |
CN111609868A (zh) | 一种基于改进光流法的视觉惯性里程计方法 | |
CN111932674A (zh) | 一种线激光视觉惯性***的优化方法 | |
CN113899364B (zh) | 定位方法及装置、设备、存储介质 | |
WO2022188334A1 (zh) | 定位初始化方法和装置、设备、存储介质、程序产品 | |
CN114485640A (zh) | 基于点线特征的单目视觉惯性同步定位与建图方法及*** | |
CN116007609A (zh) | 一种多光谱图像和惯导融合的定位方法和计算*** | |
CN114022556A (zh) | 定位初始化方法、装置和计算机可读存储介质 | |
CN114440877B (zh) | 一种异步多相机视觉惯性里程计定位方法 | |
US20240087155A1 (en) | Prior informed pose and scale estimation | |
CN113847907A (zh) | 定位方法及装置、设备、存储介质 | |
Drews et al. | Sequential pose estimation using linearized rotation matrices | |
CN116681759B (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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |