CN112862768A - 一种基于点线特征的自适应单目vio初始化方法 - Google Patents

一种基于点线特征的自适应单目vio初始化方法 Download PDF

Info

Publication number
CN112862768A
CN112862768A CN202110119124.1A CN202110119124A CN112862768A CN 112862768 A CN112862768 A CN 112862768A CN 202110119124 A CN202110119124 A CN 202110119124A CN 112862768 A CN112862768 A CN 112862768A
Authority
CN
China
Prior art keywords
point
line
imu
constraint
initialization
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
Application number
CN202110119124.1A
Other languages
English (en)
Other versions
CN112862768B (zh
Inventor
范馨月
宋子苑
陶交
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Chongqing University of Post and Telecommunications
Original Assignee
Chongqing University of Post and Telecommunications
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Chongqing University of Post and Telecommunications filed Critical Chongqing University of Post and Telecommunications
Priority to CN202110119124.1A priority Critical patent/CN112862768B/zh
Publication of CN112862768A publication Critical patent/CN112862768A/zh
Application granted granted Critical
Publication of CN112862768B publication Critical patent/CN112862768B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/0002—Inspection of images, e.g. flaw detection
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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/10—Image acquisition modality
    • G06T2207/10016—Video; Image sequence
    • 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/20112—Image segmentation details
    • G06T2207/20164—Salient point detection; Corner detection

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Manufacturing & Machinery (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Quality & Reliability (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Image Analysis (AREA)

Abstract

本发明涉及一种基于点线特征的自适应单目VIO初始化方法，属于机器人视觉定位导航技术领域，包括步骤：S1：输入图像帧，分别检测点特征与线特征，输入IMU获取的数据，在图像帧之间进行预积分计算；S2：估计相机初始位姿；S3：构建最大后验估计问题，优化惯性参数，得到尺度因子、速度信息、重力方向以及IMU的陀螺仪偏置和加速度计偏置；S4：视觉惯性对齐、尺度缩放，将相机初始位姿转换到世界坐标系下；S5：初始值收敛。本发明能在不同复杂环境和不同初始状态下完成较为稳定精确的初始化，解决了VIO初始化过程中传感器的不确定性和惯性参数的不一致性，具有更高的性能。

Description

一种基于点线特征的自适应单目VIO初始化方法
技术领域
本发明属于机器人视觉定位导航技术领域，涉及一种基于点线特征的自适应单目VIO初始化方法。
背景技术
随着计算机技术的发展，对于移动机器人领域的研究也得到了快速发展。为了实现机器人在未知环境下的自主运动，首先要解决的两个问题是机器人位姿的实时估计和如何根据位姿构建地图，进一步为后续的自主定位、路径规划、避障等任务提供条件。在实际应用中，机器人通常会搭载不同功能的传感器，搭载相机和IMU的SLAM***被称为视觉惯性SLAM，其里程计又被称为视觉惯性里程计(Visual Inertial Odometry,VIO)，具有体积小、成本低、场景辨识能力强等优点，得到了该领域的广泛关注。
对于VIO而言，初始化模块尤为重要。初始参数的确定，如重力方向、速度、IMU偏置等，决定了***的精度。特别是在单目VIO中无法直接观测到尺度，很难将视觉和惯性进行融合， VIO初始化困难。对于IMU的初始化，由于IMU数据的加速度计受重力影响，重力方向的估计也是位姿估计中的决定性因素。如果初始化工作出现误差，整个***准确度都会随之降低，在基于优化的方法中可能会陷入局部最优。目前的初始化方法主要分为紧耦合和松耦合方法，并针对上述问题提出了不同的解决方案。文献“Martinelli等人，Closed-formsolution of visual- inertial structure from motion.International Journal ofComputer Vision,2014”提供了一种封闭求解的方案，用于联合获取尺度、重力、偏置、初始速度等参数，建立在可从IMU数据中大致估计相机位姿的基础上。文献“Mur-Artal等人，Visual-inertial monocular SLAM with map reuse. IEEE Robotics and AutomationLetters,2017”和文献“T.Qin等人，VINS-Mono:A robust and versatile monocularvisual-inertial state estimator.IEEE Transactions on Robotics,vol.34,2018”基于单目相机可精确估计无尺度的相机轨迹的假设，通过相机轨迹估算惯性参数，并通过BA优化。惯性参数在视觉信息提供的线性方程中由最小二乘法求解。但上述两种初始化方案都忽略了传感器的不确定性，且惯性参数在不同步骤中分别求解，忽略了其关联性。
综上所述，目前在VIO技术领域存在的问题是：1)过于依赖场景特征。现有的VIO初始化算法中，普遍采用点特征进行纯视觉估计，但在弱纹理环境下，如走廊、墙壁等，则很难提取到足够数量的特征点从而导致初始化失败，***定位精度差。2)未考虑到传感器的不确定性和惯性参数之间的关联性，且通常会忽略IMU的加速度计偏置，导致估计值准确度较低。 3)对初始状态要求较高，需要相机在初始化阶段提供足够的旋转和平移才能完成初始化，只适用特定情况下。
发明内容
有鉴于此，本发明的目的在于针对弱纹理环境下点特征不足难以估计相机初始位姿、***定位精度差，惯性参数估计过程中未考虑到传感器不确定性和关联性，初始化方案适用性不强等问题，在纯视觉SFM中引入线特征作为可选项，在场景纹理不足以提供可靠估计时使用，提供鲁棒性；同时构建最大后验估计问题对惯性参数进行求解，保证其一致性，适用于任何初始化情况，提供一种基于点线特征的自适应单目VIO初始化方法。
为达到上述目的，本发明提供如下技术方案：
一种基于点线特征的自适应单目VIO初始化方法，包括以下步骤：
S1：输入图像帧，分别检测点特征与线特征；输入IMU获取的数据，在每个图像帧之间进行IMU预积分计算；
S2：估计相机初始位姿；首先判断点特征是否满足视差条件和数量要求，是则通过八点法求解本质矩阵，估计相机初始位姿；否则引入线特征，计算其匹配的弱约束分数，筛选出用于初始化的线特征，并根据点线距离约束估计相机初始位姿；
S3：构建最大后验估计问题，优化惯性参数，得到尺度因子、速度信息、重力方向以及 IMU的陀螺仪偏置和加速度计偏置；
S4：视觉惯性对齐，并进行尺度缩放，同时将相机初始位姿转换到世界坐标系下；
S5：初始值收敛，初始化完成。
进一步，步骤S1具体包括：通过Shi-Tomasi角点算法检测点特征，该算法基于梯度变化进行检测，属于Harris角点检测的改进算法；线特征采用LSD直线检测算法，其核心思想是将梯度方向相近的像素合并，快速检测图像中的直线段；IMU预积分是指将图像的第k帧和第k+1帧之间的所有IMU测量值进行积分，得到第k+1帧之间的PVQ值，即位置、速度和旋转的值，为视觉提供初始值，并作为后端优化的约束项。
进一步，在步骤S2中，根据点特征是否满足初始化条件分为两种情况对相机初始位姿进行估计：
情况1：点特征满足视差条件和数量要求，根据对极几何得到对应点的关系为：
Figure RE-RE-GDA0003011474060000021
其中x1＝(u1,v1,1)T、x2＝(u2,v2,1)T是对应像素点归一化平面上的坐标，R和t是两帧之间的相机运动，分别表示旋转和平移；中间部分记为本质矩阵E，表示为E＝t^R，是一个3×3的自由度为5的矩阵；
通过八点法求解该本质矩阵，由对极几何得到：
Figure RE-RE-GDA0003011474060000031
为求解E，共需要八对匹配点构成八个方程，通过奇异值分解(SVD)求解本质矩阵E，并取具有正深度的解作为最终估计；
情况2：点特征不满足初始化要求，引入线特征，通过计算弱约束分数筛选匹配对，并通过点线距离约束求解相机初始位姿；其中，弱约束包括描述子约束和对极约束，分别对其计算分数sd和se，具体如下：
LSD线段采用LBD描述子，对像素梯度进行统计并计算统计量的平均向量与标准方差作为描述子；对于描述子约束，主要考虑到需要剔除外观差异较大的误匹配，计算参考帧描述子desc1和当前帧描述子desc2之间的汉明距离，若小于阈值τdesc，描述子分数sd记为1，若大于该阈值，描述子分数sd记为0，表示为：
Figure RE-RE-GDA0003011474060000032
对于对极约束，由于线特征没有严格的对极约束，因此作为弱约束项增强可靠性；首先计算参考帧线特征两个端点的极线，当前帧的对应线特征AB所在直线与该极线相交于点C 和点D，该约束分数定义为：
Figure RE-RE-GDA0003011474060000033
其中dmin表示四点共线的最小欧式距离，dmax表示四点共线的最大欧式距离；
最终，对于每对匹配线，计算分数s＝sd·se，如果s大于某个阈值，则认为该匹配对可用于初始化，进行闭式求解；
闭式求解过程如下：3D线特征的端点投影理论上应落在相机观测到的线上，获得归一化线特征的系数；
Figure RE-RE-GDA0003011474060000041
记线特征端点的逆深度分别为ρks和ρke，则3D线端点重投影归一化地表示为：
Figure RE-RE-GDA0003011474060000042
其中π(·)是重投影函数，表示为π(x,y,z)T＝π(x/z,y/z,1)T，Ri是基于小旋转假设下的旋转矩阵，即假设连续图像帧之间的旋转较小，记相机旋转向量和平移向量分别为r＝(r1,r2,r3)T和t＝(t1,t2,t3)T，旋转矩阵按一阶泰勒展开式近似表示：
Figure RE-RE-GDA0003011474060000043
由于投影点在观测线上，二者距离为零；以起始点为例，约束表示为
Figure RE-RE-GDA0003011474060000044
即：
Figure RE-RE-GDA0003011474060000045
在小旋转假设下，ρkst1可忽略不计，因此上式化简得：
Ar1+Br2+Cr3+D＝0
其中：
Figure RE-RE-GDA0003011474060000046
Figure RE-RE-GDA0003011474060000047
Figure RE-RE-GDA0003011474060000048
Figure RE-RE-GDA0003011474060000049
此外，另一端点
Figure RE-RE-GDA00030114740600000410
也具有相同约束，因此一对匹配线得到两个方程；如果有多对匹配线，则通过如下线性方程闭式求解，由SVD得到唯一解；
Figure RE-RE-GDA00030114740600000411
进一步，在步骤S3中，构建最大后验估计问题，优化IMU相关参数，得到尺度因子、速度信息、重力方向以及IMU的陀螺仪偏置和加速度计偏置；
首先，所估计的惯性参数为：
Figure RE-RE-GDA0003011474060000051
其中s为尺度因子，Rwg为重力方向，b向量包括IMU加速度计偏差ba和陀螺仪偏差bg，
Figure RE-RE-GDA0003011474060000052
是无尺度的第0帧到第k帧的速度；由IMU预积分理论建立含先验的MAP问题；
Figure RE-RE-GDA0003011474060000053
其中是
Figure RE-RE-GDA0003011474060000054
似然值，
Figure RE-RE-GDA0003011474060000055
是先验值，
Figure RE-RE-GDA0003011474060000056
表示初始化窗口内连续关键帧之间 IMU预积分的集合；假设IMU每次的测量值是独立的，该MAP问题描述为：
Figure RE-RE-GDA0003011474060000057
假设IMU预积分和先验分布的误差为高斯误差，得到最终优化问题：
Figure RE-RE-GDA0003011474060000058
其中rp为先验误差，
Figure RE-RE-GDA0003011474060000059
为IMU预积分误差；且在优化过程中，重力方向和尺度因子的更新式为：
Figure RE-RE-GDA00030114740600000510
snew＝soldexp(δs)
该方法考虑了IMU的不确定性，将惯性参数的估计建立为最优估计问题，且不需要假设忽略加速度计的偏置，将已知信息作为先验信息加入到MAP问题中；一次性估计出所有惯性参数，避免了数据不一致问题。
进一步，在步骤S4中，惯性参数优化完成后，得到单目视觉所需的尺度信息估计值，根据该尺度进行比例缩放，得到相机位姿、速度和3D地图点，并与重力方向进行对齐，将位姿转换到世界坐标系下，重新计算IMU预积分并更新；至此已经分别估计了视觉和惯性的参数，最后进行BA优化，得到最优解。
本发明的有益效果在于：1)本发明改善了传统方法精度低、鲁棒性差、适用性不足等难题，能在不同的复杂环境下和不同初始状态下完成较为稳定精确的初始化；2)本发明提出的引入线特征的自适应纯视觉SFM估计方法能够很好地适应弱纹理环境，并提供结构信息，提高可靠度；3)本发明提出的基于最大后验估计的惯性优化方法能够很好地解决VIO初始化过程中传感器的不确定性和惯性参数的不一致性。仿真结果表明，本发明相较现有VIO算法具有更高的性能。
本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。
附图说明
为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：
图1是本发明实施例提供的基于点线特征的自适应单目VIO初始化算法流程图；
图2是本发明实施例提供的线特征处理流程图；
图3是本发明实施例提供的线特征对极约束示意图；
图4是本发明实施例提供的弱纹理环境下的特征提取示意图；
图5是本发明采用的方法与传统VIO方法得到的轨迹和真实轨迹之间的对比示意图；
图6是本发明采用的VIO方法与传统VIO方法得到的均方根误差(RMSE)对比示意图。
具体实施方式
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本发明的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。
本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”、“前”、“后”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本发明的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
请参阅图1～图6，为一种基于点线特征的自适应单目VIO初始化方法。
图1是本发明实施例提供的基于点线特征的自适应单目VIO初始化算法流程图，如图所示，本发明实施例提供的基于点线特征的自适应单目VIO初始化算法包括：
首先通过Shi-Tomasi角点算法检测点特征，该算法基于梯度变化进行检测，属于Harris 角点检测的改进算法。线特征采用LSD直线检测算法，其核心思想是将梯度方向相近的像素合并，快速检测图像中的直线段。IMU预积分是指将图像的第k帧和第k+1帧之间的所有 IMU进行积分，则可以得到第k+1帧之间的PVQ值，即位置、速度和旋转值，为视觉提供初始值，并作为后端优化的约束项。
随后根据点特征是否满足初始化条件分为两种情况对相机初始位姿进行估计：
情况1：点特征满足视差条件和数量要求，根据对极几何可以得到对应点的关系为：
Figure RE-RE-GDA0003011474060000071
其中x1＝(u1,v1,1)T、x2＝(u2,v2,1)T是对应像素点归一化平面上的坐标，R和t是两帧之间的相机运动，分别表示旋转和平移。中间部分记为本质矩阵E，表示为E＝t^R，是一个3×3的自由度为5的矩阵。
通过八点法求解该本质矩阵，由对极几何可以得到：
Figure RE-RE-GDA0003011474060000072
为求解E，共需要八对匹配点构成八个方程，通过奇异值分解(SVD)求解本质矩阵E，并取具有正深度的解作为最终估计。
情况2：点特征不满足初始化条件，引入线特征，通过计算弱约束分数筛选匹配对，并通过点线距离约束求解相机初始位姿。其中，弱约束包括描述子约束和对极约束，分别对其计算分数sd和se，流程如图2所示。过程具体如下：
LSD线段采用LBD描述子，对像素梯度进行统计并计算统计量的平均向量与标准方差作为描述子。对于描述子约束，主要考虑到需要剔除外观差异较大的误匹配，计算参考帧描述子desc1和当前帧描述子desc2之间的汉明距离，若小于阈值τdesc，描述子分数sd记为1，若大于该阈值，描述子分数sd记为0，表示为：
Figure RE-RE-GDA0003011474060000073
对于对极约束，由于线特征没有严格的对极约束，因此作为弱约束项增强可靠性。如图 3所示，表示了线段两端点的极线约束，首先计算参考帧线特征两个端点的极线l1、l2，当前帧的对应线特征AB所在直线与该极线相交于点C和点D。该约束分数定义为：
Figure RE-RE-GDA0003011474060000081
其中dmin表示四点共线的最小欧式距离，dmax表示四点共线的最大欧式距离。
最终，对于每对匹配线，计算分数s＝sd·se，如果s大于某个阈值，则认为该匹配对可用于初始化，进行闭式求解。闭式求解过程如下：3D线特征的端点投影理论上应落在相机观测到的线上，因此可获得归一化线特征的系数：
Figure RE-RE-GDA0003011474060000082
记线特征端点的逆深度分别为ρks和ρke，则3D线端点重投影可归一化地表示为：
Figure RE-RE-GDA0003011474060000083
其中π(·)是重投影函数，可表示为π(x,y,z)T＝π(x/z,y/z,1)T,Ri是基于小旋转假设下的旋转矩阵，即假设连续图像帧之间的旋转较小，记相机旋转向量和平移向量分别为 r＝(r1,r2,r3)T和t＝(t1,t2,t3)T，旋转矩阵可按一阶泰勒展开式近似表示：
Figure RE-RE-GDA0003011474060000084
由于投影点在观测线上，二者距离为零。以起始点为例，约束可表示为
Figure RE-RE-GDA0003011474060000085
即：
Figure RE-RE-GDA0003011474060000086
在小旋转假设下，ρkst1可忽略不计，因此上式化简得：
Ar1+Br2+Cr3+D＝0
其中：
Figure RE-RE-GDA0003011474060000091
Figure RE-RE-GDA0003011474060000092
Figure RE-RE-GDA0003011474060000093
Figure RE-RE-GDA0003011474060000094
此外，另一端点
Figure RE-RE-GDA0003011474060000095
也具有相同约束。因此一对匹配线可得到两个方程。如果有多对匹配线，则通过如下线性方程闭式求解，由SVD得到唯一解：
Figure RE-RE-GDA0003011474060000096
对于惯性估计，构建最大后验估计问题，优化IMU相关参数，得到尺度因子、速度信息、重力方向以及IMU的陀螺仪偏置和加速度计偏置。首先，所估计的惯性参数为：
Figure RE-RE-GDA0003011474060000097
其中s为尺度因子，Rwg为重力方向，b向量包括IMU加速度计偏差ba和陀螺仪偏差bg，
Figure RE-RE-GDA0003011474060000098
是无尺度的第0帧到第k帧的速度。由IMU预积分理论可以建立含先验的MAP问题：
Figure RE-RE-GDA0003011474060000099
其中是
Figure RE-RE-GDA00030114740600000910
似然值，
Figure RE-RE-GDA00030114740600000911
是先验值，
Figure RE-RE-GDA00030114740600000912
表示初始化窗口内连续关键帧之间IMU预积分的集合。假设IMU每次的测量值是独立的，该MAP问题可以描述为：
Figure RE-RE-GDA00030114740600000913
假设IMU预积分和先验分布的误差为高斯误差，可以得到最终优化问题：
Figure RE-RE-GDA00030114740600000914
其中rp为先验误差，
Figure RE-RE-GDA00030114740600000915
为IMU预积分误差。且在优化过程中，重力方向和尺度因子的更新式为：
Figure RE-RE-GDA00030114740600000916
snew＝soldexp(δs)
该方法考虑了IMU的不确定性，将惯性参数的估计建立为最优估计问题，且不需要假设忽略加速度计的偏置，将已知信息作为先验信息加入到MAP问题中。可一次性估计出所有惯性参数，避免了数据不一致问题。
惯性参数优化完成后，可以得到单目视觉所需的尺度信息估计值，根据该尺度进行比例缩放，可得到相机位姿、速度和3D地图点，并与重力方向进行对齐，将位姿转换到世界坐标系下，重新计算IMU预积分并更新。至此已经分别估计了视觉和惯性的参数，最后进行BA 优化，得到最优解。
图4是本发明实施例提供的弱纹理环境下的特征提取示意图，从图中可以看到，当环境纹理不明显时，难以提取点特征，此时引入线特征作为结构信息可解决这一问题，增强了VIO 的鲁棒性。
本发明使用主流数据集Euroc进行了实验。该数据集采用微型飞行器(MAV)采集工业环境中的图像信息以及IMU信息，共包含11个序列，根据照明情况、纹理和运动速度分为简单、中等和困难三种，适用于测试本发明的性能表现。
图5是本发明采用的VIO方法与传统VIO方法得到的轨迹和真实轨迹之间的对比示意图，其中(a)是V2_01_easy序列的轨迹示意图，该序列在初始阶段视差不足，平移较小；(b)是MH_05_difficult序列的轨迹示意图，该序列在初始阶段几乎保持静止，并在很长一段时间处于无光源、纹理较少的环境中。可以看到，本发明采用的VIO方法得到的轨迹更接近真实值，由此验证了本发明的方法具有更好的精度。
图6是本发明采用的VIO方法与传统VIO方法得到的均方根误差(RMSE)对比示意图，其中(a)是V2_01_easy序列的均方根误差(RMSE)变化，(b)是MH_05_difficult序列的均方根误差(RMSE)变化。可以看到，本发明采用的VIO方法得到的均方根误差(RMSE) 值整体低于传统VIO方法，且变化幅度也较小，由此验证了本发明的方法具有更好的稳定性。
表1统计了本发明的VIO方法与传统VIO算法在Euroc数据集下的平移误差(Translation) 和旋转误差(Rotation)，均采用均方根误差(RMSE)。从表1中的数据可以看到，本发明采用的VIO方法能取得更好的结果。
Figure RE-RE-GDA0003011474060000101
本发明提出的一种基于点线特征的单目VIO初始化算法有效解决了传统方法精度低、鲁棒性差、适用性不足等难题，能在不同的复杂环境下和不同初始状态下完成较为稳定精确的初始化，引入线特征，使初始化能自适应地根据环境变化进行纯视觉估计，增强了可靠性。并且很好地解决了VIO初始化过程中传感器的不确定性和惯性参数的不一致性；仿真结果表明，本发明在初始化实时性、精确度和稳定性方面都有一定的提升，具有良好的性能。最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (7)

