CN116205947B - 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 - Google Patents

基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 Download PDF

Info

Publication number
CN116205947B
CN116205947B CN202310003442.0A CN202310003442A CN116205947B CN 116205947 B CN116205947 B CN 116205947B CN 202310003442 A CN202310003442 A CN 202310003442A CN 116205947 B CN116205947 B CN 116205947B
Authority
CN
China
Prior art keywords
camera
pose
coordinate system
imu
frame
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
Application number
CN202310003442.0A
Other languages
English (en)
Other versions
CN116205947A (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.)
Harbin Institute of Technology
Original Assignee
Harbin Institute of Technology
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 Harbin Institute of Technology filed Critical Harbin Institute of Technology
Priority to CN202310003442.0A priority Critical patent/CN116205947B/zh
Publication of CN116205947A publication Critical patent/CN116205947A/zh
Application granted granted Critical
Publication of CN116205947B publication Critical patent/CN116205947B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
    • G—PHYSICS
    • 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
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30241—Trajectory
    • 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/30—Subject of image; Context of image processing
    • G06T2207/30244—Camera pose
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computational Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Computing Systems (AREA)
  • Multimedia (AREA)
  • Algebra (AREA)
  • Automation & Control Theory (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Image Analysis (AREA)

Abstract

本发明提出基于相机运动状态的双目‑惯性融合的位姿估计方法、电子设备及存储介质，属于位姿估计技术领域。包括：S1.获取相邻两帧图像特征点对，匹配相邻两帧图像的特征点对；S2.对相邻两帧图像中的IMU测量数据进行预积分处理；S3.基于相邻两帧图像的特征点对进行相机初始位姿估计；S4.将IMU信息位姿与相机初始位姿进行融合；S5.对融合后的相机初始化位姿进行紧耦合位姿优化；S6.对优化后的位姿进行闭环检测与重定位；S7.基于相机运动状态设置关键帧筛选阈值；S8.基于相机运动状态的双目‑惯性融合的位姿估计。解决无法在当设备长时间静态或者极小姿态运动时正确估计位姿，导致轨迹出现误差的问题。

Description

基于相机运动状态的双目-惯性融合的位姿估计方法、电子设 备及存储介质
技术领域
本申请涉及一种位姿估计方法，尤其涉及基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质，属于位姿估计技术领域。
背景技术
同步定位与地图构建(SLAM,Simultaneous Localization and Mapping)一直是机器人技术和计算机视觉领域研究的热点之一。SLAM早期研究是为了实现移动机器人的自主定位和导航，为了能够在未知环境中完成智能化任务，移动机器人需要具备自身状态的认知能力以及周围环境感知能力。在面对一些更复杂的任务，比如在不受控制的条件下进行搜寻和救援任务时，机器人无法快速灵活地完成任务；同时机器人的运动受地形的影响，无法在一些特殊环境执行任务，因此仍需要训练有素的特种人员来执行。随着可穿戴式传感器的不断发展，基于可穿戴式传感器的辅助系统技术已经被引入到很多工业应用中。在上述背景下，可穿戴式SLAM系统的概念被提出。一套融合多传感器的可穿戴式SLAM系统可以在高度动态的条件下实现穿戴人员的精准实时定位，同时发送和记录环境信息，有效协助使用人员在未知环境下执行特殊任务。穿戴式计算平台与机器人平台不同，穿戴式计算平台受体积、重量等限制，通常计算能力非常有限，且对功耗更加敏感。
为此有研究人员提出了以下技术方案：
现有技术一、Campos和Elbira等学者又提出了ORB-SLAM3，并设计了基于ORB-SLAM3的可穿戴场景下的VI-SALM系统，与VI-ORB-SLAM相比多了双目相机和IMU单元的视觉惯性融合支持。
该方法存在的缺点是该系统在后端优化模块特别是回环检测模块，无法在当设备长时间静态或者极小姿态运动时正确估计位姿，导致轨迹出现误差。
现有技术二、Viachaslau Kachurka等学者通过对视觉-惯性SLAM(VI-SLAM)与其它一些传感器之间的优化通信事件，实现了基于双目视觉-IMU惯性的定位方法，并设计了一套可穿戴式协同SLAM系统。该系统的优点在于它能够同时执行两种互补的SLAM方法，以实现实时的位姿估计过程。
该方法存在的缺点是系统设计比较复杂，且硬件成本较高。
发明内容
在下文中给出了关于本发明的简要概述，以便提供关于本发明的某些方面的基本理解。应当理解，这个概述并不是关于本发明的穷举性概述。它并不是意图确定本发明的关键或重要部分，也不是意图限定本发明的范围。其目的仅仅是以简化的形式给出某些概念，以此作为稍后论述的更详细描述的前序。
鉴于此，为解决现有技术中存在的无法在当设备长时间静态或者极小姿态运动时正确估计位姿，导致轨迹出现误差的技术问题，本发明提供基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质
方案一：基于相机运动状态的双目-惯性融合的位姿估计方法，包括以下步骤：
S1.获取摄像头拍摄的视频流中相邻两帧图像特征点对，匹配相邻两帧图像的特征点对；
S2.对相邻两帧图像中的IMU测量数据进行预积分处理，得到IMU信息位姿；
S3.基于相邻两帧图像的特征点对进行相机初始位姿估计；
S4.将S2所述IMU信息位姿与S3所述相机初始位姿进行融合；
S5.对融合后的相机初始化位姿进行紧耦合位姿优化；
S6.对优化后的位姿进行闭环检测与重定位，方法是：判断相机是否回到之前到过的某个位置，当形成回环时进行回环校正和全局优化以消除误差；
S7.基于相机运动状态设置关键帧筛选阈值；
S8.基于相机运动状态的双目-惯性融合的位姿估计。
优选的，S1具体方法是：
S11.提取当前帧图像的FAST角点作为关键点，用BRIEF特征点描述算法计算关键点的特征描述向量，提取方法是：
S111.选取图像中任一像素点p，获取其亮度lp；
S112.设置阈值T，T设为像素点p亮度的20％；
S113.取像素点p点为圆心的半径为3的圆上的16个像素点；
S114.比较像素点p和16个像素点的亮度，若出现连续N个像素点的亮度与像素点p点亮度差超过阈值T，则认为像素点p点是特征点，否则，不认为像素点p点是特征点，执行S111；N取9或12；
S115.依次对图像中的所有像素点重复S111-S114步骤，直至遍历完图像的所有像素点，得到多个特征点；
S12.采用RANSAC算法匹配相邻两帧图像的特征点对，方法是，包括以下步骤；
S121.将采集到的特征点作为样本，将其分为正确数据和异常数据；
S122.将正确数据作为内点；将异常数据作为外点；
S123.随机匹配一组子集作为内点，将模型拟合至该组子集上；形成拟合模型；
S124.利用拟合模型测试其他数据，形成共识集，利用共识集的所有成员对模型进行迭代获得最优模型参数值。
优选的，S2具体方法是：
对系统输入的当前帧图像bk和相邻帧图像bk+1，对IMU的测量模型积分可得位姿：
其中，上标g表示gyro，上标a表示acc，上标w表示在世界坐标系world，上标b表示imu本体坐标系body；则表示下标帧在世界坐标系下的位置，同理表示当前帧在世界坐标系下的位置；表示当前帧在世界坐标系下的速度，同理表示标帧在世界坐标系下的位置；Δtk表示两帧间隔时间；gw为世界坐标系下的重力；dt时间微分；表示下标帧在世界坐标系下的姿态，同理表示当前帧在世界坐标系下的姿态；Rw表示方向余弦矩阵；
加速度计测量值陀螺仪测量值bias游走误差测量值buby，随机白噪声nuny
其中：
对加速度和角速度进行积分并将式(1-1)转换到IMU坐标系得：
式(1-2)中表示当前帧图像bk与相邻帧图像bk+1时刻的位置增量；表示当前帧图像bk与相邻帧图像bk+1时刻的增量；表示当前帧图像bk与相邻帧图像bk+1时刻的旋转矩阵变化
将IMU的离散数据，改为欧拉积分进行处理，积分之后可得：
式(1-3)中、R、ai、δt、ωi、分别表示，表示i+1时刻位置增量的四元数，表示当前图像i+1帧与图像bk时刻的位置增量；表示i+1时刻速度增量的四元数，表示图像i帧与图像bk+1时刻的位置增量；表示i+1时刻旋转矩阵变化的四元数，表示帧图像i+1与图像bk时刻的速度增量；R表示方向余弦矩阵，ai表示i时刻加速度，δt表示时间增量，ωi表示i时刻角速度，表示bias游走误差；
式(1-3)成立的条件是IMU的偏置不会发生改变，系统实际运行时IMU的偏置是不断变化的，因此，对积分后的离散数据进行优化，在迭代计算的过程中，预积分量中有一个与偏置有关的误差：
式(1-4)中分别表示当前帧图像bk与相邻帧图像bk+1时刻的位置增量预测值，当前帧图像bk与相邻帧图像bk+1时刻的位置增量实际值，δba为加速度增量，δbw为角速度增量，表示当前帧图像bk与相邻帧图像bk+1时刻的速度增量；表示当前帧图像bk与相邻帧图像bk+1时刻的旋转矩阵变化；分别为预积分项相对于偏置误差的雅克比；
建立线性高斯误差状态递推方程计算方程的协方差矩阵，同时计算相应的雅可比矩阵，假定测量值与实际值的误差为δ，则在t时刻误差项的导数为：
简写为：
进一步整理可得：
由当前时刻的值可以计算出下一时刻的协方差：
其中初始的协方差矩阵且有
同时得到对应的雅克布矩阵迭代公式：
Jt+δt＝(I+Ftδt)Jt(1-10)。
优选的，S3具体方法是：通过S1所述相邻两帧图像的特征点对估计相机的运动，利用已知的n个3D空间点以及投影位置求解相机位姿，具体包括：
定义世界坐标系下的n个3D点：
pi i＝1,...,n (2-1)
定义世界坐标系下的4个控制点：
cj, j＝1,...,4 (2-2)
使用世界坐标系下的4个控制点的线性组合表示该世界坐标系下的任意三维点：
其中，αij表示其次重心坐标系数，表示世界坐标系下选取的控制点而空间中三维点在相机坐标系中也适用(2-3)
关系：
表示相机坐标系选取的控制点；
设(ui,vi)为3D-2D匹配点对中某一匹配对在后一帧图像的像素坐标，为后帧相机坐标系下控制点的坐标，其中j＝1,...,4，联立相机模型可得：
将其代入式(2-5)前两列整理可得：
代入已知的n个三维点坐标到式(2-6)中可得：
MX＝0 (2-7)
其中，矩阵M大小为2n×12，将等式两边同时左乘M的转置矩阵得：
MTMX＝0 (2-8)
将MTM当作系数矩阵进行SVD分解得：
式(2-9)中N是MTM核空间维数，βi是待定系数，Vi是奇异值最小得特征向量，对应特征值为0；
因为两个不同控制点在两个坐标系下的距离相等，所以求解系数βi，以N＝1为例，其约束公式为：
则有
通过式(2-11)得到相机坐标系下4个控制点的坐标值，优化过后获得最优解，进而得到向量X，再由质心系数获得地图点在相机坐标系下的坐标，根据对应的三维点集合估计不同帧间的相机运动。
优选的，S4具体方法是：包括以下步骤：
S41.标定获取相机坐标系到IMU坐标系的旋转矩阵
S42.通过相机旋转间的约束关系实现陀螺仪偏置的标定；
S43.使用IMU的平移约束估计速度和重力的方向，优化重力向量；
S44.计算相机初始坐标系C到世界坐标系W的旋转矩阵RWC，并将轨迹对齐到世界坐标系下，方法是：在纯双目的视觉SLAM中，将滑动窗口内第k帧的坐标表示为即相对于相机初始坐标系的位姿，利用相机与IMU之间的转换矩阵将坐标从相机坐标系转换到IMU坐标系下，则有：
假设为IMU坐标系下[bk,bk+1]时间内IMU预积分结果得到的约束值，利用视觉的状态估计，分别得到两个时刻相对初始帧之间的旋转为结合相机的旋转约束估计陀螺仪的偏差有：
式(3-13)为一个最小二乘问题，B为滑窗内所有图像帧的集合，将式(3-1)代入上式可得：
整理后的：
由于式(3-15)中四元数只需考虑虚部，实部无需标定，即有：
将式(3-16)左边转换为正定阵，再用Cholesky分解可得：
在获取到陀螺仪的bias的初始值bω后，便可对IMU预积分项重新计算。
优选的，S5具体方法是：
假定某一时刻下，双目相机观测到n个空间中的路标点集，记为Pi＝[Xi,Yi,Zi]T，pi＝[ui,vi]T为路标点投影到像素坐标系下的坐标，将相机的旋转R和平移t用李代数表示成ξ，则像素坐标于空间点坐标的关系表示成：
将路标点用估状态值投影到图像平面上会和实际观测的坐标形成重投影误差，将所有的误差项进行求和，通过最小化误差的方法，不断迭代计算求解最优状态变量值，其损失函数为：
通过非线性优化将视觉和惯性的原始观测数据进行融合，并用优化状态向量的方法得到最小化损失函数；
将视觉构造产生的视觉残差和IMU测量产生的惯性残差联合进行优化
定义待优化的状态向量为：
其中表示相机到IMU的转换参数，由相机-IMU联合标定获得；xi为滑窗内第i帧图像的状态，xk是第k帧图像的IMU状态，其中包含IMU预积分得到的PVQ数据，λi表示第i个路标点的逆深度；
将边缘化的先验信息、视觉残差和惯性残差视觉残差进行联合优化，通过最小化误差的思想方法，找到能使后验估计最大的最优状态向量，将代价函数定义为：
式中，表示第k帧关键帧的状态，χ表示一组地图点的状态集，κj表示观察到地图点j的关键帧集其中，对于视觉重投影误差，使用Huber鲁棒核ρHub来减小误匹配带来的影响。
优选的，S7具体方法是：包括以下步骤：
S71.获取姿态角变化值，方法是：首先将前端计算得到的帧间本质矩阵E用奇异值分解法分解成旋转矩阵R和位移向量T，假定两帧图像JP、JQ，P、Q为两帧图像间对应特征点位姿集合，P到Q的对于两帧图像间对应的特征点可通过旋转矩阵R和位移向量T进行位姿转换：
Q＝RP+t (5-1)
其中
令俯仰角为α，航向角为β，横滚角为γ，则用方向余弦矩阵表示R可得：
其中sμ＝sin(μ),cμ＝cos(μ)，由以上等价关系由旋转矩阵R推算出各个姿态角的值；
S72.根据位移向量的大小和姿态角变化值的大小设定不同的关键帧插入阈值通过分解本质矩阵得到帧间姿态变化；
S73.设置关键帧筛选阈值，方法是：计算当前帧与上一关键帧之间的位姿变化，判断当前帧与上一参考帧的变化程度，判断位移向量的大小变化，若位移大则不改变关键帧的筛选阈值大小；若位移小，再判断姿态角变化大小，若姿态角变化小则提高关键帧的筛选阈值，同时将关键帧插入最大时间间隔增大，反之则降低关键帧的筛选阈值。
优选的，S8具体方法是：
S81.当IMU初始化完成且系统跟踪正常时，对IMU数据的计算得到帧间姿态变化，判断当前机体是否处在相对静止的状态；假定连续三帧间两两相邻的帧间变化时相同的，当连续三帧的帧间位姿变化都极小时，即认为当前机体处在相对静止状态；
S82.将对图像特征采取隔帧提取的方式，未提取特征的图像帧使用IMU预测的位姿数据。
方案二：一种电子设备，包括存储器和处理器，存储器存储有计算机程序，所述的处理器执行所述计算机程序时实现方案一所述基于相机运动状态的双目-惯性融合的位姿估计方法的步骤。
方案三：一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现方案一所述的基于相机运动状态的双目-惯性融合的位姿估计方法。
本发明的有益效果如下：
(1)对双目与IMU融合的定位算法进行研究，基于双目-惯性融合的方式估计采出相机的运动状态，通过减少相机相对静态时的图像特征提取数目避免不必要的运算，同时以惯性测量数据计算出的位姿估计保证系统位姿跟踪正常；
(2)对前端关键帧筛选策略进行了改进，基于相机不同的运动状态改变关键帧的插入频率，减少前端筛选的关键帧数据，提高后端优化与回环检测线程的效率。通过对数据集进行测试，实验结果表明该方法可实现位姿估计精度不变的情况下减少冗余关键帧数量；
(3)在相机频繁抖动和快速转角时也能保证跟踪良好，同时位姿精度也更高。
附图说明
此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：
图1为基于相机运动状态的双目-惯性融合的位姿估计方法流程示意图；
图2为FAST角点提取示意图；
图3为ORB特征匹配示意图；
图4为图像帧与IMU对应关系示意图；
图5为重投影误差示意图；
图6为视觉-惯性耦合滑动窗口示意图；
图7为回环校正示意图；
图8为V101序列轨迹对比和轨迹误差优化前后对比示意图；
图9为V201序列轨迹对比和轨迹误差优化前后对比示意图；
图10为V103序列轨迹对比和轨迹误差优化前后对比示意图；
图11为V203序列轨迹对比和轨迹误差优化前后对比示意图。
具体实施方式
为了使本申请实施例中的技术方案及优点更加清楚明白，以下结合附图对本申请的示例性实施例进行进一步详细的说明，显然，所描述的实施例仅是本申请的一部分实施例，而不是所有实施例的穷举。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。
实施例1、参照图1-图11说明本实施方式，基于相机运动状态的双目-惯性融合的位姿估计方法，首先，进行特征提取与匹配，获取当前帧与上一帧的匹配特征点以及当前帧左、右摄图像的匹配特征点对；本实施例采用ORB特征法的方案，ORB特征是由FAST角点和BRIEF特征描述子两部分组成，在ORB特征提取过程中，先提取图像的FAST角点作关键点，然后用BRIEF描述子计算关键点的特征描述向量。FAST角点是实时系统中视觉特征匹配寻找关键点的首选方法，其关键在于只对图像中像素灰度变化剧烈的区域进行检测，使得其检测速度比较快；参照图2所示。
具体的，获取摄像头拍摄的视频流中相邻两帧图像特征点对，匹配相邻两帧图像的特征点对,具体方法是：
S11.提取当前帧图像的FAST角点作为关键点，用BRIEF特征点描述算法计算关键点的特征描述向量，提取方法是：
S111.选取图像中任一像素点p，获取其亮度lp；
S112.设置阈值T，T设为像素点p亮度的20％；
S113.取像素点p点为圆心的半径为3的圆上的16个像素点；
S114.比较像素点p和16个像素点的亮度，若出现连续N个像素点的亮度与像素点p点亮度差超过阈值T，则认为像素点p点是特征点，否则，不认为像素点p点是特征点，执行S111；N取9或12；
S115.依次对图像中的所有像素点重复S111-S114步骤，直至遍历完图像的所有像素点，得到多个特征点；
特征提取完成之后，紧接着需要对相邻帧图像的特进行匹配以建立帧间的数据关联。本实施例采用RANSAC算法，它能鲁棒地估计模型参数。
具体的，S12.采用RANSAC算法匹配相邻两帧图像的特征点对，方法是，包括以下步骤；
S121.将采集到的特征点作为样本，将其分为正确数据和异常数据；
S122.将正确数据作为内点；将异常数据作为外点；
S123.随机匹配一组子集作为内点，将模型拟合至该组子集上；形成拟合模型；
S124.利用拟合模型测试其他数据，形成共识集，利用共识集的所有成员对模型进行迭代获得最优模型参数值。
如果有足够多的点被归类为共识集的一部分，则认为估计模型足够好。之后通过使用共识集的所有成员重新估计模型来改进模型，经过多次迭代之后将得到最优的模型参数值，如图3所示为ORB特征匹配的结果。
接下来进行IMU预积分，在进行双目相机与IMU的紧耦合之前，需要将IMU的数据进行预处理。由于IMU的工作频率远高于相机的采样频率，往往两帧图像间存在较多帧IMU测量数据，如图4所示。由于系统是对从bk时刻到bk+1时刻IMU的数据进行累积积分得到bk+1时刻的PVQ状态数据，而后端优化的过程中，往往会因为优化迭代的原因使得前面某一帧的状态数据发生改变，一旦发生改变则意味着该帧后面的所有数据都需要重新积分计算。大量重复计算工作将大大消耗计算资源，因此需要对相邻两帧图像间的IMU测量数据进行预积分处理。
预积分的目的就是使得积分运算里的内容只和IMU的测量值有关，而与被优化的状态变化量无关，这样当出现前向某一帧被优化时，只需要再积分一次，避免重复积分运算。具体的，S2.对相邻两帧图像中的IMU测量数据进行预积分处理，得到IMU信息位姿；
IMU传感器具有成本低、体积小、精度高等优点，可以通过其获取高频的惯性测量值。IMU可以通过其内部的加速度计和陀螺仪测量载体自身运动的三轴加速度和三轴角速度，常被用于移动载体的定位和运动状态的估计。IMU的误差主要来源于高斯白噪声和bias随机游走这两种随机误差。其中高斯白噪声为IMU数据在一定时间内的均值为0，方差为σ，每个时刻间相互独立的高斯过程n(t)：
E[n(t)]＝0
E[n(t1)n(t2)]＝σ2δ(t1-t2)
其中δ(t)表示狄拉克函数，由于在实际情况下，IMU测量的为离散数据，在只考虑高斯白噪声时有：
其协方差为：
将其转换为离散高斯白噪声：
nd[k]＝σdω[k]
其中ω[k]～N(0,1),IMU的随机游走误差用维纳过程表示其随时间变化的过程为：
式中，ω(t)是方差为1的高斯白噪声，对bias随机游走部分积分可得：
对式进行离散化处理可得：
bd[k]＝bd[k-1]+σdbω[k]
其中σbd为离散的bias随机游走的方差。
根据上述分析的IMU误差模型，在只计算高斯白噪声和bias随机游走误差时，IMU的测量模型为：
式中，上标b表示IMU坐标系，g表示陀螺仪，w表示世界坐标系，ω和a为真实值，和为测量值，gw为世界坐标系下的重力，qbw为坐标变换矩阵。IMU的运动模型则表示为：
由此可推出连续时间下，从i时刻到j时刻的位置P、速度V、旋转Q可表示为：
将其离散化后得到：
对系统输入的当前帧图像bk和相邻帧图像bk+1，对IMU的测量模型积分可得位姿：
其中，上标g表示gyro，上标a表示acc，上标w表示在世界坐标系world，上标b表示imu本体坐标系body；则表示下标帧在世界坐标系下的位置，同理表示当前帧在世界坐标系下的位置；表示当前帧在世界坐标系下的速度，同理表示标帧在世界坐标系下的位置；Δtk表示两帧间隔时间；gw为世界坐标系下的重力；dt时间微分；表示下标帧在世界坐标系下的姿态，同理表示当前帧在世界坐标系下的姿态；Rw表示方向余弦矩阵；
加速度计测量值陀螺仪测量值bias游走误差测量值buby，随机白噪声nuny
其中：
可以看出IMU的状态传递与上一帧有关，由于系统对相机位姿进行优化时会导致前一帧的状态值发生变化，导致系统需要重新累计计算积分。为了避免重复积分所带来的计算开销，算法采用预积分的方式处理IMU的数据；
对加速度和角速度进行积分并将式(1-1)转换到IMU坐标系得：
式(1-2)中表示当前帧图像bk与相邻帧图像bk+1时刻的位置增量；表示当前帧图像bk与相邻帧图像bk+1时刻的增量；表示当前帧图像bk与相邻帧图像bk+1时刻的旋转矩阵变化；
式(1-2)中各行分别为bk到bk+1时刻的位置增量、速度增量和旋转矩阵变化的四元数。通过转换成该式后状态变量的传递只与IMU本身的参数有关，与其他状态无关。利用预积分的处理方式，当前一时刻的状态值发生变化时，只需要利用预积分得到的增量值进行一次积分便可以推到计算出当前帧的状态值，从而大大减少系统前端的计算量。由于上式是基于连续时间积分所得，而IMU的测量数据是离散数据，故需要改为欧拉积分进行处理。积分之后可得：
式(1-3)中、R、ai、δt、ωi、分别表示，表示i+1时刻位置增量的四元数，表示当前图像i+1帧与图像bk时刻的位置增量；表示i+1时刻速度增量的四元数，表示图像i帧与图像bk+1时刻的位置增量；表示i+1时刻旋转矩阵变化的四元数，表示帧图像i+1与图像bk时刻的速度增量；R表示方向余弦矩阵，ai表示i时刻加速度，δt表示时间增量，ωi表示i时刻角速度，表示bias游走误差；
式(1-3)成立的条件是IMU的偏置不会发生改变，系统实际运行时IMU的偏置是不断变化的，因此，对积分后的离散数据进行优化，在迭代计算的过程中，预积分量中有一个与偏置有关的误差：
式(1-4)中分别表示当前帧图像bk与相邻帧图像bk+1时刻的位置增量预测值，当前帧图像bk与相邻帧图像bk+1时刻的位置增量实际值，δba为加速度增量，δbw为角速度增量，表示当前帧图像bk与相邻帧图像bk+1时刻的速度增量；表示当前帧图像bk与相邻帧图像bk+1时刻的旋转矩阵变化；分别为预积分项相对于偏置误差的雅克比；
IMU预积分得到的测量值需要用到非线性优化中，因而要建立线性高斯误差状态递推方程来计算出方程的协方差矩阵，同时计算出相应的雅可比矩阵。假定测量值与实际值的误差为δ，则在t时刻误差项的导数为：
具体的，建立线性高斯误差状态递推方程计算方程的协方差矩阵，同时计算相应的雅可比矩阵，假定测量值与实际值的误差为δ，则在t时刻误差项的导数为：
简写为：
进一步整理可得：
由当前时刻的值可以计算出下一时刻的协方差：
其中初始的协方差矩阵且有
同时得到对应的雅克布矩阵迭代公式：
Jt+δt＝(I+Ftδt)Jt(1-10)。
视觉SLAM是通过摄像头拍摄的视频流里连续两帧图像之间的匹配特征点对来估计相机的运动，利用已知的n个3D空间点以及它们的投影位置来求解相机位姿。本实施例采用EPnP法计算位姿，EPnP法需要至少6对匹配点，且当有更多匹配点对信息时能够迭代优化以获得更高精度的位姿，同时还具有更好的鲁棒性。
具体的，S3.基于相邻两帧图像的特征点对进行相机初始位姿估计,具体方法是：通过S1所述相邻两帧图像的特征点对估计相机的运动，利用已知的n个3D空间点以及投影位置求解相机位姿，具体包括：
定义世界坐标系下的n个3D点：
pi i＝1,...,n (2-1)
定义世界坐标系下的4个控制点：
cj, j＝1,...,4 (2-2)
使用世界坐标系下的4个控制点的线性组合表示该世界坐标系下的任意三维点：
其中，αij表示其次重心坐标系数，表示世界坐标系下选取的控制点，而空间中三维点在相机坐标系中也适用(2-3)
关系：
表示相机坐标系选取的控制点；
设(ui,vi)为3D-2D匹配点对中某一匹配对在后一帧图像的像素坐标，为后帧相机坐标系下控制点的坐标，其中j＝1,...,4，联立相机模型可得：
将其代入式(2-5)前两列整理可得：
代入已知的n个三维点坐标到式(2-6)中可得：
MX＝0 (2-7)
其中，矩阵M大小为2n×12，将等式两边同时左乘M的转置矩阵得：
MTMX＝0 (2-8)
将MTM当作系数矩阵进行SVD分解得：
式(2-9)中N是MTM核空间维数，βi是待定系数，Vi是奇异值最小得特征向量，对应特征值为0；
因为两个不同控制点在两个坐标系下的距离相等，所以求解系数βi，以N＝1为例，其约束公式为：
则有
通过式(2-11)得到相机坐标系下4个控制点的坐标值，优化过后获得最优解，进而得到向量X，再由质心系数获得地图点在相机坐标系下的坐标，根据对应的三维点集合估计不同帧间的相机运动。
视觉的初始化位姿估计完成后，需要将利用视觉信息估计得到的状态值与IMU预积分的结果进行对齐，视觉信息的参考坐标系为初始化完成后滑动窗口内的第一帧图像的相机坐标系，假定相机初始的坐标系C和世界坐标系重合，根据几何约束计算IMU与视觉之间的联系。
具体的，S4.将S2所述IMU信息位姿与S3所述相机初始位姿进行融合，S4具体方法是：包括以下步骤：
S41.标定获取相机坐标系到IMU坐标系的旋转矩阵
S42.通过相机旋转间的约束关系实现陀螺仪偏置的标定；
S43.使用IMU的平移约束估计速度和重力的方向，优化重力向量；
S44.计算相机初始坐标系C到世界坐标系W的旋转矩阵RWC，并将轨迹对齐到世界坐标系下，方法是：在纯双目的视觉SLAM中，将滑动窗口内第k帧的坐标表示为即相对于相机初始坐标系的位姿，利用相机与IMU之间的转换矩阵将坐标从相机坐标系转换到IMU坐标系下，则有：
假设为IMU坐标系下[bk,bk+1]时间内IMU预积分结果得到的约束值，利用视觉的状态估计，分别得到两个时刻相对初始帧之间的旋转为结合相机的旋转约束估计陀螺仪的偏差有：
式(3-13)为一个最小二乘问题，B为滑窗内所有图像帧的集合，将式(3-1)代入上式可得：
整理后的：
由于式(3-15)中四元数只需考虑虚部，实部无需标定，即有：
将式(3-16)左边转换为正定阵，再用Cholesky分解可得：
在获取到陀螺仪的bias的初始值bω后，便可对IMU预积分项重新计算。
传感器的噪声和特征误匹配的影响会产生一定偏差，开始运行时，位姿估计偏差往往都比较小，但随着运行时间的增长，偏差也会越来越大，对相机的轨迹估算精度产生严重影响，因此，需要初始数据进行优化，减少估计误差，本实施例采用基于非线性优化的图优化方法；
BA即Bundle Adjustment，又称为光束法平差，是PnP问题常见的非线性优化方法；BA优化问题通常将VO中估算出的位姿和地图点作为初值，然后不断查找下降方向，直到整体代价函数最小时即可获得最佳的相机位姿和路标点，BA问题本质上是一个最小化重投影误差的问题，如图5所示为重投影误差示意图，图中点P为真实世界中的一个空间点，P1、P2为点P在不同相机位姿下所拍摄图像的真实投影；利用图像对特征点进行三角定位，用对极几何法构建三角形来确定三维空间点的位置和空间点到像素点的坐标转换关系[R,t]，最后使用三维点坐标和相机位姿进行二次投影得到点由于计算出的三维点坐标和相机位姿存在误差，导致重投影后的像素点与真实映射的像素点存在偏差，因此称作重投影误差。
具体的，S5.对融合后的相机初始化位姿进行紧耦合位姿优化，S5具体方法是：
假定某一时刻下，双目相机观测到n个空间中的路标点集，记为Pi＝[Xi,Yi,Zi]T，pi＝[ui,vi]T为路标点投影到像素坐标系下的坐标，将相机的旋转R和平移t用李代数表示成ξ，则像素坐标于空间点坐标的关系表示成：
将路标点用估状态值投影到图像平面上会和实际观测的坐标形成重投影误差，将所有的误差项进行求和，通过最小化误差的方法，不断迭代计算求解最优状态变量值，其损失函数为：
如图6所示为视觉-惯性紧耦合的滑动窗口模型，本实施例采用滑动窗口算法，通过非线性优化将视觉和惯性的原始观测数据进行融合，并用优化状态向量的方法得到最小化损失函数；
将视觉构造产生的视觉残差和IMU测量产生的惯性残差联合进行优化；
定义待优化的状态向量为：
其中表示相机到IMU的转换参数，由相机-IMU联合标定获得；xi为滑窗内第i帧图像的状态，xk是第k帧图像的IMU状态，其中包含IMU预积分得到的PVQ数据，λi表示第i个路标点的逆深度；
通过利用相机拍摄标定板录制数据包，再利用kalibr包标定相机的内外参数
将边缘化的先验信息、视觉残差和惯性残差视觉残差进行联合优化，通过最小化误差的思想方法，找到能使后验估计最大的最优状态向量，将代价函数定义为：
式中，表示第k帧关键帧的状态，χ表示一组地图点的状态集，κj表示观察到地图点j的关键帧集其中，对于视觉重投影误差，使用Huber鲁棒核ρHub来减小误匹配带来的影响。
S6.对优化后的位姿进行闭环检测与重定位，方法是：判断相机是否回到之前到过的某个位置，当形成回环时进行回环校正和全局优化以消除误差；参照图7；
为了减少冗余数据，在视差判定的基础上，基于相机运动状态设定不同的关键帧筛选阈值，控制不同运动状态下的关键帧插入频率。同样也可以减少不必要的计算，相机处于相对静态时利用IMU处理数据进行位姿估计，减少特征提取和匹配的处理帧数。
具体的方法是，在匹配前，将当前帧及其共视图中的每个关键帧转化为词袋向量，计算二者间的向量距离，该距离即为二者间的匹配得分，之后对分数进行排序，将最大距离即最小的分设为查找候选关键帧的阈值，然后在除去共视图关键帧的关键帧数据库中将每个关键帧转化为词袋向量进行描述并计算其与当前帧的词袋向量距离，逐个将距离与前面设定的阈值进行比较，当得分高于阈值时便将其设为候选关键帧；再计算候选关键帧对应共视图与当前帧的词袋匹配分数的总和，选出候选关键帧中分数最高的将其设为阈值1，剔掉分数小于阈值1*0.75的候选关键帧；最后取剩余关键帧的连续三张与当前帧构成的连续三张进行几何验证判断是否出现回环；
当回环判定成功时，需要进行回环校正，回环校正主要有两个步骤：一是回环融合，将回环检测匹配到的特征点所相关联的地图点融合，将重复的地图点剔除；二是进行位姿校正，首先是进行当前帧位姿校正，利用回环检测得到的基础矩阵算出当前帧和回环帧之间的位姿转换关系来校正当前帧位姿，然后利用BA优化进行回环内所有帧的位姿校正。
具体的，S7.基于相机运动状态设置关键帧筛选阈值，S7具体方法是：包括以下步骤：
S71.获取姿态角变化值，方法是：首先将前端计算得到的帧间本质矩阵E用奇异值分解法分解成旋转矩阵R和位移向量T，假定两帧图像JP、JQ，P、Q为两帧图像间对应特征点位姿集合，P到Q的对于两帧图像间对应的特征点可通过旋转矩阵R和位移向量T进行位姿转换：
令俯仰角为α，航向角为β，横滚角为γ，则用方向余弦矩阵表示R可得：
R(α,β,γ)＝Rz(γ)Ry(β)Rz(α)
其中sμ＝sin(μ),cμ＝cos(μ)，由以上等价关系由旋转矩阵R推算出各个姿态角的值；
S72.根据位移向量的大小和姿态角变化值的大小设定不同的关键帧插入阈值通过分解本质矩阵得到帧间姿态变化；
S73.设置关键帧筛选阈值，方法是：计算当前帧与上一关键帧之间的位姿变化，判断当前帧与上一参考帧的变化程度，判断位移向量的大小变化，若位移大则不改变关键帧的筛选阈值大小；若位移小，再判断姿态角变化大小，若姿态角变化小则提高关键帧的筛选阈值，同时将关键帧插入最大时间间隔增大，反之则降低关键帧的筛选阈值。
S8.基于相机运动状态的双目-惯性融合的位姿估计，S8具体方法是：
S81.当IMU初始化完成且系统跟踪正常时，对IMU数据的计算得到帧间姿态变化，判断当前机体是否处在相对静止的状态；假定连续三帧间两两相邻的帧间变化时相同的，当连续三帧的帧间位姿变化都极小时，即认为当前机体处在相对静止状态；
S82.将对图像特征采取隔帧提取的方式，未提取特征的图像帧使用IMU预测的位姿数据。
本实施例的效果：
为了验证本实施例的有效性，该部分实验主要基于EUROC公共数据集进行仿真测试，以此来验证对同一输入序列下优化后的算法计算量要小于优化前。
首先是验证关键帧筛选策略的有效性，依旧是选用同一场景下不同运动状态的数据集序列进行测试，包括MH01、MH04、V101、V103、V201、V203等六组数据。实验主要测试的指标有跟踪线程筛选送入关键帧缓冲队列的关键帧数量、局部建图线程与回环检测线程的时间开销、估计轨迹与真实轨迹的误差。测试结果如表表1-1优化前后的关键帧数量变化表、1-2优化前后的局部建图线程时间开销表和表1-3优化前后的回环检测线程时间开销表所示。
表1-1优化前后的关键帧数量变化
MH01 MH04 V101 V103 V201 V203
优化前(帧) 602 383 373 391 310 512
优化后(帧) 487 307 214 330 182 465
表1-2优化前后的局部建图线程时间开销
MH01 MH04 V101 V103 V201 V203
优化前(s) 155.26 78.77 105.58 84.47 70.94 88.02
优化后(s) 127.43 63.98 55.32 69.51 38.08 78.15
表1-3优化前后的回环检测线程时间开销
MH01 MH04 V101 V103 V201 V203
优化前(s) 3.81 3.44 3.04 4.14 3.13 3.88
优化后(s) 3.37 3.01 1.55 3.07 1.29 3.49
从以上表格中的数据可以看出，算法优化后筛选的关键帧数量明显减少，其中V101数据集序列和V201数据集序列的结果最好，关键帧数量较优化前减少了40％以上。对于运动变化较为剧烈的V203数据集序列而言，关键帧数量虽然有减少，但只比优化前减少了9％。剩余的几个数据集序列的优化后的关键帧的减少量基本都在15％到20％以内。综合以上结果可以证明，本文提出的基于相机运动状态的关键帧筛选策略在相机运行平稳时能有效减少冗余关键帧数量，而在相机运动变化较大时又能保证不丢失关键帧信息。因为局部建图线程只对关键帧数据进行处理，所以通过减少冗余关键帧的方式可以有效地减少局部建图线程的任务量，节约平台算力。而由于以上数据集序列的运行轨迹均未检测出回环，所以回环检测线程的时间开销基本没有太大变化。
从计算开销的角度来看，本发明设计的关键帧筛选策略能够实现系统计算量的减少，但对于SLAM系统而言，最关键的指标还是位姿估计精度。首先从位姿跟踪状况上看，所有数据集都能做到位姿跟踪良好，并未出现因光线过暗或相机运动状态较为剧烈而引起的跟踪丢失的现象。这证明双目-惯性融合的VI-SLAM系统能够保证穿戴场景下对跟踪时高鲁棒性的需求。另一方面，为了评估优化后的算法精度是否准确，将对比算法估计的位姿轨迹与数据集提供的真实轨迹的绝对误差。在这将对关键帧数量减少最多的V101和V201数据集序列、以及关键帧数量减少最小的V203和V103数据集序列的误差进行对比分析。参照图8(a)V101序列轨迹对比优化前和(b)V101序列轨迹对比优化后可以看出，优化前后的精度基本保持一致，绝对位姿误差中值几乎没有太大变化，参照图8(c)V101序列轨迹误差优化前和(d)V101序列轨迹误差优化后中轨迹误差图中可以看出，除了优化后的最大绝对轨迹误差比优化前略微高一点以外，平均绝对位姿误差和优化前几乎完全相同。进一步分析可以看出，误差最大的地方为相机进行快速转角运动时，这也是优化算法在相机运动姿态变化较大时要保持高频率插入关键帧的原因。
参照图9可以看出，(图9中a表示V201序列轨迹对比优化前，b表示V201序列轨迹对比优化后，c表示V201序列轨迹误差优化前，d表示V201序列轨迹误差优化后)；V201序列的结果与V101类似，优化前后的轨迹精度基本保持一致。从V101和V201序列的结果可以看出，对于相机运行平稳的状态下，算法优化后减少的关键帧并未影响轨迹估计的精度，与优化前的精度基本保持一致，证实该优化策略能够在保证精度的同时减少系统冗余数据量。
参照图10，可以看出，(图10中a表示V103序列轨迹对比优化前，b表示V103序列轨迹对比优化后，c表示V103序列轨迹误差优化前，d表示V103序列轨迹误差优化后)；V103序列数据集下，相机大部分时间都处在曲线运动状态下，这也可以说明V103序列数据集优化后关键帧数量变化相对较小，是因为优化策略为了保证关键信息不丢失，设定了在姿态变化较大时关键帧插入频率增大，从而使得V103序列优化后的效果不如V101和V201序列。尽管优化带来的收益没有那么大，但这也说明该策略在相机运动变化较快的情况下能够保证轨迹估计的精度。
对于运动轨迹较为剧烈的V203序列数据集，参照图11可以看出，(图11中a表示V203序列轨迹对比优化前，b表示V203序列轨迹对比优化后，c表示V203序列轨迹误差优化前，d表示V203序列轨迹误差优化后)算法减少筛选的关键帧数量后，系统的位姿估计精度并没有发生太大变换，平均绝对位姿误差与优化前保持一致。这说明对于运动状态变换较大的情况，该优化策略能够在保证系统精度的前提下最大化地去除冗余数据，避免不必要的计算任务，提高系统运行效率。
为了验证基于相机运动状态的双目与IMU融合的位姿估计策略的有效性，本实施例在实验过程中记录各数据集序列运行时所消耗的特征提取时间以及每次运行时未提取特征的图像帧数，具体结果如表1-4位姿估计策略优化前后对比表所示。
表1-4位姿估计策略优化前后对比
从表中数据可以明显看出，优化后的算法可以有效减少特征提取帧数。对于V101和V201序列这种运动状态较为平稳且图像采集良好的情况下，该策略可以时特征提取用时减少，最大减少了17％的用时。而V103和V203序列这种转角运动多、相机抖动大的情况下，也能带来计算量的减少，但由于算法需要保证位姿估计的精度，优化效果略低于运动平稳状态下的结果。
综合上述分析，可以证实本实施例可以在保证系统轨迹估计精度的前提下，通过减少冗余数据，降低系统计算量，达到提升性能的目的。
实施例2、本发明的计算机装置可以是包括有处理器以及存储器等装置，例如包含中央处理器的单片机等。并且，处理器用于执行存储器中存储的计算机程序时实现上述的基于CREO软件的可修改由关系驱动的推荐数据的推荐方法的步骤。
所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。
实施例3、计算机可读存储介质实施例
本发明的计算机可读存储介质可以是被计算机装置的处理器所读取的任何形式的存储介质，包括但不限于非易失性存储器、易失性存储器、铁电存储器等，计算机可读存储介质上存储有计算机程序，当计算机装置的处理器读取并执行存储器中所存储的计算机程序时，可以实现上述的基于CREO软件的可修改由关系驱动的建模数据的建模方法的步骤。
所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。
尽管根据有限数量的实施例描述了本发明，但是受益于上面的描述，本技术领域内的技术人员明白，在由此描述的本发明的范围内，可以设想其它实施例。此外，应当注意，本说明书中使用的语言主要是为了可读性和教导的目的而选择的，而不是为了解释或者限定本发明的主题而选择的。因此，在不偏离所附权利要求书的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。对于本发明的范围，对本发明所做的公开是说明性的，而非限制性的，本发明的范围由所附权利要求书限定。

Claims (5)

1.基于相机运动状态的双目-惯性融合的位姿估计方法，其特征在于，包括以下步骤：
S1.获取摄像头拍摄的视频流中相邻两帧图像特征点对，匹配相邻两帧图像的特征点对；
S2.对相邻两帧图像中的IMU测量数据进行预积分处理，得到IMU信息位姿，方法是：
对系统输入的当前帧图像bk和相邻帧图像bk+1，对IMU的测量模型积分得位姿：
其中，上标g表示gyro，上标a表示acc，上标w表示在世界坐标系world，上标b表示imu本体坐标系body；则表示下标帧在世界坐标系下的位置，同理表示当前帧在世界坐标系下的位置；表示当前帧在世界坐标系下的速度，同理表示标帧在世界坐标系下的位置；Δtk表示两帧间隔时间；gw为世界坐标系下的重力；dt时间微分；表示下标帧在世界坐标系下的姿态，同理表示当前帧在世界坐标系下的姿态；Rw表示方向余弦矩阵；
加速度计测量值陀螺仪测量值bias游走误差测量值babg，随机白噪声nang其中：
对加速度和角速度进行积分并将式(1-1)转换到IMU坐标系得：
式(1-2)中表示当前帧图像bk与相邻帧图像bk+1时刻的位置增量；表示当前帧图像bk与相邻帧图像bk+1时刻的增量；表示当前帧图像bk与相邻帧图像bk+1时刻的旋转矩阵变化
将IMU的离散数据，改为欧拉积分进行处理，积分之后得：
式(1-3)中、R、ai、δt、ωi、分别表示，表示i+1时刻位置增量的四元数，表示当前图像i+1帧与图像bk时刻的位置增量；表示i+1时刻速度增量的四元数，表示图像i帧与图像bk+1时刻的位置增量；表示i+1时刻旋转矩阵变化的四元数，表示帧图像i+1与图像bk时刻的速度增量；R表示方向余弦矩阵，ai表示i时刻加速度，δt表示时间增量，ωi表示i时刻角速度，表示bias游走误差；
式(1-3)成立的条件是IMU的偏置不会发生改变，系统实际运行时IMU的偏置是不断变化的，因此，对积分后的离散数据进行优化，在迭代计算的过程中，预积分量中有一个与偏置有关的误差：
式(1-4)中δbw、分别表示当前帧图像bk与相邻帧图像bk+1时刻的位置增量预测值，当前帧图像bk与相邻帧图像bk+1时刻的位置增量实际值，δba为加速度增量，δbw为角速度增量，表示当前帧图像bk与相邻帧图像bk+1时刻的速度增量；表示当前帧图像bk与相邻帧图像bk+1时刻的旋转矩阵变化；分别为预积分项相对于偏置误差的雅克比；
建立线性高斯误差状态递推方程计算方程的协方差矩阵，同时计算相应的雅可比矩阵，假定测量值与实际值的误差为δ，则在t时刻误差项的导数为：
简写为：
进一步整理得：
由当前时刻的值计算出下一时刻的协方差：
其中初始的协方差矩阵且有
同时得到对应的雅克布矩阵迭代公式：
Jt+δt＝(I+Ftδt)Jt (1-10)；
S3.基于相邻两帧图像的特征点对进行相机初始位姿估计，方法是：通过S1所述相邻两帧图像的特征点对估计相机的运动，利用已知的n个3D空间点以及投影位置求解相机位姿，具体包括：
定义世界坐标系下的n个3D点：
pi i＝1,...,n (2-1)
定义世界坐标系下的4个控制点：
cj,j＝1,...,4 (2-2)
使用世界坐标系下的4个控制点的线性组合表示该世界坐标系下的任意三维点：
其中，αij表示其次重心坐标系数，表示世界坐标系下选取的控制点，而空间中三维点在相机坐标系中也适用(2-3)
关系：
表示相机坐标系选取的控制点；
设(ui,vi)为3D-2D匹配点对中某一匹配对在后一帧图像的像素坐标，为后帧相机坐标系下控制点的坐标，其中j＝1,...,4，联立相机模型得：
将其代入式(2-5)前两列整理得：
代入已知的n个三维点坐标到式(2-6)中得：
MX＝0 (2-7)
其中，矩阵M大小为2n×12，将等式两边同时左乘M的转置矩阵得：
MTMX＝0 (2-8)
将MTM当作系数矩阵进行SVD分解得：
式(2-9)中N是MTM核空间维数，βi是待定系数，Vi是奇异值最小得特征向量，对应特征值为0；
因为两个不同控制点在两个坐标系下的距离相等，所以求解系数βi，以N＝1为例，其约束公式为：
则有
通过式(2-11)得到相机坐标系下4个控制点的坐标值，优化过后获得最优解，进而得到向量X，再由质心系数获得地图点在相机坐标系下的坐标，根据对应的三维点集合估计不同帧间的相机运动；
S4.将S2所述IMU信息位姿与S3所述相机初始位姿进行融合，包括以下步骤：
S41.标定获取相机坐标系到IMU坐标系的旋转矩阵
S42.通过相机旋转间的约束关系实现陀螺仪偏置的标定；
S43.使用IMU的平移约束估计速度和重力的方向，优化重力向量；
S44.计算相机初始坐标系C到世界坐标系W的旋转矩阵RWC，并将轨迹对齐到世界坐标系下，方法是：在纯双目的视觉SLAM中，将滑动窗口内第k帧的坐标表示为即相对于相机初始坐标系的位姿，利用相机与IMU之间的转换矩阵将坐标从相机坐标系转换到IMU坐标系下，则有：
假设为IMU坐标系下[bk,bk+1]时间内IMU预积分结果得到的约束值，利用视觉的状态估计，分别得到两个时刻相对初始帧之间的旋转为结合相机的旋转约束估计陀螺仪的偏差有：
式(3-13)为一个最小二乘问题，B为滑窗内所有图像帧的集合，将式(3-1)代入上式得：
整理后的：
由于式(3-15)中四元数只需考虑虚部，实部无需标定，即有：
将式(3-16)左边转换为正定阵，再用Cholesky分解得：
在获取到陀螺仪的bias的初始值bω后，对IMU预积分项重新计算；
S5.对融合后的相机初始化位姿进行紧耦合位姿优化，方法是：
假定某一时刻下，双目相机观测到n个空间中的路标点集，记为Pi＝[Xi,Yi,Zi]T，pi＝[ui,vi]T为路标点投影到像素坐标系下的坐标，将相机的旋转R和平移t用李代数表示成ξ，则像素坐标于空间点坐标的关系表示成：
将路标点用估状态值投影到图像平面上会和实际观测的坐标形成重投影误差，将所有的误差项进行求和，通过最小化误差的方法，不断迭代计算求解最优状态变量值，其损失函数为：
通过非线性优化将视觉和惯性的原始观测数据进行融合，并用优化状态向量的方法得到最小化损失函数；
将视觉构造产生的视觉残差和IMU测量产生的惯性残差联合进行优化
定义待优化的状态向量为：
其中表示相机到IMU的转换参数，由相机-IMU联合标定获得；xi为滑窗内第i帧图像的状态，xk是第k帧图像的IMU状态，其中包含IMU预积分得到的PVQ数据，λi表示第i个路标点的逆深度；
将边缘化的先验信息、视觉残差和惯性残差视觉残差进行联合优化，通过最小化误差的思想方法，找到能使后验估计最大的最优状态向量，将代价函数定义为：
式中，表示第k帧关键帧的状态，χ表示一组地图点的状态集，Kj表示观察到地图点j的关键帧集其中，对于视觉重投影误差，使用Huber鲁棒核ρHub来减小误匹配带来的影响；
S6.对优化后的位姿进行闭环检测与重定位，方法是：判断相机是否回到之前到过的某个位置，当形成回环时进行回环校正和全局优化以消除误差；
S7.基于相机运动状态设置关键帧筛选阈值，包括以下步骤：
S71.获取姿态角变化值，方法是：首先将前端计算得到的帧间本质矩阵E用奇异值分解法分解成旋转矩阵R和位移向量T，假定两帧图像JP、JQ，P、Q为两帧图像间对应特征点位姿集合，P到Q的对于两帧图像间对应的特征点通过旋转矩阵R和位移向量T进行位姿转换：
Q＝RP+t (5-1)
其中R*RT＝I,det(R)＝1；
令俯仰角为α，航向角为β，横滚角为γ，则用方向余弦矩阵表示R得：
其中sμ＝sin(μ),cμ＝cos(μ)，由以上等价关系由旋转矩阵R推算出各个姿态角的值；
S72.根据位移向量的大小和姿态角变化值的大小设定不同的关键帧插入阈值通过分解本质矩阵得到帧间姿态变化；
S73.设置关键帧筛选阈值，方法是：计算当前帧与上一关键帧之间的位姿变化，判断当前帧与上一参考帧的变化程度，判断位移向量的大小变化，若位移大则不改变关键帧的筛选阈值大小；若位移小，再判断姿态角变化大小，若姿态角变化小则提高关键帧的筛选阈值，同时将关键帧插入最大时间间隔增大，反之则降低关键帧的筛选阈值；
S8.基于相机运动状态的双目-惯性融合的位姿估计。
2.根据权利要求1所述的基于相机运动状态的双目-惯性融合的位姿估计方法，其特征在于，S1具体方法是：
S11.提取当前帧图像的FAST角点作为关键点，用BRIEF特征点描述算法计算关键点的特征描述向量，提取方法是：
S111.选取图像中任一像素点p，获取其亮度lp；
S112.设置阈值T，T设为像素点p亮度的20％；
S113.取像素点p点为圆心的半径为3的圆上的16个像素点；
S114.比较像素点p和16个像素点的亮度，若出现连续N个像素点的亮度与像素点p点亮度差超过阈值T，则认为像素点p点是特征点，否则，不认为像素点p点是特征点，执行S111；N取9或12；
S115.依次对图像中的所有像素点重复S111-S114步骤，直至遍历完图像的所有像素点，得到多个特征点；
S12.采用RANSAC算法匹配相邻两帧图像的特征点对，方法是，包括以下步骤；
S121.将采集到的特征点作为样本，将其分为正确数据和异常数据；
S122.将正确数据作为内点；将异常数据作为外点；
S123.随机匹配一组子集作为内点，将模型拟合至该组子集上；形成拟合模型；
S124.利用拟合模型测试其他数据，形成共识集，利用共识集的所有成员对模型进行迭代获得最优模型参数值。
3.根据权利要求1所述的基于相机运动状态的双目-惯性融合的位姿估计方法，其特征在于，S8具体方法是：
S81.当IMU初始化完成且系统跟踪正常时，对IMU数据的计算得到帧间姿态变化，判断当前机体是否处在相对静止的状态；假定连续三帧间两两相邻的帧间变化时相同的，当连续三帧的帧间位姿变化都极小时，即认为当前机体处在相对静止状态；
S82.将对图像特征采取隔帧提取的方式，未提取特征的图像帧使用IMU预测的位姿数据。
4.一种电子设备，其特征在于，包括存储器和处理器，存储器存储有计算机程序，所述的处理器执行所述计算机程序时实现权利要求1-3任一项所述的基于相机运动状态的双目-惯性融合的位姿估计方法的步骤。
5.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1-3任一项所述的基于相机运动状态的双目-惯性融合的位姿估计方法。
CN202310003442.0A 2023-01-03 2023-01-03 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质 Active CN116205947B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310003442.0A CN116205947B (zh) 2023-01-03 2023-01-03 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310003442.0A CN116205947B (zh) 2023-01-03 2023-01-03 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质

Publications (2)

Publication Number Publication Date
CN116205947A CN116205947A (zh) 2023-06-02
CN116205947B true CN116205947B (zh) 2024-06-07

Family

ID=86508714

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310003442.0A Active CN116205947B (zh) 2023-01-03 2023-01-03 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质

Country Status (1)

Country Link
CN (1) CN116205947B (zh)

Families Citing this family (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116957958A (zh) * 2023-06-25 2023-10-27 东南大学 一种基于惯性先验校正图像灰度的vio前端改良方法
CN116883502B (zh) * 2023-09-05 2024-01-09 深圳市智绘科技有限公司 相机位姿和路标点位置的确定方法、装置、介质及设备
CN116958267B (zh) * 2023-09-21 2024-01-12 腾讯科技（深圳）有限公司 位姿处理方法、装置、电子设备及存储介质
CN117421384B (zh) * 2023-10-24 2024-09-17 哈尔滨理工大学 基于共视投影匹配的视觉惯性slam系统滑窗优化方法
CN117726678B (zh) * 2023-12-12 2024-09-20 中山大学·深圳 无人系统位姿估计方法、装置、计算机设备和存储介质
CN117470248B (zh) * 2023-12-27 2024-04-02 四川三江数智科技有限公司 一种移动机器人室内定位方法
CN117495698A (zh) * 2024-01-02 2024-02-02 福建卓航特种设备有限公司 飞行物识别方法、系统、智能终端及计算机可读存储介质
CN117928527B (zh) * 2024-02-01 2024-09-20 中国科学院空天信息创新研究院 一种基于行人运动特征优化的视觉惯性定位方法
CN117974809B (zh) * 2024-03-25 2024-06-18 季华实验室 机载面阵相机时空标定方法、装置、设备及存储介质

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN111882607A (zh) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 一种适用于增强现实应用的视觉惯导融合位姿估计方法
CN113012196A (zh) * 2021-03-05 2021-06-22 华南理工大学 一种基于双目相机与惯导传感器信息融合的定位方法
CN114485637A (zh) * 2022-01-18 2022-05-13 中国人民解放军63919部队 头戴式增强现实系统的视觉和惯性混合位姿跟踪方法
CN114964276A (zh) * 2022-05-26 2022-08-30 哈尔滨工业大学 一种融合惯导的动态视觉slam方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10621751B2 (en) * 2017-06-16 2020-04-14 Seiko Epson Corporation Information processing device and computer program
EP3451288A1 (en) * 2017-09-04 2019-03-06 Universität Zürich Visual-inertial odometry with an event camera
US10970425B2 (en) * 2017-12-26 2021-04-06 Seiko Epson Corporation Object detection and tracking
US10565728B2 (en) * 2018-06-01 2020-02-18 Tusimple, Inc. Smoothness constraint for camera pose estimation

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法
CN111882607A (zh) * 2020-07-14 2020-11-03 中国人民解放军军事科学院国防科技创新研究院 一种适用于增强现实应用的视觉惯导融合位姿估计方法
CN113012196A (zh) * 2021-03-05 2021-06-22 华南理工大学 一种基于双目相机与惯导传感器信息融合的定位方法
CN114485637A (zh) * 2022-01-18 2022-05-13 中国人民解放军63919部队 头戴式增强现实系统的视觉和惯性混合位姿跟踪方法
CN114964276A (zh) * 2022-05-26 2022-08-30 哈尔滨工业大学 一种融合惯导的动态视觉slam方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
An improved SLAM based on RK-VIF:Vision and inertial information fusion via Runge-Kutta method;Jia-shan cui et al.;《Defence Technology》;20211029;133-146 *
一种高精度紧耦合的双目VI-SLAM算法;陈世明等;《信息与控制》;20210820;403-409 *

Also Published As

Publication number Publication date
CN116205947A (zh) 2023-06-02

Similar Documents

Publication Publication Date Title
CN116205947B (zh) 基于相机运动状态的双目-惯性融合的位姿估计方法、电子设备及存储介质
CN111156984B (zh) 一种面向动态场景的单目视觉惯性slam方法
Milella et al. Stereo-based ego-motion estimation using pixel tracking and iterative closest point
Panahandeh et al. Vision-aided inertial navigation based on ground plane feature detection
US9071829B2 (en) Method and system for fusing data arising from image sensors and from motion or position sensors
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
US11138742B2 (en) Event-based feature tracking
US12073630B2 (en) Moving object tracking method and apparatus
CN109029433A (zh) 一种移动平台上基于视觉和惯导融合slam的标定外参和时序的方法
Li et al. A 4-point algorithm for relative pose estimation of a calibrated camera with a known relative rotation angle
Mourikis et al. A dual-layer estimator architecture for long-term localization
CN110296702A (zh) 视觉传感器与惯导紧耦合的位姿估计方法及装置
Michot et al. Bi-objective bundle adjustment with application to multi-sensor slam
JP6782903B2 (ja) 自己運動推定システム、自己運動推定システムの制御方法及びプログラム
CN114001733B (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN111609868A (zh) 一种基于改进光流法的视觉惯性里程计方法
CN114964276B (zh) 一种融合惯导的动态视觉slam方法
CN114485640A (zh) 基于点线特征的单目视觉惯性同步定位与建图方法及系统
Bloesch State estimation for legged robots-kinematics, inertial sensing, and computer vision
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
Wang et al. LF-VISLAM: A SLAM framework for large field-of-view cameras with negative imaging plane on mobile agents
Panahandeh et al. Vision-aided inertial navigation using planar terrain features
CN112731503A (zh) 一种基于前端紧耦合的位姿估计方法及系统
CN113920194B (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