CN112683305B - 一种基于点线特征的视觉-惯性里程计状态估计方法 - Google Patents
一种基于点线特征的视觉-惯性里程计状态估计方法 Download PDFInfo
- Publication number
- CN112683305B CN112683305B CN202011401030.5A CN202011401030A CN112683305B CN 112683305 B CN112683305 B CN 112683305B CN 202011401030 A CN202011401030 A CN 202011401030A CN 112683305 B CN112683305 B CN 112683305B
- Authority
- CN
- China
- Prior art keywords
- feature
- frame
- point
- characteristic
- module
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 81
- 238000001514 detection method Methods 0.000 claims abstract description 107
- 238000005457 optimization Methods 0.000 claims abstract description 94
- 230000010354 integration Effects 0.000 claims abstract description 92
- 230000000007 visual effect Effects 0.000 claims abstract description 84
- 238000012545 processing Methods 0.000 claims abstract description 15
- 238000010276 construction Methods 0.000 claims description 12
- 238000007792 addition Methods 0.000 claims description 7
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 230000008030 elimination Effects 0.000 claims description 6
- 238000003379 elimination reaction Methods 0.000 claims description 6
- 239000000284 extract Substances 0.000 claims description 6
- 238000012216 screening Methods 0.000 claims description 5
- 238000000354 decomposition reaction Methods 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 3
- 230000003287 optical effect Effects 0.000 claims description 3
- 239000013589 supplement Substances 0.000 claims description 3
- 230000007547 defect Effects 0.000 abstract description 5
- 241000714197 Avian myeloblastosis-associated virus Species 0.000 description 16
- 230000006870 function Effects 0.000 description 10
- 230000008569 process Effects 0.000 description 9
- 238000002474 experimental method Methods 0.000 description 8
- 238000012360 testing method Methods 0.000 description 6
- 238000005516 engineering process Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000011156 evaluation Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 238000010801 machine learning Methods 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 101150037717 Mavs gene Proteins 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000001413 cellular effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000003909 pattern recognition Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
- 230000003245 working effect Effects 0.000 description 1
Images
Landscapes
- Image Analysis (AREA)
Abstract
本发明公开了一种基于点线特征的视觉‑惯性里程计状态估计方法,目的是解决现有状态估计方法耗时长、精度不高的缺点。技术方案是先构建由数据获取模块、视觉特征追踪模块、IMU预积分模块、特征重建模块、闭环检测与重定位模块、非线性优化模块组成的基于特征点和特征线的视觉‑惯性里程计***。数据获取模块获取单目相机图像帧和IMU数据;IMU预积分模块对IMU数据进行预积分处理;视觉特征追踪模块使用特征点和特征线之间的约束对特征点线进行追踪;特征重建模块通过点线约束进行特征三维重建;闭环检测与重定位模块基于特征点信息进行闭环检测;非线性优化模块构建非线性优化损失函数并求解,获取机器人状态。本发明可降低处理时间,提升精度。
Description
技术领域
本发明涉及计算机图像处理、多传感融合领域,具体涉及运用“单目相机-IMU(Inertial Measurement Unit,中文名为惯性测量单元)传感套件”完成对机器人的状态估计的方法。
背景技术
SLAM(Simultaneous Localization And Mapping,即同时定位与建图)作为机器人领域一个重要分支,近年来得到了大家的广泛研究。SLAM试图解决这样的问题:一个机器人在未知的环境中运动,如何通过对环境的观测确定自身的运动轨迹,同时构建出环境的地图。 SLAM技术正是为了实现这个目标涉及到的诸多技术的总和。一个完整的SLAM***主要包括视觉里程计部分和优化建图部分。视觉里程计部分粗略估计机器人状态,估计方法主要分为两种方法:特征点法和直接法。特征点法是目前机器人状态估计的主流方法,即首先在图像中提取特征点,对不同帧之间的特征点进行匹配,进而对匹配的特征点对做相关操作,估算相机的位姿。常用的点特征包括Harris角点、SIFT、SURF、ORB、HOG特征。不同于特征点法,直接法可以省去提取特征点的过程,直接利用图像中的灰度信息来估计机器人状态,但该方法尚不成熟,鲁棒性较差。优化建图部分根据全局信息优化机器人状态,并根据传感信息和机器人状态对机器人周围环境进行建图。
如果不需要对环境进行建图,视觉里程计就可以对机器人进行较为高效且准确的状态估计。但是仅基于一台单目相机,视觉里程计无法估计出周围环境的真实尺度,同时也会因为光线变化、运动模糊而产生较大的状态估计误差。近年来,IMU与单目相机的组合“单目相机-IMU传感套件”被更多的用在视觉里程计中,被称作视觉-惯性里程计。IMU可以很好的与单目相机形成互补,提供尺度信息以及极短时间内准确的状态估计,同时单目相机可以修正IMU长时间下的漂移问题。视觉-惯性里程计已经成为SLAM研究中一个重要领域了。
目前,大部分视觉-惯性里程计使用特征法进行特征追踪,并配合IMU数据对机器人的状态进行估计。视觉-惯性里程计的典型代表工作为VINS-Mono(VisualInertialNavigation System-Monocular,中文名为单目视觉-惯性导航***),2018年由TongQin等人在期刊IEEE Transactions on Robotics第34卷,第4期,第1004-1020页上发表的文章《VINS- Mono:Arobustandversatilemonocular visual-inertial state estimator》即《单目视觉-惯性导航***:一个鲁棒且多功能的单目视觉-惯性状态估计器》提出。VINS-Mono基于单目相机对特征点进行追踪,利用IMU预积分方法对IMU测量数据进行处理,并通过将处理后的视觉信息和IMU 信息融合,估计机器人状态。
但是特征点能提供的环境结构信息较少,在一些纹理少、特征弱的场景,工作效果并不理想。基于此,2018年Yijia He等人在期刊Sensors第18卷,第4期,第1159-1179页上发表的文章《PL-VIO:Tightly-coupled monocular visual–inertial odometry usingpoint and line features》即《点线视觉-惯性里程计:利用特征点和特征线的紧耦合单目视觉-惯性里程计》提出了一种基于特征点和特征线的视觉-惯性里程计PL-VIO(Point andLine Visual Inertial Odometry,中文名为点线视觉-惯性里程计)。PL-VIO由于在特征追踪过程中加入了线特征信息,展现出了优于仅基于特征点的视觉-惯性里程计的精度和鲁棒性。但是目前基于特征线的视觉-惯性里程计,在处理线特征时,往往使用LSD(LineSegment Detector,中文名为线段检测器,2008年由Rafael Grompone von Gioi等人在期刊IEEE transactions on pattern analysis and machine intelligence第32卷,第4期,第722-732页上发表的文章《LSD:A fast line segment detector with a falsedetection control》即《线段检测器:一种具有错误检测控制的快速线段检测器》中提出)对视觉图像中的特征线段进行提取,再利用LBD(Line Band Descriptor,中文名为带状直线区域描述符,2013年由ZhangLilian等人在期刊Journal of Visual Communication andImage Representation第24卷,第7期,第794-805页上发表的文章《An efficient androbust line segment matching approach basedonLBD descriptor and pairwisegeometric consistency》即《基于带状直线区域描述符和成对几何一致性的高效鲁棒线段匹配方法》中提出)对特征线段进行描述,最后基于对特征线段的描述对不同图像中的特征线进行匹配,这个过程耗时是特征点追踪耗时的十余倍,同时错误的特征线匹配也会对视觉-惯性里程计精度造成影响。
因此,现有的基于特征点的视觉-惯性里程计状态估计方法具有状态估计精度不高的缺点,现有的基于特征点和特征线的视觉-惯性里程计状态估计方法具有特征追踪耗时较长的缺点。
发明内容
本发明要解决的技术问题是提供一种基于特征点和特征线的视觉-惯性里程计状态估计方法,以解决基于特征点和特征线的视觉-惯性里程计状态估计方法耗时长的缺点,同时解决仅基于特征点的视觉-惯性里程计状态估计方法精度不高的缺点。
为解决此问题,本发明提出了一种快速的基于特征点和特征线的视觉-惯性里程计状态估计方法,本发明在特征追踪和特征重建过程中,使用EDLines算法(EdgeDrawingLines,中文名为边缘绘制直线,2011年由CuneytAkinlar等人在期刊PatternRecognition Letters第32 卷,第13期,第1633-1642页上发表的文章《EDLines:A real-time line segment detector with a false detection control》即《边缘绘制直线:一种具有错误测控制的实时线段检测器》中提出) 提取特征线,使用特征点和特征线之间的约束对特征线进行追踪,通过点线约束进行特征线的三维重建,一方面压缩特征处理耗时,另一方面提升状态估计精度。
具体技术方案是:
第一步,构建基于特征点和特征线的视觉-惯性里程计***。基于特征点和特征线的视觉 -惯性里程计***由数据获取模块、视觉特征追踪模块、IMU预积分模块、特征重建模块、非线性优化模块、闭环检测与重定位模块组成。
数据获取模块与视觉特征追踪模块、IMU预积分模块相连。数据获取模块从苏黎世联邦理工学院的公开视觉-惯性里程计数据集“EuRoC MAV Datasets”(European RoboticsChallenge Micro Aerial Vehicle Datasets,中文名为欧洲机器人挑战赛微型飞行器数据集,该数据集中包括双目图像帧和IMU数据)下载数据,获取双目图像帧中左目相机图像帧和IMU数据,将双目图像帧中左目相机图像帧作为单目相机图像帧,将单目相机图像帧发送给视觉特征追踪模块,将IMU数据发送给IMU预积分模块。
视觉特征追踪模块与数据获取模块、特征重建模块、闭环检测与重定位模块相连。视觉特征追踪模块从数据获取模块接收单目相机图像帧,从单目相机图像帧中提取特征点和特征线信息,对连续单目相机图像帧中的特征进行匹配和追踪,将特征点和特征线信息以及特征追踪信息发送给特征重建模块,将特征点信息发送给闭环检测与重定位模块。
IMU预积分模块与数据获取模块、非线性优化模块相连。IMU预积分模块从数据获取模块接收IMU数据,利用IMU预积分算法对连续两帧单目相机图像帧中的IMU数据进行预积分处理,将预积分处理后的IMU预积分数据发送给非线性优化模块。
特征重建模块与视觉特征追踪模块、非线性优化模块相连。特征重建模块从视觉特征追踪模块获取特征点和特征线信息以及特征追踪信息,分别构建特征点管理和特征线管理数据结构,对第一次出现的特征进行三维重建,更新特征点管理和特征线管理数据结构,将更新后的特征点管理和特征线管理数据结构发送给非线性优化模块。
闭环检测与重定位模块与视觉特征追踪模块、非线性优化模块相连。闭环检测与重定位模块从视觉特征追踪模块获取每一帧单目相机图像帧中的特征点信息并进行储存,通过特征点信息匹配,对每一帧单目相机图像帧中的特征点信息进行闭环检测,将闭环检测结果发送给非线性优化模块。
非线性优化模块与特征重建模块、IMU预积分模块、闭环检测与重定位模块相连。非线性优化模块从特征重建模块获取最新的特征点管理数据结构和特征线管理数据结构,从IMU 预积分模块获取最新两帧图像间的IMU预积分数据,从闭环检测与重定位模块获取闭环检测结果,基于特征点管理数据结构和特征线管理数据结构、IMU预积分数据、***状态估计历史数据、闭环检测结果构建非线性优化损失函数并进行求解,获取机器人状态。
第二步,令变量k=1,数据获取模块从视觉-惯性里程计数据集“EuRoC MAVDatasets”中获取双目图像帧中左目相机图像帧和IMU数据,将双目图像帧中左目相机图像帧作为单目相机图像帧。令数据获取模块接收到第一帧左目相机图像帧时的时刻为t0时刻;
2.1,t0时刻数据获取模块将第一帧左目相机图像帧发送给视觉特征追踪模块;
2.2,视觉特征追踪模块从数据获取模块得到第一帧左目相机图像帧作为第一帧单目相机图像帧,视觉特征追踪模块利用FAST算法(中文名为快速,2008年由EdwardRosten等人在期刊IEEE transactions on pattern analysis and machineintelligence第32卷,第1期,第105- 119页上发表的文章《Faster and better:A machinelearning approach to corner detection》即《更快更好:用于角点检测的机器学习方法》中提出)提取第一帧单目相机图像帧中的MAXpoint个特征点,利用EDLines算法提取第一帧单目相机图像帧中MAXline条特征线,根据特征点坐标与特征线坐标关系获取第一帧单目相机图像帧中特征线上特征点信息,并将特征点和特征线信息发送至特征重建模块,将特征点信息发送至闭环检测与重定位模块,MAXpoint、MAXline均为正整数,本发明中MAXpoint设置为200、MAXline设置为200;
2.3,特征重建模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息与特征线信息,特征重建模块构建第一帧特征点管理数据结构实例,该实例包括第一帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标,特征重建模块构建第一帧特征线管理数据结构实例,该实例包括第一帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合;
2.4,闭环检测与重定位模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息,并基于DBoW2算法(Distributed Bag of Words2算法,中文名分布式词袋2算法,2012 年由DorianGalvez-Lopez等人在期刊IEEE Transactions onRobotics第28卷,第5期,第1188- 1197页上发表的文章《Bags ofbinarywords for fastplace recognitionin image sequences》即《应用于图像序列中快速场景识别的二进制单词词袋》中提出)对第一帧单目相机图像中的特征点信息进行转化、存储。
第三步,数据获取模块从视觉-惯性里程计数据集“EuRoC MAV Datasets”中获取双目图像帧中左目相机图像帧和IMU数据,将双目图像帧中左目相机图像帧作为单目相机图像帧。数据获取模块在tk时刻从视觉-惯性里程计数据集“EuRoC MAVDatasets”中接收到第k+1帧单目相机图像帧。数据获取模块将第k+1帧单目相机图像帧发送给视觉特征追踪模块,将 [tk-1,tk]时间区间内从视觉-惯性里程计数据集“EuRoC MAV Datasets”中接收到IMU数据发送给IMU预积分模块。
第四步,视觉特征追踪模块对第k+1帧单目相机图像进行特征提取与特征追踪,过程如下:
4.1,视觉特征追踪模块利用KLT稀疏光流算法(KLT为该方法发明者BruceD.Lucas与 Takeo Kanade姓名缩写,1981年由Bruce D.Lucas等人在会议The InternationalJoint Conference on Artificial Intelligence上发表的文章《An iterative imageregistration technique with an application to stereo vision》即《迭代图像配准技术及其在双目视觉中的应用》中提出),在第 k+1帧单目相机图像中跟踪第k帧单目相机图像中的特征点,得到第k+1帧单目相机图像中的特征点集合Pk+1,Pk+1中的元素为特征点,每个特征点由其在第k+1帧单目相机图像中的坐标表示;
4.2,视觉特征追踪模块利用EDLines算法提取第k+1帧单目相机图像中的MAXline条特征线段,得到第k+1帧单目相机图像中的特征线段集合Lk+1,Lk+1中的元素为特征线段la, a=1,2,...,A,A表示Lk+1中特征线段的总数,la表示为la(sa,ea),sa为特征线段la的起点, ea为特征线段la的终点,表示sa在单目图像坐标系的x轴坐标,表示sa在单目图像坐标系的y轴坐标。
4.3,视觉特征追踪模块对Lk+1中的特征线段进行整合,删除误检测线段,合并属于同一条直线的多条线段,得到整合后的新特征线段集合Lk+1,具体步骤如下:
4.3.1,对Lk+1中的特征线段由长至短进行排序,并对特征线段进行编号;
4.3.2,选取Lk+1中的特征线段la,对la与lb(lb∈la+1,la+2,...,lA)进行整合匹配:
4.3.2.2,若lb起点sb到la的距离小于距离阈值Td,Td大小为1个像素,且终点eb到la的距离大于阈值Td,且lb所在直线与la所在直线夹角小于角度阈值Tθ,Tθ大小为5度,将 la与lb合并为la(sa,eb),转4.3.3;
4.3.3,此时la与lb合并成功,将合并后的la存入Lk+1,并将lb从Lk+1中删除,若a<A,转 4.3.2,令a=a+1;若a=A,转4.3.5;
4.3.4,此时la与lb合并不成功,若b<A,转4.3.2,令b=b+1;若b=A,且a<A,则令a=a+1,转4.3.2;若b=A,且a=A,转4.3.5;
4.3.5,得到整合后的新特征线段集合Lk+1;
4.4,视觉追踪模块根据特征点与特征线关系,对第k+1帧单目相机图像中的特征点进行增补,输出新的特征点集合Pk+1,具体步骤如下:
4.4.1,根据2.2中的设置,单目相机图像帧中特征点数最大值MAXpoint=200,设置单目图像帧中特征线段上特征点最大值MAXpoint_inline,令MAXpoint_inline=150,并统计第k+1帧单目图像中的特征点数npoint,和第k+1帧单目图像中特征线段上的特征点数npoint_inline;
4.4.2,基于第k+1帧单目图像制作相对应的掩模图像Mat1,Mat1与第k+1帧单目图像尺寸相同,Mat1所有像素为黑,只有特征线段(Lk+1中所有线段)附近像素(距离特征线段10像素以内区域)为白色,同时特征线段上的特征点(Pk+1中特征点)周围像素(以特征点为圆心,半径为20像素区域)为黑色;
4.4.3,计算第一次特征点增补数目N1:
4.4.3.1,若MAXpoint_inline>[MAXpoint-(npoint-npoint_inline)],则N1=MAXpoint- npoint,转4.4.4;
4.4.3.2,若MAXpoint_inline<[MAXpoint-(npoint-npoint_inline)],且MAXpoint_inline>npoint_inline,则N1=MAXpoint_inline-npoint_inline,转4.4.4;
4.4.4,在第k+1帧单目相机图像中,Mat1中白色像素所对应的区域内,使用FAST算法 (中文名为快速,2008年由Edward Rosten等人在期刊IEEEtransactions onpatternanalysis and machine intelligence第32卷,第1期,第105-119页上发表的文章《Fasterand better:A machine learning approach to comer detection》即《更快更好:用于角点检测的机器学习方法》中提出) 添加N1个特征点,且这些新添加的特征点之间间隔大于20个像素,将新加入的N1个特征点信息添加至特征点集合Pk+1中以更新Pk+1,并更新特征点数npoint=npoint+N1;
4.4.5,基于第k+1帧单目相机图像制作相对应的掩模图像Mat2,Mat2与第k+1帧单目图像尺寸相同,Mat2所有像素为白,只有特征点(Pk+1中所有特征点)周围像素(以特征点为圆心,半径为20像素区域)为黑色;
4.4.6,计算第二次特征点增补数目N2,若MAXpoint>npoint,则N2=MAXpoint-npoint,转4.4.7;
4.4.7,在第k+1帧单目相机图像中,Mat2中白色像素所对应的区域内,使用FAST算法添加N2个特征点,且这些新添加的特征点之间间隔大于20个像素,并将新加入的N2个特征点信息添加至特征点集合Pk+1中以更新Pk+1;并更新特征点数npoint=npoint+N2;
4.5,视觉追踪模块根据特征点与特征线关系,对第k+1帧单目相机图像中的特征线段和第k帧单目相机图像中的特征线段进行匹配,以实现单目相机图像帧间特征线的追踪,具体步骤如下:
4.5.1,对Lk+1中的特征线段进行筛选,记录Lk+1中特征线与Pk+1中特征点之间的关系(即每条特征线段上有哪些特征点),将Lk+1中特征线段上特征点数小于2的特征线段从Lk+1中剔除,对Lk+1中的特征线段由长至短重新排序,更新Lk+1;
4.5.2,对重新排序后的Lk+1中的线段进行编号,令线段lc∈Lk+1,c=1,2,...,C,C表示 Lk+1中线段的总数,lc可表示为lc(sc,ec),sc为线段lc的起点,ec为线段lc的终点,表示sc在单目图像坐标系x轴坐标,表示sc在单目图像坐标系y轴坐标;
4.5.3,选取Lk+1中的特征线段lc,在第k帧单目相机图像特征线段集合Lk中依次寻找与lc相匹配的特征线段,即属于不同图像帧的同一条特征线段:
4.5.3.1,选取Lk中的特征线段ld,ld∈Lk,d=1,2,...,D,D表示Lk中线段的总数,若交换ld起点与终点,即令ld(sd,ed)=(ed,sd),转4.5.3.2;否则,直接转4.5.3.2;
4.5.3.2,若lc与ld拥有相同的特征点数超过2个,lc与ld线段长度差值小于阈值Tl,Tl大小为30个像素,且lc起点sc到ld起点sd距离小于阈值Tpp,Tpp大小为60个像素,且lc终点 ec到ld终点ed距离小于阈值Tpp,则lc与ld匹配成功;转4.5.4;若上述条件有一项不满足,则lc与ld匹配不成功,转4.5.5;
4.5.4,此时lc与ld匹配成功,表示第k帧单目图像中的特征线段ld在第k+1帧单目图像中追踪成功,记录匹配与追踪信息,包括lc与ld在Lk+1与Lk中的序号,若c<C,令c=c+1,转4.5.3;若c=C,转4.5.6;
4.5.5,此时lc与ld匹配不成功,若d<D,令d=d+1,转4.5.3.1;若d=D且c<C,令 c=c+1,转4.5.3;若d=D且c=C,转4.5.6;
4.5.6,得到特征线匹配后第k+1帧单目相机图像与第k帧单目相机图像中特征线段的追踪匹配信息,即第k+1帧单目相机图像中特征线段与第k帧单目相机图像中特征线段之间的对应关系;
4.6,视觉追踪模块将第k+1帧单目相机图像中的特征点信息、特征线信息传送至特征重建模块,将第k+1帧单目相机图像与第k帧单目相机图像中的特征点、特征线追踪匹配信息传送至特征重建模块,将第k+1帧单目相机图像中的特征点信息传送至闭环检测与重定位模块。
第五步,IMU预积分模块对从数据获取模块得到的[tk-1,tk]时间区间内的IMU数据,采用预积分算法进行预积分处理,预积分算法是2015年由Shaojie Shen等人在会议IEEE International Conference on Robotics and Automation上发表的文章《Tightly-coupled monocular visual-inertial fusion for autonomous flight ofrotorcraftMAVs》即《用于旋翼飞行器自动驾驶飞行的紧耦合单目视觉-惯性融合技术》中提出。IMU预积分模块将预积分处理后的[tk-1,tk]时间区间内的IMU预积分值发送至非线性优化模块。
第六步,特征重建模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息、特征线信息,以及第k帧单目相机图像与第k+1帧单目相机图像中的特征点、特征线追踪匹配信息,根据以上信息分别构建第k+1帧特征点管理和特征线管理数据结构实例,对特征进行三维重建,更新特征点管理和特征线管理数据结构,将更新过的特征点管理和特征线管理数据结构发送给非线性优化模块。方法如下:
6.1,特征重建模块根据第k+1帧单目相机图像中的特征点信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征点追踪匹配信息,构建第k+1帧特征点管理数据结构实例,该实例包括第k+1帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标;
6.2,特征重建模块根据第k+1帧单目相机图像中的特征线信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征线追踪匹配信息,构建第k+1帧特征线管理数据结构实例,该实例包括第k+1帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合;
6.3,若k<4,特征检测模块将此时的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块,转第七步;
6.4,若k≥4,特征检测模块对点特征和线特征进行三维重建,方法是:
6.4.1,特征重建模块对于第k+1帧单目相机图像中的特征点,筛选出至少在第k帧中出现过的特征点,联立这些特征点在第k+1、k、k-1、k-2、k-3帧中的相机坐标系坐标信息,利用奇异值分解算法(2017年由高翔等人在著作《视觉SLAM十四讲:从理论到实践》中介绍),对这些特征点在相机坐标系中的深度进行求解,并在第k+1帧单目相机图像特征点管理数据结构实例中添加这些特征点的相机坐标系深度;
6.4.2,特征重建模块对于第k+1帧单目相机图像中的特征线,筛选出至少在第k帧中出现过的特征线lf,lf∈LF,LF表示第k+1帧单目相机图像中所有被筛选出来的特征线的集合,基于lf上特征点信息,进行三维重建,具体步骤如下:
6.4.2.1,特征重建模块将第k+1、k、k-1、k-2、k-3共计五帧图像帧中,所有lf上的特征点三维坐标转换至第k+1帧图像帧相机坐标系中,建立特征点集合储存所有被转换至第k+1帧图像帧相机坐标系中特征点的三维坐标,此时中特征点数量为
6.4.2.2,特征重建模块对中的所有特征点进行基于RANSAC(Random SampleConsensus,中文名为随机样本共识,2017年由高翔等人在著作《视觉SLAM十四讲:从理论到实践》中介绍)算法的直线拟合外点剔除,即将不属于同一三维直线的特征点从中剔除,此时中特征点数量为
6.4.3,特征重建模块将更新过的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块,转第七步。
第七步,闭环检测与重定位模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息,并基于DBoW2算法(DistributedBagofWords2算法,中文名分布式词袋2算法,2012年由DorianGalvez-Lopez等人在期刊IEEE Transactions on Robotics第28卷,第5期,第1188-1197页上发表的文章《Bags of binary words for fastplace recognitionin image sequences》即《应用于图像序列中快速场景识别的二进制单词词袋》中提出)对第k+1帧单目相机图像中的特征点信息进行转化、存储与检索,并利用VINS-Mono中的闭环检测算法,对第k+1 帧单目相机图像帧与之前的单目图像帧进行闭环检测匹配,若匹配成功,则表明发生了闭环,即机器人运行到了之前运行过的地点,闭环检测与重定位模块将闭环检测结果发送给非线性优化模块,转第八步;若匹配不成功,则表明没有发生闭环,闭环检测与重定位模块不与非线性优化模块进行交互,直接转第八步。
第八步,非线性优化模块从特征重建模块获取第k+1帧特征点管理和特征线管理数据结构,从IMU预积分模块获取[tk-1,tk]时间区间内的IMU预积分数据,从闭环检测与重定位模块获取闭环检测结果,若k<9,非线性优化模块不对***状态进行求解,直接转第九步;若 k≥9,非线性优化模块基于特征点数据、特征线数据、IMU预积分数据、***状态估计历史数据、闭环检测数据构建非线性优化损失函数并进行求解,获取机器人状态。具体步骤如下:
8.1,非线性优化模块构造滑动窗口,窗口内包括{tk-9,tk-8,...,tk-1,tk}共十个时刻内机器人***相关信息,机器人***相关信息包括从特征重建模块接收到的第k-8,k-7,...,k+1 共十帧图像帧特征点管理和特征线管理数据结构、从IMU预积分模块中接收到[tk-10,tk-9],[tk-9,tk-8],...,[tk-1,tk]共十个时间区间内的IMU预积分信息、从闭环检测与重定位模块中接收到的闭环检测信息、除tk时刻外每个时刻机器人的位姿信息;
8.2,非线性优化模块将十个时刻中所有特征线段的普吕克坐标参数转化为线段所在三维直线的正交法表达参数;
8.3,非线性优化模块根据滑动窗口的信息,构造优化变量χ:
其中,xk-9,xk-8,...,xk表示滑动窗口内十个时刻机器人***状态,λ0,λ1,...,λn表示滑动窗口内十个时刻中所有特征点在三维世界中的深度,表示滑动窗口内十个时刻中所有特征线段的正交法表达参数,ti时刻机器人***状态xi可以表达为:
其中,代表ti时刻机器人在IMU坐标系相对世界坐标系的位移向量,代表ti时刻机器人在IMU坐标系相对世界坐标系的速度向量,代表ti时刻机器人在IMU坐标系相对世界坐标系的四元数,代表ti时刻机器人在IMU坐标系中的IMU加速度计偏置,代表ti时刻机器人在IMU坐标系中的IMU陀螺仪偏置;
8.4,非线性优化模块根据8.3中构造的优化变量构造优化损失函数:
其中:
表示IMU预积分残差项,表示IMU坐标系中从ti时刻到ti+1时刻IMU预积分值,表示IMU预积分数据集合,表示IMU从ti时刻到ti+1时刻IMU预积分残差,该残差的具体构造方式同VINS-Mono;
表示闭环检测重定位残差项,若闭环检测发生在ti时刻与tv时刻之间,即tv时刻单目图像帧和ti时刻单目图像帧被判定为发生闭环检测,tv时刻可为ti时刻之前任一时刻,v<i,表示相机坐标系中ti时刻第j个特征点在tv时刻单目图像帧上的重投影误差,表示世界坐标系中tv时刻机器人的旋转四元数,表示世界坐标系中tv时刻机器人的位移向量,下标w表示该数据为世界坐标系中的数据,表示闭环检测模块中存储的图像信息集合,表示滑动窗口内所有特征点的集合,表示ti时刻闭环检测残差,该残差的具体构造方式同VINS-Mono;
8.5,非线性优化模块采用最小二乘优化方法(2017年由高翔等人在著作《视觉SLAM十四讲:从理论到实践》中介绍)对优化损失函数求解,获得tk时刻机器人位姿估计值、速度估计值、窗口内所有特征点深度估计值、窗口内所有特征线普吕克坐标参数估计值,转第九步。
第九步,令k=k+1,若还能从视觉-惯性里程计数据集“EuRoC MAV Datasets”数据集中获取双目图像帧中左目相机图像帧和IMU数据,返回第三步,若不能从视觉-惯性里程计数据集“EuRoC MAV Datasets”数据集中获取双目图像帧中左目相机图像帧和IMU数据,转第十步。
第十步,结束。
采用本发明可以达到以下技术效果:
1.本发明在视觉-惯性里程计中同时利用了特征点信息和特征线信息,相比传统基于特征点和特征线的视觉-惯性里程计估计方法,本发明首先使用EDLines提取图像帧中的特征线,然后根据特征点和特征线之间的关系对连续两帧图像中的特征线进行匹配跟踪,在维持一定数量的匹配特征线基础上,大大降低了特征线处理时间;
2.在第六步特征重建过程中,本发明利用特征点和特征线之间的关系,对特征线进行三维拟合和重建,提升了特征线三维重建的精度,提升了视觉-惯性里程计状态估计的精度。
本发明在苏黎世联邦理工学院发布的公开视觉-惯性里程计数据集“EuRoC MAVDatasets” (European Robotics Challenge Micro Aerial Vehicle Datasets,中文名为欧洲机器人挑战赛微型飞行器数据集)上进行了实验验证,并与VINS-Mono、PL-VIO等视觉-惯性里程计方法做了对比实验,实验结果表明本发明不仅提高了机器人状态估计精度,还缩短了状态估计时间。
附图说明
图1为本发明总体流程图;
图2为本发明第一步构建的基于特征点和特征线的视觉-惯性里程计***逻辑结构图;
图3为本发明与现有特征点和特征线的惯性-视觉里程计估计方法平均状态估计精度误差对比实验结果;
图4为本发明与现有基于特征点和特征线的惯性-视觉里程计方法特征追踪平均时间对比实验结果。
具体实施方式
图1为本发明总体流程图;如图1所示,本发明包括以下步骤:
第一步,构建基于特征点和特征线的视觉-惯性里程计***。基于特征点和特征线的视觉 -惯性里程计***如图2所示,由数据获取模块、视觉特征追踪模块、IMU预积分模块、特征重建模块、非线性优化模块、闭环检测与重定位模块组成。
数据获取模块与视觉特征追踪模块、IMU预积分模块相连。数据获取模块从苏黎世联邦理工学院的公开视觉-惯性里程计数据集“EuRoC MAV Datasets”下载数据,获取双目图像帧中左目相机图像帧和IMU数据,将双目图像帧中左目相机图像帧作为单目相机图像帧,将单目相机图像帧发送给视觉特征追踪模块,将IMU数据发送给IMU预积分模块。
视觉特征追踪模块与数据获取模块、特征重建模块、闭环检测与重定位模块相连。视觉特征追踪模块从数据获取模块接收单目相机图像帧,从单目相机图像帧中提取特征点和特征线信息,对连续单目相机图像帧中的特征进行匹配和追踪,将特征点和特征线信息以及特征追踪信息发送给特征重建模块,将特征点信息发送给闭环检测与重定位模块。
IMU预积分模块与数据获取模块、非线性优化模块相连。IMU预积分模块从数据获取模块接收IMU数据,利用IMU预积分算法对连续两帧单目相机图像帧中的IMU数据进行预积分处理,将预积分处理后的IMU预积分数据发送给非线性优化模块。
特征重建模块与视觉特征追踪模块、非线性优化模块相连。特征重建模块从视觉特征追踪模块获取特征点和特征线信息以及特征追踪信息,分别构建特征点管理和特征线管理数据结构,对第一次出现的特征进行三维重建,更新特征点管理和特征线管理数据结构,将更新后的特征点管理和特征线管理数据结构发送给非线性优化模块。
闭环检测与重定位模块与视觉特征追踪模块、非线性优化模块相连。闭环检测与重定位模块从视觉特征追踪模块获取每一帧单目相机图像帧中的特征点信息并进行储存,通过特征点信息匹配,对每一帧单目相机图像帧中的特征点信息进行闭环检测,将闭环检测结果发送给非线性优化模块。
非线性优化模块与特征重建模块、IMU预积分模块、闭环检测与重定位模块相连。非线性优化模块从特征重建模块获取最新的特征点管理数据结构和特征线管理数据结构,从IMU 预积分模块获取最新两帧图像间的IMU预积分数据,从闭环检测与重定位模块获取闭环检测结果,基于特征点管理数据结构和特征线管理数据结构、IMU预积分数据、***状态估计历史数据、闭环检测结果构建非线性优化损失函数并进行求解,获取机器人状态。
第二步,令变量k=1,数据获取模块从视觉-惯性里程计数据集“EuRoC MAVDatasets”中获取双目图像帧中左目相机图像帧和IMU数据,将双目图像帧中左目相机图像帧作为单目相机图像帧。令数据获取模块接收到第一帧左目相机图像帧时的时刻为t0时刻;
2.1,t0时刻数据获取模块将第一帧左目相机图像帧发送给视觉特征追踪模块;
2.2,视觉特征追踪模块从数据获取模块得到第一帧左目相机图像帧作为第一帧单目相机图像帧,视觉特征追踪模块利用FAST算法提取第一帧单目相机图像帧中的MAXpoint个特征点,利用EDLines算法提取第一帧单目相机图像帧中MAXline条特征线,根据特征点坐标与特征线坐标关系获取第一帧单目相机图像帧中特征线上特征点信息,并将特征点和特征线信息发送至特征重建模块,将特征点信息发送至闭环检测与重定位模块,MAXpoint设置为200、 MAXline设置为200;
2.3,特征重建模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息与特征线信息,特征重建模块构建第一帧特征点管理数据结构实例,该实例包括第一帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标,特征重建模块构建第一帧特征线管理数据结构实例,该实例包括第一帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合;
2.4,闭环检测与重定位模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息,并基于DBoW2算法对第一帧单目相机图像中的特征点信息进行转化、存储。
第三步,数据获取模块从视觉-惯性里程计数据集“EuRoC MAV Datasets”中获取双目图像帧中左目相机图像帧和IMU数据,将双目图像帧中左目相机图像帧作为单目相机图像帧。数据获取模块在tk时刻从视觉-惯性里程计数据集“EuRoC MAV Datasets”中接收到第k+1帧单目相机图像帧。数据获取模块将第k+1帧单目相机图像帧发送给视觉特征追踪模块,将 [tk-1,tk]时间区间内从视觉-惯性里程计数据集“EuRoC MAV Datasets”中接收到IMU数据发送给IMU预积分模块。
第四步,视觉特征追踪模块对第k+1帧单目相机图像进行特征提取与特征追踪,过程如下:
4.1,视觉特征追踪模块利用KLT稀疏光流算法,在第k+1帧单目相机图像中跟踪第k帧单目相机图像中的特征点,得到第k+1帧单目相机图像中的特征点集合Pk+1,Pk+1中的元素为特征点,每个特征点由其在第k+1帧单目相机图像中的坐标表示;
4.2,视觉特征追踪模块利用EDLines算法提取第k+1帧单目相机图像中的MAXline条特征线段,得到第k+1帧单目相机图像中的特征线段集合Lk+1,Lk+1中的元素为特征线段la, a=1,2,...,A,A表示Lk+1中特征线段的总数,la表示为la(sa,ea),sa为特征线段la的起点, ea为特征线段la的终点,表示sa在单目图像坐标系的x轴坐标,表示sa在单目图像坐标系的y轴坐标。
4.3,视觉特征追踪模块对Lk+1中的特征线段进行整合,删除误检测线段,合并属于同一条直线的多条线段,得到整合后的新特征线段集合Lk+1,具体步骤如下:
4.3.1,对Lk+1中的特征线段由长至短进行排序,并对特征线段进行编号;
4.3.2,选取Lk+1中的特征线段la,对la与lb(lb∈la+1,la+2,...,lA)进行整合匹配:
4.3.2.2,若lb起点sb到la的距离小于距离阈值Td,Td大小为1个像素,且终点eb到la的距离大于阈值Td,且lb所在直线与la所在直线夹角小于角度阈值Tθ,Tθ大小为5度,将 la与lb合并为la(sa,eb),转4.3.3;
4.3.3,此时la与lb合并成功,将合并后的la存入Lk+1,并将lb从Lk+1中删除,若a<A,转 4.3.2,令a=a+1;若a=A,转4.3.5;
4.3.4,此时la与lb合并不成功,若b<A,转4.3.2,令b=b+1;若b=A,且a<A,则令a=a+1,转4.3.2;若b=A,且a=A,转4.3.5;
4.3.5,得到整合后的新特征线段集合Lk+1;
4.4,视觉追踪模块根据特征点与特征线关系,对第k+1帧单目相机图像中的特征点进行增补,输出新的特征点集合Pk+1,具体步骤如下:
4.4.1,根据2.2中的设置,单目相机图像帧中特征点数最大值MAXpoint=200,设置单目图像帧中特征线段上特征点最大值MAXpoint_inline,MAXpoint_inline=150,并统计第k+1帧单目图像中的特征点数npoint,和第k+1帧单目图像中特征线段上的特征点数npoint_inline;
4.4.2,基于第k+1帧单目图像制作相对应的掩模图像Mat1,Mat1与第k+1帧单目图像尺寸相同,Mat1所有像素为黑,只有特征线段(Lk+1中所有线段)附近像素(距离特征线段10像素以内区域)为白色,同时特征线段上的特征点(Pk+1中特征点)周围像素(以特征点为圆心,半径为20像素区域)为黑色;
4.4.3,计算第一次特征点增补数目N1:
4.4.3.1,若MAXpoint_inline>[MAXpoint-(npoint-npoint_inline)],则N1=MAXpoint- npoint,转4.4.4;
4.4.3.2,若MAXpoint_inline<[MAXpoint-(npoint-npoint_inline)],且MAXpoint_inline>npoint_inline,则N1=MAXpoint_inline-npoint_inline,转4.4.4;
4.4.4,在第k+1帧单目相机图像中,Mat1中白色像素所对应的区域内,使用FAST算法添加N1个特征点,且这些新添加的特征点之间间隔大于20个像素,将新加入的N1个特征点信息添加至特征点集合Pk+1中以更新Pk+1,并更新特征点数npoint=npoint+N1;
4.4.5,基于第k+1帧单目相机图像制作相对应的掩模图像Mat2,Mat2与第k+1帧单目图像尺寸相同,Mat2所有像素为白,只有特征点(Pk+1中所有特征点)周围像素(以特征点为圆心,半径为20像素区域)为黑色;
4.4.6,计算第二次特征点增补数目N2,若MAXpoint>npoint,则N2=MAXpoint-npoint,转4.4.7;
4.4.7,在第k+1帧单目相机图像中,Mat2中白色像素所对应的区域内,使用FAST算法添加N2个特征点,且这些新添加的特征点之间间隔大于20个像素,并将新加入的N2个特征点信息添加至特征点集合Pk+1中以更新Pk+1;并更新特征点数npoint=npoint+N2;
4.5,视觉追踪模块根据特征点与特征线关系,对第k+1帧单目相机图像中的特征线段和第k帧单目相机图像中的特征线段进行匹配,以实现单目相机图像帧间特征线的追踪,具体步骤如下:
4.5.1,对Lk+1中的特征线段进行筛选,记录Lk+1中特征线与Pk+1中特征点之间的关系(即每条特征线段上有哪些特征点),将Lk+1中特征线段上特征点数小于2的特征线段从Lk+1中剔除,对Lk+1中的特征线段由长至短重新排序,更新Lk+1;
4.5.2,对重新排序后的Lk+1中的线段进行编号,令线段lc∈Lk+1,c=1,2,...,C,C表示 Lk+1中线段的总数,lc可表示为lc(sc,ec),sc为线段lc的起点,ec为线段lc的终点,表示sc在单目图像坐标系x轴坐标,表示sc在单目图像坐标系y轴坐标;
4.5.3,选取Lk+1中的特征线段lc,在第k帧单目相机图像特征线段集合Lk中依次寻找与lc相匹配的特征线段,即属于不同图像帧的同一条特征线段:
4.5.3.1,选取Lk中的特征线段ld,ld∈Lk,d=1,2,...,D,D表示Lk中线段的总数,若交换ld起点与终点,即令ld(sd,ed)=(ed,sd),转4.5.3.2;否则,直接转4.5.3.2;
4.5.3.2,若lc与ld拥有相同的特征点数超过2个,lc与ld线段长度差值小于阈值Tl,Tl大小为30个像素,且lc起点sc到ld起点sd距离小于阈值Tpp,Tpp大小为60个像素,且lc终点 ec到ld终点ed距离小于阈值Tpp,则lc与ld匹配成功;转4.5.4;若上述条件有一项不满足,则lc与ld匹配不成功,转4.5.5;
4.5.4,此时lc与ld匹配成功,表示第k帧单目图像中的特征线段ld在第k+1帧单目图像中追踪成功,记录匹配与追踪信息,包括lc与ld在Lk+1与Lk中的序号,若c<C,令c=c+1,转4.5.3;若c=C,转4.5.6;
4.5.5,此时lc与ld匹配不成功,若d<D,令d=d+1,转4.5.3.1;若d=D且c<C,令 c=c+1,转4.5.3;若d=D且c=C,转4.5.6;
4.5.6,得到特征线匹配后第k+1帧单目相机图像与第k帧单目相机图像中特征线段的追踪匹配信息,即第k+1帧单目相机图像中特征线段与第k帧单目相机图像中特征线段之间的对应关系;
4.6,视觉追踪模块将第k+1帧单目相机图像中的特征点信息、特征线信息传送至特征重建模块,将第k+1帧单目相机图像与第k帧单目相机图像中的特征点、特征线追踪匹配信息传送至特征重建模块,将第k+1帧单目相机图像中的特征点信息传送至闭环检测与重定位模块。
第五步,IMU预积分模块对从数据获取模块得到的[tk-1,tk]时间区间内的IMU数据,采用预积分算法进行预积分处理。IMU预积分模块将预积分处理后的[tk-1,tk]时间区间内的 IMU预积分值发送至非线性优化模块。
第六步,特征重建模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息、特征线信息,以及第k帧单目相机图像与第k+1帧单目相机图像中的特征点、特征线追踪匹配信息,根据以上信息分别构建第k+1帧特征点管理和特征线管理数据结构实例,对特征进行三维重建,更新特征点管理和特征线管理数据结构,将更新过的特征点管理和特征线管理数据结构发送给非线性优化模块。方法如下:
6.1,特征重建模块根据第k+1帧单目相机图像中的特征点信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征点追踪匹配信息,构建第k+1帧特征点管理数据结构实例,该实例包括第k+1帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标;
6.2,特征重建模块根据第k+1帧单目相机图像中的特征线信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征线追踪匹配信息,构建第k+1帧特征线管理数据结构实例,该实例包括第k+1帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合;
6.3,若k<4,特征检测模块将此时的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块,转第七步;
6.4,若k≥4,特征检测模块对点特征和线特征进行三维重建,方法是:
6.4.1,特征重建模块对于第k+1帧单目相机图像中的特征点,筛选出至少在第k帧中出现过的特征点,联立这些特征点在第k+1、k、k-1、k-2、k-3帧中的相机坐标系坐标信息,利用奇异值分解算法,对这些特征点在相机坐标系中的深度进行求解,并在第k+1帧单目相机图像特征点管理数据结构实例中添加这些特征点的相机坐标系深度;
6.4.2,特征重建模块对于第k+1帧单目相机图像中的特征线,筛选出至少在第k帧中出现过的特征线lf,lf∈LF,LF表示第k+1帧单目相机图像中所有被筛选出来的特征线的集合,基于lf上特征点信息,进行三维重建,具体步骤如下:
6.4.2.1,特征重建模块将第k+1、k、k-1、k-2、k-3共计五帧图像帧中,所有lf上的特征点三维坐标转换至第k+1帧图像帧相机坐标系中,建立特征点集合储存所有被转换至第k+1帧图像帧相机坐标系中特征点的三维坐标,此时中特征点数量为
6.4.3,特征重建模块将更新过的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块,转第七步。
第七步,闭环检测与重定位模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息,并基于DBoW2算法对第k+1帧单目相机图像中的特征点信息进行转化、存储与检索,并利用VINS-Mono中的闭环检测算法,对第k+1帧单目相机图像帧与之前的单目图像帧进行闭环检测匹配,若匹配成功,则表明发生了闭环,即机器人运行到了之前运行过的地点,闭环检测与重定位模块将闭环检测结果发送给非线性优化模块,转第八步;若匹配不成功,则表明没有发生闭环,闭环检测与重定位模块不与非线性优化模块进行交互,直接转第八步。
第八步,非线性优化模块从特征重建模块获取第k+1帧特征点管理和特征线管理数据结构,从IMU预积分模块获取[tk-1,tk]时间区间内的IMU预积分数据,从闭环检测与重定位模块获取闭环检测结果,若k<9,非线性优化模块不对***状态进行求解,直接转第九步;若 k≥9,非线性优化模块基于特征点数据、特征线数据、IMU预积分数据、***状态估计历史数据、闭环检测数据构建非线性优化损失函数并进行求解,获取机器人状态。具体步骤如下:
8.1,非线性优化模块构造滑动窗口,窗口内包括{tk-9,tk-8,...,tk-1,tk}共十个时刻内机器人***相关信息,机器人***相关信息包括从特征重建模块接收到的第k-8,k-7,...,k+1 共十帧图像帧特征点管理和特征线管理数据结构、从IMU预积分模块中接收到[tk-10,tk-9],[tk-9,tk-8],...,[tk-1,tk]共十个时间区间内的IMU预积分信息、从闭环检测与重定位模块中接收到的闭环检测信息、除tk时刻外每个时刻机器人的位姿信息;
8.2,非线性优化模块将十个时刻中所有特征线段的普吕克坐标参数转化为线段所在三维直线的正交法表达参数;
8.3,非线性优化模块根据滑动窗口的信息,构造优化变量χ:
其中,xk-9,xk-8,...,xk表示滑动窗口内十个时刻机器人***状态,λ0,λ1,...,λn表示滑动窗口内十个时刻中所有特征点在三维世界中的深度,表示滑动窗口内十个时刻中所有特征线段的正交法表达参数,ti时刻机器人***状态xi可以表达为:
其中,代表ti时刻机器人在IMU坐标系相对世界坐标系的位移向量,代表ti时刻机器人在IMU坐标系相对世界坐标系的速度向量,代表ti时刻机器人在IMU坐标系相对世界坐标系的四元数,代表ti时刻机器人在IMU坐标系中的IMU加速度计偏置,代表 ti时刻机器人在IMU坐标系中的IMU陀螺仪偏置;
8.4,非线性优化模块根据8.3中构造的优化变量构造优化损失函数:
其中:
表示IMU预积分残差项,表示IMU坐标系中从ti时刻到ti+1时刻IMU预积分值,表示IMU预积分数据集合,表示IMU从ti时刻到ti+1时刻IMU预积分残差,该残差的具体构造方式同VINS-Mono;
表示闭环检测重定位残差项,若闭环检测发生在ti时刻与tv时刻之间,即tv时刻单目图像帧和ti时刻单目图像帧被判定为发生闭环检测,tv时刻可为ti时刻之前任一时刻,v<i,表示相机坐标系中ti时刻第j个特征点在tv时刻单目图像帧上的重投影误差,表示世界坐标系中tv时刻机器人的旋转四元数,表示世界坐标系中tv时刻机器人的位移向量,下标w表示该数据为世界坐标系中的数据,表示闭环检测模块中存储的图像信息集合,表示滑动窗口内所有特征点的集合,表示ti时刻闭环检测残差,该残差的具体构造方式同VINS-Mono;
8.5,非线性优化模块采用最小二乘优化方法对优化损失函数求解,获得tk时刻机器人位姿估计值、速度估计值、窗口内所有特征点深度估计值、窗口内所有特征线普吕克坐标参数估计值,转第九步。
第九步,令k=k+1,若还能从视觉-惯性里程计数据集“EuRoC MAV Datasets”数据集中获取双目图像帧中左目相机图像帧和IMU数据,返回第三步,若不能从视觉-惯性里程计数据集“EuRoC MAV Datasets”数据集中获取双目图像帧中左目相机图像帧和IMU数据,转第十步。
第十步,结束。
图3为本发明与现有基于特征点(和特征线)的惯性-视觉里程计方法平均状态估计精度误差对比实验结果;该实验结果为在“EuRoC MAV Datasets”数据集上采用本发明和背景技术所述基于特征点或同时基于特征点和特征线的惯性-视觉里程计方法平均状态估计方法进行测试得到的结果。实验环境为一台配置为i73.2GHzCPU、32GRAM的台式机。该实验的评价指标为状态估计轨迹结果与真值的均方根误差,单位为米。图中左侧第一列为目前效果较好的视觉-惯性里程计方法名,上方第一行为数据集测试数据序列名。图中每行实验数据分别为本发明、其他视觉-惯性里程计在相同的测试数据序列、同样的实验环境下测试的结果。实验结果显示,与其它方法相比,本发明在大部分测试数据序列上都具有更低的平均跟踪误差。图中“本发明w/o loop”表示本发明去除闭环检测与重定位模块,“VINS-Mono_loop”表示 VINS-Mono带闭环检测与重定位模块,黑色粗体为该数据序列中误差最小的结果,蓝色粗体为该数据序列中误差次小的结果,“×”表示无数据。
图4为本发明与现有基于特征点和特征线的惯性-视觉里程计方法特征追踪平均时间对比实验结果。该实验与图3对应的实验的测试数据集和实验环境相同。该实验的评价指标为视觉特征追踪模块中每个部分的平均耗时和视觉特征追踪模块的总耗时,单位为秒。图中左侧第一列为不同方法名。实验结果显示,与传统的使用LSD和LBD跟踪特征线方法PL-VIO相比,本发明在对视觉特征追踪的时候总耗时更少,尤其是从提取直线到匹配直线的过程耗时更少。
Claims (8)
1.一种基于点线特征的视觉-惯性里程计状态估计方法,其特征在于包括以下步骤:
第一步,构建基于特征点和特征线的视觉-惯性里程计***;基于特征点和特征线的视觉-惯性里程计***由数据获取模块、视觉特征追踪模块、IMU预积分模块、特征重建模块、非线性优化模块、闭环检测与重定位模块组成;
数据获取模块与视觉特征追踪模块、IMU预积分模块相连;数据获取模块从视觉-惯性里程计数据集下载数据,获取双目图像帧中左目相机图像帧和IMU数据,将双目图像帧中左目相机图像帧作为单目相机图像帧,将单目相机图像帧发送给视觉特征追踪模块,将IMU数据发送给IMU预积分模块;
视觉特征追踪模块与数据获取模块、特征重建模块、闭环检测与重定位模块相连;视觉特征追踪模块从数据获取模块接收单目相机图像帧,从单目相机图像帧中提取特征点和特征线信息,对连续单目相机图像帧中的特征进行匹配和追踪,将特征点和特征线信息以及特征追踪信息发送给特征重建模块,将特征点信息发送给闭环检测与重定位模块;
IMU预积分模块与数据获取模块、非线性优化模块相连;IMU预积分模块从数据获取模块接收IMU数据,利用IMU预积分算法对连续两帧单目相机图像帧中的IMU数据进行预积分处理,将预积分处理后的IMU预积分数据发送给非线性优化模块;
特征重建模块与视觉特征追踪模块、非线性优化模块相连;特征重建模块从视觉特征追踪模块获取特征点和特征线信息以及特征追踪信息,分别构建特征点管理和特征线管理数据结构,对第一次出现的特征进行三维重建,更新特征点管理和特征线管理数据结构,将更新后的特征点管理和特征线管理数据结构发送给非线性优化模块;
闭环检测与重定位模块与视觉特征追踪模块、非线性优化模块相连;闭环检测与重定位模块从视觉特征追踪模块获取每一帧单目相机图像帧中的特征点信息并进行储存,通过特征点信息匹配,对每一帧单目相机图像帧中的特征点信息进行闭环检测,将闭环检测结果发送给非线性优化模块;
非线性优化模块与特征重建模块、IMU预积分模块、闭环检测与重定位模块相连;非线性优化模块从特征重建模块获取最新的特征点管理数据结构和特征线管理数据结构,从IMU预积分模块获取最新两帧图像间的IMU预积分数据,从闭环检测与重定位模块获取闭环检测结果,基于特征点管理数据结构和特征线管理数据结构、IMU预积分数据、***状态估计历史数据、闭环检测结果构建非线性优化损失函数并进行求解,获取机器人状态;
第二步,令变量k=1,数据获取模块从视觉-惯性里程计数据集中获取双目图像帧中左目相机图像帧和IMU数据,将双目图像帧中左目相机图像帧作为单目相机图像帧;令数据获取模块接收到第一帧左目相机图像帧时的时刻为t0时刻;
2.1,t0时刻数据获取模块将第一帧左目相机图像帧发送给视觉特征追踪模块;
2.2,视觉特征追踪模块从数据获取模块得到第一帧左目相机图像帧作为第一帧单目相机图像帧,视觉特征追踪模块利用FAST算法提取第一帧单目相机图像帧中的MAXpoint个特征点,利用EDLines算法提取第一帧单目相机图像帧中MAXline条特征线,根据特征点坐标与特征线坐标关系获取第一帧单目相机图像帧中特征线上特征点信息,并将特征点和特征线信息发送至特征重建模块,将特征点信息发送至闭环检测与重定位模块,MAXpoint、MAXline均为正整数;
2.3,特征重建模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息与特征线信息,特征重建模块构建第一帧特征点管理数据结构实例,该实例包括第一帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标,特征重建模块构建第一帧特征线管理数据结构实例,该实例包括第一帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合;
2.4,闭环检测与重定位模块从视觉特征追踪模块得到第一帧单目相机图像帧中的特征点信息,采用DBoW2算法对第一帧单目相机图像中的特征点信息进行转化、存储;
第三步,数据获取模块从视觉-惯性里程计数据集中获取双目图像帧中左目相机图像帧和IMU数据,将双目图像帧中左目相机图像帧作为单目相机图像帧;数据获取模块在tk时刻从视觉-惯性里程计数据集中接收到第k+1帧单目相机图像帧;数据获取模块将第k+1帧单目相机图像帧发送给视觉特征追踪模块,将[tk-1,tk]时间区间内从视觉-惯性里程计数据集中接收到IMU数据发送给IMU预积分模块;
第四步,视觉特征追踪模块对第k+1帧单目相机图像进行特征提取与特征追踪,方法是:
4.1,视觉特征追踪模块利用KLT稀疏光流算法,在第k+1帧单目相机图像中跟踪第k帧单目相机图像中的特征点,得到第k+1帧单目相机图像中的特征点集合Pk+1,Pk+1中的元素为特征点,每个特征点由其在第k+1帧单目相机图像中的坐标表示;
4.2,视觉特征追踪模块利用EDLines算法提取第k+1帧单目相机图像中的MAXline条特征线段,得到第k+1帧单目相机图像中的特征线段集合Lk+1,Lk+1中的元素为特征线段la,a=1,2,…,A,A表示Lk+1中特征线段的总数,la表示为la(sa,ea),sa为特征线段la的起点,ea为特征线段la的终点,表示sa在单目图像坐标系的x轴坐标,表示sa在单目图像坐标系的y轴坐标;
4.3,视觉特征追踪模块对Lk+1中的特征线段进行整合,删除误检测线段,合并属于同一条直线的多条线段,得到整合后的新特征线段集合Lk+1;
4.4,视觉追踪模块根据特征点与特征线关系,对第k+1帧单目相机图像中的特征点进行增补,得到新的特征点集合Pk+1;
4.5,视觉追踪模块根据特征点与特征线关系,对第k+1帧单目相机图像中的特征线段和第k帧单目相机图像中的特征线段进行匹配,以实现单目相机图像帧间特征线的追踪,方法是:
4.5.1,对Lk+1中的特征线段进行筛选,记录Lk+1中特征线与Pk+1中特征点之间的关系即每条特征线段上有哪些特征点,将Lk+1中特征线段上特征点数小于2的特征线段从Lk+1中剔除,对Lk+1中的特征线段由长至短重新排序,更新Lk+1;
4.5.2,对重新排序后的Lk+1中的线段进行编号,令线段lc∈Lk+1,c=1,2,…,C,C表示Lk+1中线段的总数,lc可表示为lc(sc,ec),sc为线段lc的起点,ec为线段lc的终点,表示sc在单目图像坐标系x轴坐标,表示sc在单目图像坐标系y轴坐标;
4.5.3,选取Lk+1中的特征线段lc,在第k帧单目相机图像特征线段集合Lk中依次寻找与lc相匹配的特征线段,即属于不同图像帧的同一条特征线段:
4.5.3.1,选取Lk中的特征线段ld,ld∈Lk,d=1,2,…,D,D表示Lk中线段的总数,若交换ld起点与终点,即令ld(sd,ed)=(ed,sd),转4.5.3.2;否则,直接转4.5.3.2;
4.5.3.2,若lc与ld拥有相同的特征点数超过2个,lc与ld线段长度差值小于阈值Tl,Tl大小为30个像素,且lc起点sc到ld起点sd距离小于阈值Tpp,Tpp大小为60个像素,且lc终点ec到ld终点ed距离小于阈值Tpp,则lc与ld匹配成功;转4.5.4;若上述条件有一项不满足,则lc与ld匹配不成功,转4.5.5;
4.5.4,此时lc与ld匹配成功,表示第k帧单目图像中的特征线段ld在第k+1帧单目图像中追踪成功,记录匹配与追踪信息,包括lc与ld在Lk+1与Lk中的序号,若c<C,令c=c+1,转4.5.3;若c=C,转4.5.6;
4.5.5,此时lc与ld匹配不成功,若d<D,令d=d+1,转4.5.3.1;若d=D且c<C,令c=c+1,转4.5.3;若d=D且c=C,转4.5.6;
4.5.6,得到特征线匹配后第k+1帧单目相机图像与第k帧单目相机图像中特征线段的追踪匹配信息,即第k+1帧单目相机图像中特征线段与第k帧单目相机图像中特征线段之间的对应关系;
4.6,视觉追踪模块将第k+1帧单目相机图像中的特征点信息、特征线信息传送至特征重建模块,将第k+1帧单目相机图像与第k帧单目相机图像中的特征点、特征线追踪匹配信息传送至特征重建模块,将第k+1帧单目相机图像中的特征点信息传送至闭环检测与重定位模块;
第五步,IMU预积分模块对从数据获取模块得到的[tk-1,tk]时间区间内的IMU数据,采用预积分算法进行预积分处理,将预积分处理后的[tk-1,tk]时间区间内的IMU预积分值发送至非线性优化模块;
第六步,特征重建模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息、特征线信息,以及第k帧单目相机图像与第k+1帧单目相机图像中的特征点、特征线追踪匹配信息,根据以上信息分别构建第k+1帧特征点管理和特征线管理数据结构实例,对特征进行三维重建,更新特征点管理和特征线管理数据结构,将更新过的特征点管理和特征线管理数据结构发送给非线性优化模块,方法如下:
6.1,特征重建模块根据第k+1帧单目相机图像中的特征点信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征点追踪匹配信息,构建第k+1帧特征点管理数据结构实例,该实例包括第k+1帧单目相机图像中每个特征点的图像坐标系坐标、每个特征点的相机坐标系坐标;
6.2,特征重建模块根据第k+1帧单目相机图像中的特征线信息和第k帧单目相机图像与第k+1帧单目相机图像中的特征线追踪匹配信息,构建第k+1帧特征线管理数据结构实例,该实例包括第k+1帧单目相机图像中每条特征线段起始点的图像坐标系坐标、每条特征线段起始点的相机坐标系坐标、每条特征线段上的特征点图像坐标系坐标、每条特征线段上所有特征点相机坐标系坐标集合;
6.3,若k<4,特征检测模块将此时的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块,转第七步;
6.4,若k≥4,特征检测模块对点特征和线特征进行三维重建,方法是:
6.4.1,特征重建模块对于第k+1帧单目相机图像中的特征点,筛选出至少在第k帧中出现过的特征点,联立这些特征点在第k+1、k、k-1、k-2、k-3帧中的相机坐标系坐标信息,利用奇异值分解算法,对这些特征点在相机坐标系中的深度进行求解,并在第k+1帧单目相机图像特征点管理数据结构实例中添加这些特征点的相机坐标系深度;
6.4.2,特征重建模块对于第k+1帧单目相机图像中的特征线,筛选出至少在第k帧中出现过的特征线lf,lf∈LF,LF表示第k+1帧单目相机图像中所有被筛选出来的特征线的集合,基于lf上特征点信息,进行三维重建;
6.4.3,特征重建模块将更新过的第k+1帧特征点管理数据结构和特征线管理数据结构发送给非线性优化模块,转第七步;
第七步,闭环检测与重定位模块从视觉特征追踪模块获取第k+1帧单目相机图像中的特征点信息,并基于DBoW2算法对第k+1帧单目相机图像中的特征点信息进行转化、存储与检索,并利用VINS-Mono中的闭环检测算法,对第k+1帧单目相机图像帧与之前的单目图像帧进行闭环检测匹配,若匹配成功,闭环检测与重定位模块将闭环检测结果发送给非线性优化模块,转第八步;若匹配不成功,直接转第八步;
第八步,非线性优化模块从特征重建模块获取第k+1帧特征点管理和特征线管理数据结构,从IMU预积分模块获取[tk-1,tk]时间区间内的IMU预积分数据,从闭环检测与重定位模块获取闭环检测结果,若k<9,直接转第九步;若k≥9,非线性优化模块基于特征点数据、特征线数据、IMU预积分数据、***状态估计历史数据、闭环检测数据构建非线性优化损失函数并进行求解,获取机器人状态,方法是:
8.1,非线性优化模块构造滑动窗口,窗口内包括{tk-9,tk-8,…,tk-1,tk}共十个时刻内机器人***相关信息,机器人***相关信息包括从特征重建模块接收到的第k-8,k-7,…,k+1共十帧图像帧特征点管理和特征线管理数据结构、从IMU预积分模块中接收到[tk-10,tk-9],[tk-9,tk-8],…,[tk-1,tk]共十个时间区间内的IMU预积分信息、从闭环检测与重定位模块中接收到的闭环检测信息、除tk时刻外每个时刻机器人的位姿信息;
8.2,非线性优化模块将十个时刻中所有特征线段的普吕克坐标参数转化为线段所在三维直线的正交法表达参数;
8.3,非线性优化模块根据滑动窗口的信息,构造优化变量χ:
其中,xk-9,xk-8,…,xk表示滑动窗口内十个时刻机器人***状态,λ0,λ1,…,λn表示滑动窗口内十个时刻中所有特征点在三维世界中的深度,表示滑动窗口内十个时刻中所有特征线段的正交法表达参数,ti时刻机器人***状态xi可以表达为:
其中,代表ti时刻机器人在IMU坐标系相对世界坐标系的位移向量,代表ti时刻机器人在IMU坐标系相对世界坐标系的速度向量,代表ti时刻机器人在IMU坐标系相对世界坐标系的四元数,代表ti时刻机器人在IMU坐标系中的IMU加速度计偏置,代表ti时刻机器人在IMU坐标系中的IMU陀螺仪偏置;
8.4,非线性优化模块根据8.3中构造的优化变量构造优化损失函数:
其中:
表示IMU预积分残差项,表示IMU坐标系中从ti时刻到ti+1时刻IMU预积分值,表示IMU预积分数据集合,表示IMU从ti时刻到ti+1时刻IMU预积分残差,该残差的具体构造方式同VINS-Mono;
表示闭环检测重定位残差项,若闭环检测发生在ti时刻与tv时刻之间,即tv时刻单目图像帧和ti时刻单目图像帧被判定为发生闭环检测,tv时刻可为ti时刻之前任一时刻,v<i,表示相机坐标系中ti时刻第j个特征点在tv时刻单目图像帧上的重投影误差,表示世界坐标系中tv时刻机器人的旋转四元数,表示世界坐标系中tv时刻机器人的位移向量,下标w表示该数据为世界坐标系中的数据,表示闭环检测模块中存储的图像信息集合,表示滑动窗口内所有特征点的集合,表示ti时刻闭环检测残差,该残差的具体构造方式同VINS-Mono;
8.5,非线性优化模块采用最小二乘优化方法对优化损失函数求解,获得tk时刻机器人位姿估计值、速度估计值、窗口内所有特征点深度估计值、窗口内所有特征线普吕克坐标参数估计值,转第九步;
第九步,令k=k+1,若还能从视觉-惯性里程计数据集中获取双目图像帧中左目相机图像帧和IMU数据,返回第三步,若不能从视觉-惯性里程计数据集中获取双目图像帧中左目相机图像帧和IMU数据,转第十步;
第十步,结束。
2.如权利要求1所述的一种基于点线特征的视觉-惯性里程计状态估计方法,其特征在于所述视觉-惯性里程计数据集指苏黎世联邦理工学院的“EuRoC MAV Datasets”数据集。
3.如权利要求1所述的一种基于点线特征的视觉-惯性里程计状态估计方法,其特征在于2.2步所述MAXpoint为200,所述MAXline为200。
4.如权利要求1所述的一种基于点线特征的视觉-惯性里程计状态估计方法,其特征在于4.3步所述视觉特征追踪模块对Lk+1中的特征线段进行整合,删除误检测线段,合并属于同一条直线的多条线段,得到整合后的新特征线段集合Lk+1的方法是:
4.3.1,对Lk+1中的特征线段由长至短进行排序,并对特征线段进行编号;
4.3.2,选取Lk+1中的特征线段la,对la与lb进行整合匹配,lb∈la+1,la+2,…,lA:
4.3.3,此时la与lb合并成功,将合并后的la存入Lk+1,并将lb从Lk+1中删除,若a<A,转4.3.2,令a=a+1;若a=A,转4.3.5;
4.3.4,此时la与lb合并不成功,若b<A,转4.3.2,令b=b+1;若b=A,且a<A,则令a=a+1,转4.3.2;若b=A,且a=A,转4.3.5;
4.3.5,得到整合后的新特征线段集合Lk+1。
5.如权利要求4所述的一种基于点线特征的视觉-惯性里程计状态估计方法,其特征在于所述距离阈值Td为1个像素,所述角度阈值Tθ为5度。
6.如权利要求1所述的一种基于点线特征的视觉-惯性里程计状态估计方法,其特征在于4.4步所述视觉追踪模块对第k+1帧单目相机图像中的特征点进行增补,得到新的特征点集合Pk+1的方法是:
4.4.1,设置单目图像帧中特征线段上特征点最大值MAXpoint_inline,并统计第k+1帧单目图像中的特征点数npoint,和第k+1帧单目图像中特征线段上的特征点数npoint_inline;MAXpoint_inline为正整数;
4.4.2,基于第k+1帧单目图像制作相对应的掩模图像Mat1,Mat1与第k+1帧单目图像尺寸相同,Mat1所有像素为黑,只有特征线段即Lk+1中所有线段附近像素为白色,同时特征线段上的特征点即Pk+1中特征点周围像素为黑色;
4.4.3,计算第一次特征点增补数目N1:
4.4.3.1,若MAXpoint_inline>[MAXpoint-(npoint-npoint_inline)],则N1=MAXpoint-npoint,转4.4.4;
4.4.3.2,若MAXpoint_inline<[MAXpoint-(npoint-npoint_inline)],且MAXpoint_inline>npoint_inline,则N1=MAXpoint_inline-npoint_inline,转4.4.4;
4.4.4,在第k+1帧单目相机图像中,Mat1中白色像素所对应的区域内,使用FAST算法添加N1个特征点,且这些新添加的特征点之间间隔大于20个像素,将新加入的N1个特征点信息添加至特征点集合Pk+1中以更新Pk+1,并更新特征点数npoint=npoint+N1;
4.4.5,基于第k+1帧单目相机图像制作相对应的掩模图像Mat2,Mat2与第k+1帧单目图像尺寸相同,Mat2所有像素为白,只有Pk+1中所有特征点周围像素为黑色;
4.4.6,计算第二次特征点增补数目N2,若MAXpoint>npoint,则N2=MAXpoint-npoint,转4.4.7;
4.4.7,在第k+1帧单目相机图像中,Mat2中白色像素所对应的区域内,使用FAST算法添加N2个特征点,且这些新添加的特征点之间间隔大于20个像素,并将新加入的N2个特征点信息添加至特征点集合Pk+1中以更新Pk+1;并更新特征点数npoint=npoint+N2。
7.如权利要求6所述的一种基于点线特征的视觉-惯性里程计状态估计方法,其特征在于所述MAXpotnt_inline为150,所述Lk+1中所有线段附近像素指距离Lk+110像素以内区域,所述Pk+1中特征点周围像素指以特征点为圆心,半径为20像素区域。
8.如权利要求1所述的一种基于点线特征的视觉-惯性里程计状态估计方法,其特征在于6.4.2步所述特征重建模块对于第k+1帧单目相机图像中的特征线,筛选出至少在第k帧中出现过的特征线lf,基于lf上特征点信息进行三维重建的方法是:
6.4.2.1,特征重建模块将第k+1、k、k-1、k-2、k-3共计五帧图像帧中,所有lf上的特征点三维坐标转换至第k+1帧图像帧相机坐标系中,建立特征点集合储存所有被转换至第k+1帧图像帧相机坐标系中特征点的三维坐标,此时中特征点数量为
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011401030.5A CN112683305B (zh) | 2020-12-02 | 2020-12-02 | 一种基于点线特征的视觉-惯性里程计状态估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202011401030.5A CN112683305B (zh) | 2020-12-02 | 2020-12-02 | 一种基于点线特征的视觉-惯性里程计状态估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112683305A CN112683305A (zh) | 2021-04-20 |
CN112683305B true CN112683305B (zh) | 2022-03-04 |
Family
ID=75445863
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202011401030.5A Active CN112683305B (zh) | 2020-12-02 | 2020-12-02 | 一种基于点线特征的视觉-惯性里程计状态估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112683305B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113763470B (zh) * | 2021-08-10 | 2024-06-07 | 北京理工大学 | 点线特征融合的rgbd视觉惯性同时定位与地图构建 |
CN114429500A (zh) * | 2021-12-14 | 2022-05-03 | 中国科学院深圳先进技术研究院 | 一种基于点线特征融合的视觉惯性定位方法 |
CN114485640A (zh) * | 2022-01-20 | 2022-05-13 | 河北工业职业技术学院 | 基于点线特征的单目视觉惯性同步定位与建图方法及*** |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9406171B2 (en) * | 2014-08-25 | 2016-08-02 | Daqri, Llc | Distributed aperture visual inertia navigation |
US10371530B2 (en) * | 2017-01-04 | 2019-08-06 | Qualcomm Incorporated | Systems and methods for using a global positioning system velocity in visual-inertial odometry |
CN108827315B (zh) * | 2018-08-17 | 2021-03-30 | 华南理工大学 | 基于流形预积分的视觉惯性里程计位姿估计方法及装置 |
CN109211277B (zh) * | 2018-10-31 | 2021-11-16 | 北京旷视科技有限公司 | 视觉惯性里程计的状态确定方法、装置和电子设备 |
CN111982103B (zh) * | 2020-08-14 | 2021-09-14 | 北京航空航天大学 | 一种权值优化的点线综合视觉惯性里程计方法 |
CN111811506B (zh) * | 2020-09-15 | 2020-12-01 | 中国人民解放军国防科技大学 | 视觉/惯性里程计组合导航方法、电子设备及存储介质 |
-
2020
- 2020-12-02 CN CN202011401030.5A patent/CN112683305B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN112683305A (zh) | 2021-04-20 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112683305B (zh) | 一种基于点线特征的视觉-惯性里程计状态估计方法 | |
Zhan et al. | Visual odometry revisited: What should be learnt? | |
CN111311666B (zh) | 一种融合边缘特征和深度学习的单目视觉里程计方法 | |
Qin et al. | Vins-mono: A robust and versatile monocular visual-inertial state estimator | |
CN108242079B (zh) | 一种基于多特征视觉里程计和图优化模型的vslam方法 | |
Zheng et al. | Trifo-VIO: Robust and efficient stereo visual inertial odometry using points and lines | |
Civera et al. | 1-point RANSAC for EKF-based structure from motion | |
CN109166149A (zh) | 一种融合双目相机与imu的定位与三维线框结构重建方法与*** | |
CN112634451A (zh) | 一种融合多传感器的室外大场景三维建图方法 | |
CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
CN108257154B (zh) | 基于区域信息和cnn的极化sar图像变化检测方法 | |
CN111780754A (zh) | 基于稀疏直接法的视觉惯性里程计位姿估计方法 | |
CN114001733B (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
Jiang et al. | DVIO: An optimization-based tightly coupled direct visual-inertial odometry | |
CN108615246A (zh) | 提高视觉里程计***鲁棒性和降低算法计算消耗的方法 | |
Wen et al. | Hybrid semi-dense 3D semantic-topological mapping from stereo visual-inertial odometry SLAM with loop closure detection | |
Chen et al. | A stereo visual-inertial SLAM approach for indoor mobile robots in unknown environments without occlusions | |
CN112802096A (zh) | 实时定位和建图的实现装置和方法 | |
CN111998862A (zh) | 一种基于bnn的稠密双目slam方法 | |
CN112101160A (zh) | 一种面向自动驾驶场景的双目语义slam方法 | |
Xu et al. | Direct visual-inertial odometry with semi-dense mapping | |
Huai et al. | Stereo-inertial odometry using nonlinear optimization | |
Jin et al. | Beyond learning: Back to geometric essence of visual odometry via fusion-based paradigm | |
CN117367427A (zh) | 一种适用于室内环境中的视觉辅助激光融合IMU的多模态slam方法 | |
CN112991400B (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 |