1.一种基于点线特征的自适应单目VIO初始化方法，其特征在于：包括以下步骤：
S1：输入图像帧，分别检测点特征与线特征；输入IMU获取的数据，在每个图像帧之间进行IMU预积分计算；
S2：估计相机初始位姿；首先判断点特征是否满足视差条件和数量要求，是则通过八点法求解本质矩阵，估计相机初始位姿；否则引入线特征，计算其匹配的弱约束分数，筛选出用于初始化的线特征，并根据点线距离约束估计相机初始位姿；
S3：构建最大后验估计问题，优化惯性参数，得到尺度因子、速度信息、重力方向以及IMU的陀螺仪偏置和加速度计偏置；
S4：视觉惯性对齐，并进行尺度缩放，同时将相机初始位姿转换到世界坐标系下；
S5：初始值收敛，初始化完成。
2.根据权利要求1所述的基于点线特征的自适应单目VIO初始化方法，其特征在于：步骤S1具体包括：通过Shi-Tomasi角点算法检测点特征，；线特征采用LSD直线检测算法，将梯度方向相近的像素合并，快速检测图像中的直线段；所述IMU预积分是指将图像的第k帧和第k+1帧之间的所有IMU测量值进行积分，得到第k+1帧之间的PVQ值，即位置、速度和旋转的值，为视觉提供初始值，并作为后端优化的约束项。
3.根据权利要求1所述的基于点线特征的自适应单目VIO初始化方法，其特征在于：步骤S2具体包括：根据点特征是否满足初始化条件分为两种情况对相机初始位姿进行估计：
情况1：点特征满足视差条件和数量要求，根据对极几何得到对应点的关系为：
Figure FDA0002921816540000011
其中x1＝(u1,v1,1)T、x2＝(u2,v2,1)T是对应像素点归一化平面上的坐标，R和t是两帧之间的相机运动，分别表示旋转和平移；中间部分记为本质矩阵E，表示为E＝t^R，是一个3×3的自由度为5的矩阵；
通过八点法求解该本质矩阵，由对极几何得到：
Figure FDA0002921816540000012
为求解E，通过八对匹配点构成八个方程，通过奇异值分解SVD求解本质矩阵E，并取具有正深度的解作为最终估计；
情况2：点特征不满足初始化要求，引入线特征，通过计算弱约束分数筛选匹配对，并通过点线距离约束求解相机初始位姿；其中，弱约束包括描述子约束和对极约束，分别对其计算分数sd和se。
4.根据权利要求3所述的基于点线特征的自适应单目VIO初始化方法，其特征在于：所述情况2具体包括以下步骤：
S221：LSD线段采用LBD描述子，对像素梯度进行统计并计算统计量的平均向量与标准方差作为描述子；对于描述子约束，主要考虑到需要剔除外观差异较大的误匹配，计算参考帧描述子desc1和当前帧描述子desc2之间的汉明距离，若小于阈值τdesc，描述子分数sd记为1，若大于该阈值，描述子分数sd记为0，表示为；
Figure FDA0002921816540000021
S222：将对极约束作为弱约束项增强可靠性；首先计算参考帧线特征两个端点的极线，当前帧的对应线特征AB所在直线与该极线相交于点C和点D，该约束分数定义为；
Figure FDA0002921816540000022
其中dmin表示四点共线的最小欧式距离，dmax表示四点共线的最大欧式距离；
S223：对于每对匹配线，计算分数s＝sd·se，如果s大于预设阈值，则认为该匹配对可用于初始化，进行闭式求解。
5.根据权利要求4所述的基于点线特征的自适应单目VIO初始化方法，其特征在于：步骤S223中所述闭式求解过程如下：
3D线特征的端点投影理论上落在相机观测到的线上，获得归一化线特征的系数：
Figure FDA0002921816540000023
记线特征端点的逆深度分别为ρks和ρke，则3D线端点重投影归一化地表示为：
Figure FDA0002921816540000024
其中π(·)是重投影函数，表示为π(x,y,z)T＝π(x/z,y/z,1)T，Ri是基于小旋转假设下的旋转矩阵，即假设连续图像帧之间的旋转较小，记相机旋转向量和平移向量分别为r＝(r1,r2,r3)T和t＝(t1,t2,t3)T，旋转矩阵按一阶泰勒展开式近似表示为：
Figure FDA0002921816540000031
由于投影点在观测线上，二者距离为零；以起始点为例，约束表示为
Figure FDA0002921816540000032
即：
Figure FDA0002921816540000033
在小旋转假设下，ρkst1忽略不计，因此上式化简得：
Ar1+Br2+Cr3+D＝0
其中：
Figure FDA0002921816540000034
Figure FDA0002921816540000035
Figure FDA0002921816540000036
Figure FDA0002921816540000037
此外，另一端点
Figure FDA0002921816540000038
也具有相同约束，因此一对匹配线得到两个方程；如果有多对匹配线，则通过如下线性方程闭式求解，由SVD得到唯一解：
Figure FDA0002921816540000039
6.根据权利要求1所述的基于点线特征的自适应单目VIO初始化方法，其特征在于：步骤S3具体包括：构建最大后验估计问题，优化IMU相关参数，得到尺度因子、速度信息、重力方向以及IMU的陀螺仪偏置和加速度计偏置；
首先，所估计的惯性参数为：
Figure FDA00029218165400000310
其中s为尺度因子，Rwg为重力方向，b向量包括IMU加速度计偏差ba和陀螺仪偏差bg，
Figure FDA00029218165400000311
是无尺度的第0帧到第k帧的速度；由IMU预积分理论建立含先验的MAP问题：
Figure FDA00029218165400000312
其中
Figure FDA0002921816540000041
是似然值，
Figure FDA0002921816540000042
是先验值，
Figure FDA0002921816540000043
表示初始化窗口内连续关键帧之间IMU预积分的集合；IMU每次的测量值是独立的，所述MAP问题描述为：
Figure FDA0002921816540000044
IMU预积分和先验分布的误差为高斯误差，最终优化问题为：
Figure FDA0002921816540000045
其中rp为先验误差，
Figure FDA0002921816540000046
为IMU预积分误差；且在优化过程中，重力方向和尺度因子的更新式为：
Figure FDA0002921816540000047
snew＝soldexp(δs)。
7.根据权利要求1所述的基于点线特征的自适应单目VIO初始化方法，其特征在于：步骤S4具体包括：惯性参数优化完成后，得到单目视觉所需的尺度信息估计值，根据该尺度进行比例缩放，得到相机位姿、速度和3D地图点，并与重力方向进行对齐，将位姿转换到世界坐标系下，重新计算IMU预积分并更新；最后进行BA优化，得到最优解。
CN202110119124.1A 2021-01-28 2021-01-28 一种基于点线特征的自适应单目vio初始化方法 Active CN112862768B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110119124.1A CN112862768B (zh) 2021-01-28 2021-01-28 一种基于点线特征的自适应单目vio初始化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110119124.1A CN112862768B (zh) 2021-01-28 2021-01-28 一种基于点线特征的自适应单目vio初始化方法

Publications (2)

Publication Number Publication Date
CN112862768A true CN112862768A (zh) 2021-05-28
CN112862768B CN112862768B (zh) 2022-08-02

Family

ID=75987748

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110119124.1A Active CN112862768B (zh) 2021-01-28 2021-01-28 一种基于点线特征的自适应单目vio初始化方法

Country Status (1)

Country Link
CN (1) CN112862768B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113298796A (zh) * 2021-06-10 2021-08-24 西北工业大学 一种基于最大后验imu的线特征slam初始化方法
CN113376669A (zh) * 2021-06-22 2021-09-10 东南大学 一种基于点线特征的单目vio-gnss融合定位算法
CN114234959A (zh) * 2021-12-22 2022-03-25 深圳市普渡科技有限公司 机器人、vslam初始化方法、装置和可读存储介质
CN114998389A (zh) * 2022-06-20 2022-09-02 珠海格力电器股份有限公司 室内定位方法
CN116957958A (zh) * 2023-06-25 2023-10-27 东南大学 一种基于惯性先验校正图像灰度的vio前端改良方法

Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180192035A1 (en) * 2017-01-04 2018-07-05 Qualcomm Incorporated Systems and methods for object location
CN108981693A (zh) * 2018-03-22 2018-12-11 东南大学 基于单目相机的vio快速联合初始化方法
CN109544696A (zh) * 2018-12-04 2019-03-29 中国航空工业集团公司西安航空计算技术研究所 一种基于视觉惯性组合的机载增强合成视景虚实图像精确配准方法
CN110030994A (zh) * 2019-03-21 2019-07-19 东南大学 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN110375738A (zh) * 2019-06-21 2019-10-25 西安电子科技大学 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN110411476A (zh) * 2019-07-29 2019-11-05 视辰信息科技(上海)有限公司 视觉惯性里程计标定适配及评价方法和***
CN110702107A (zh) * 2019-10-22 2020-01-17 北京维盛泰科科技有限公司 一种单目视觉惯性组合的定位导航方法
CN110763251A (zh) * 2019-10-18 2020-02-07 华东交通大学 视觉惯性里程计优化的方法及***
CN111156984A (zh) * 2019-12-18 2020-05-15 东南大学 一种面向动态场景的单目视觉惯性slam方法
CN111197984A (zh) * 2020-01-15 2020-05-26 重庆邮电大学 一种基于环境约束下的视觉-惯性运动估计方法
US20200219267A1 (en) * 2017-09-04 2020-07-09 Universität Zürich Visual-inertial odometry with an event camera
CN111578937A (zh) * 2020-05-29 2020-08-25 天津工业大学 同时优化外参数的视觉惯性里程计***
CN111780754A (zh) * 2020-06-23 2020-10-16 南京航空航天大学 基于稀疏直接法的视觉惯性里程计位姿估计方法

Patent Citations (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20180192035A1 (en) * 2017-01-04 2018-07-05 Qualcomm Incorporated Systems and methods for object location
US20200219267A1 (en) * 2017-09-04 2020-07-09 Universität Zürich Visual-inertial odometry with an event camera
CN108981693A (zh) * 2018-03-22 2018-12-11 东南大学 基于单目相机的vio快速联合初始化方法
CN109544696A (zh) * 2018-12-04 2019-03-29 中国航空工业集团公司西安航空计算技术研究所 一种基于视觉惯性组合的机载增强合成视景虚实图像精确配准方法
CN110030994A (zh) * 2019-03-21 2019-07-19 东南大学 一种基于单目的鲁棒性视觉惯性紧耦合定位方法
CN110375738A (zh) * 2019-06-21 2019-10-25 西安电子科技大学 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN110411476A (zh) * 2019-07-29 2019-11-05 视辰信息科技(上海)有限公司 视觉惯性里程计标定适配及评价方法和***
CN110763251A (zh) * 2019-10-18 2020-02-07 华东交通大学 视觉惯性里程计优化的方法及***
CN110702107A (zh) * 2019-10-22 2020-01-17 北京维盛泰科科技有限公司 一种单目视觉惯性组合的定位导航方法
CN111156984A (zh) * 2019-12-18 2020-05-15 东南大学 一种面向动态场景的单目视觉惯性slam方法
CN111197984A (zh) * 2020-01-15 2020-05-26 重庆邮电大学 一种基于环境约束下的视觉-惯性运动估计方法
CN111578937A (zh) * 2020-05-29 2020-08-25 天津工业大学 同时优化外参数的视觉惯性里程计***
CN111780754A (zh) * 2020-06-23 2020-10-16 南京航空航天大学 基于稀疏直接法的视觉惯性里程计位姿估计方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
FENG ZHENG: "Trifo-VIO: Robust and Efficient Stereo Visual Inertial Odometry Using Points and Lines", 《2018 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS (IROS)》 *
HUANYU WEN: "PLS-VIO: Stereo Vision-inertial Odometry Based on Point and Line Features", 《2020 INTERNATIONAL CONFERENCE ON HIGH PERFORMANCE BIG DATA AND INTELLIGENT SYSTEMS (HPBD&IS)》 *
蒋满城: "基于点线综合特征的视觉惯性里程计方法研究", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 *
黄仁强: "基于IMU与单目视觉融合算法的视觉惯性里程计软件设计", 《中国优秀博硕士学位论文全文数据库(硕士)信息科技辑》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113298796A (zh) * 2021-06-10 2021-08-24 西北工业大学 一种基于最大后验imu的线特征slam初始化方法
CN113298796B (zh) * 2021-06-10 2024-04-19 西北工业大学 一种基于最大后验imu的线特征slam初始化方法
CN113376669A (zh) * 2021-06-22 2021-09-10 东南大学 一种基于点线特征的单目vio-gnss融合定位算法
CN114234959A (zh) * 2021-12-22 2022-03-25 深圳市普渡科技有限公司 机器人、vslam初始化方法、装置和可读存储介质
CN114234959B (zh) * 2021-12-22 2024-02-20 深圳市普渡科技有限公司 机器人、vslam初始化方法、装置和可读存储介质
CN114998389A (zh) * 2022-06-20 2022-09-02 珠海格力电器股份有限公司 室内定位方法
CN116957958A (zh) * 2023-06-25 2023-10-27 东南大学 一种基于惯性先验校正图像灰度的vio前端改良方法

Also Published As

Publication number Publication date
CN112862768B (zh) 2022-08-02

Similar Documents

Publication Publication Date Title
CN112862768B (zh) 一种基于点线特征的自适应单目vio初始化方法
CN109307508B (zh) 一种基于多关键帧的全景惯导slam方法
CN112634451B (zh) 一种融合多传感器的室外大场景三维建图方法
CN107869989B (zh) 一种基于视觉惯导信息融合的定位方法及***
CN109029433B (zh) 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
Yang et al. Monocular object and plane slam in structured environments
CN110389348B (zh) 基于激光雷达与双目相机的定位与导航方法及装置
CN109166149B (zh) 一种融合双目相机与imu的定位与三维线框结构重建方法与***
CN108682027A (zh) 基于点、线特征融合的vSLAM实现方法及***
CN113108771B (zh) 一种基于闭环直接稀疏视觉里程计的移动位姿估计方法
Liu et al. Direct visual odometry for a fisheye-stereo camera
CN112556719B (zh) 一种基于cnn-ekf的视觉惯性里程计实现方法
CN114323033B (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN112419497A (zh) 基于单目视觉的特征法与直接法相融合的slam方法
CN114485640A (zh) 基于点线特征的单目视觉惯性同步定位与建图方法及***
CN114529576A (zh) 一种基于滑动窗口优化的rgbd和imu混合跟踪注册方法
CN112101160A (zh) 一种面向自动驾驶场景的双目语义slam方法
Chen et al. Stereo visual inertial pose estimation based on feedforward-feedback loops
Li et al. A binocular MSCKF-based visual inertial odometry system using LK optical flow
CN115147344A (zh) 一种增强现实辅助汽车维修中的零件三维检测与跟踪方法
CN113763470B (zh) 点线特征融合的rgbd视觉惯性同时定位与地图构建
CN115218889A (zh) 一种基于点线特征融合的多传感器室内定位方法
Zhao et al. Robust depth-aided rgbd-inertial odometry for indoor localization
Mu et al. Visual navigation features selection algorithm based on instance segmentation in dynamic environment
CN104156933A (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