CN113034538A - 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备 - Google Patents

一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备 Download PDF

Info

Publication number
CN113034538A
CN113034538A CN201911354390.1A CN201911354390A CN113034538A CN 113034538 A CN113034538 A CN 113034538A CN 201911354390 A CN201911354390 A CN 201911354390A CN 113034538 A CN113034538 A CN 113034538A
Authority
CN
China
Prior art keywords
frame
current
map
visual
constraint relation
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
CN201911354390.1A
Other languages
English (en)
Other versions
CN113034538B (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.)
Hangzhou Hikvision Digital Technology Co Ltd
Original Assignee
Hangzhou Hikvision Digital Technology Co Ltd
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 Hangzhou Hikvision Digital Technology Co Ltd filed Critical Hangzhou Hikvision Digital Technology Co Ltd
Priority to CN201911354390.1A priority Critical patent/CN113034538B/zh
Publication of CN113034538A publication Critical patent/CN113034538A/zh
Application granted granted Critical
Publication of CN113034538B publication Critical patent/CN113034538B/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/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
    • 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)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Multimedia (AREA)
  • Image Analysis (AREA)

Abstract

本申请实施例提供了一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备，由于在纹理稀疏的特殊场景下，当前帧的视觉约束关系相对较弱，上一帧对应的设备位姿会受惯性约束关系影响较大，本申请实施例中，除了当前帧中特征点与三维地图中地图点对应的第一视觉约束关系、当前帧与上一帧之间的惯性约束关系和上一帧之前的历史帧对上一帧的先验约束关系以外，还需利用上一帧中特征点与三维地图中地图点对应的第二视觉约束关系，以增大上一帧历史观测的实际影响，令其在当前帧优化时的波动较小，提高了纹理稀疏场景下的设备位姿跟踪精度。

Description

一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备
技术领域
本申请涉及计算机视觉技术领域，特别是涉及一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备。
背景技术
实时跟踪设备在未知环境中的位置与姿态问题，是增强现实、虚拟实现、导航与控制、移动机器人、无人驾驶、无人机等领域的核心问题之一。作为解决此类问题的常用方法，SLAM(Simultaneous Localization and Mapping，即时定位与地图构建)在机器人学和计算机视觉等相关领域被广泛研究。采用SLAM技术的设备能够在自身位置不确定的条件下，在完全未知的环境中创建地图，同时利用地图进行自主定位和导航。
最近，基于计算机视觉和惯性测量单元融合的定位算法，由于成本低、精度高、互补性强等特点收到越来越多的关注。这种利用设备本身的图像采集单元和IMU(InertialMeasurement Unit，惯性测量单元)采集的信息，进行实时设备位置和姿态跟踪的方法，统称为视觉惯性里程计，这种设备称为视觉惯导设备。
当前的视觉惯导设备的位姿跟踪方法中，主要包括通过图像采集设备实时采集图像数据、通过IMU实时采集惯性数据，并根据图像数据和惯性数据建立初始空间三维地图，在采集到新的图像数据后，对空间三维地图进行实时更新，利用当前图像特征点与三维地图点对应的视觉约束关系、图像帧之间特征匹配约束关系和图像帧间IMU的约束信息来确定当前时刻设备的位姿。
上述方法主要针对视觉惯性融合的一般性场景和系统框架，然而在纹理稀疏的特殊场景下，由于当前帧视觉约束较弱，当前帧的位姿跟踪会通过惯性约束引起上一帧位姿的较大波动，进而导致视觉惯导设备的位姿跟踪精度较差。
发明内容
本申请实施例的目的在于提供一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备，以提高纹理稀疏的特殊场景下视觉惯导设备的位姿跟踪精度。
具体技术方案如下：
第一方面，本申请实施例提供了一种视觉惯导设备的位姿跟踪方法，该方法包括：
获取图像采集单元采集的当前帧，以及惯性测量单元采集的当前惯性数据；
将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系；获取当前帧的上一帧中特征点与三维地图中地图点对应的第二视觉约束关系；根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系；获取上一帧之前的历史帧对上一帧的先验约束关系；
利用第一视觉约束关系、第二视觉约束关系、惯性约束关系及先验约束关系，确定视觉惯导设备的当前位姿。
可选的，在将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系；获取当前帧的上一帧中特征点与三维地图中地图点对应的第二视觉约束关系；根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系；获取上一帧之前的历史帧对上一帧的先验约束关系的步骤之前，该方法还包括：
统计当前帧中特征点的数目；
将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系；获取当前帧的上一帧中特征点与三维地图中地图点对应的第二视觉约束关系；根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系；获取上一帧之前的历史帧对上一帧的先验约束关系的步骤，包括：
若统计的数目小于预设阈值，则将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系；获取当前帧的上一帧中特征点与三维地图中地图点对应的第二视觉约束关系；根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系；获取上一帧之前的历史帧对上一帧的先验约束关系。
可选的，将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系的步骤，包括：
获取视觉惯导设备的当前跟踪状态；
根据当前跟踪状态，估计引导匹配窗口的大小；
将三维地图中的地图点投影至当前帧，得到地图点对应的投影像素点；
将当前帧中投影像素点周围符合上述大小的引导匹配窗口内的特征点与地图点进行匹配，并根据匹配结果，确定当前帧中特征点与地图点对应的第一视觉约束关系。
可选的，当前跟踪状态包括跟踪率及丢失时间，跟踪率为当前帧跟踪到三维地图中地图点的数目与上一帧跟踪到三维地图中地图点的数目的比值。
可选的，根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系的步骤，包括：
根据当前惯性数据，利用预积分策略，将当前惯性数据转换为当前帧与上一帧之间的惯性约束关系。
可选的，获取上一帧之前的历史帧对上一帧的先验约束关系的步骤，包括：
获取边缘化时刻上一帧的状态值、边缘化时刻上一帧位姿的信息矩阵以及上一帧的实时状态值；
根据边缘化时刻上一帧的状态值、边缘化时刻上一帧位姿的信息矩阵以及上一帧的实时状态值，确定上一帧之前的历史帧对上一帧的先验约束关系。
可选的，在利用第一视觉约束关系、第二视觉约束关系、惯性约束关系及先验约束关系，确定视觉惯导设备的当前位姿的步骤之后，该方法还包括：
根据当前位姿，将三维地图中与当前帧的特征点匹配的地图点重新投影至当前帧，计算当前帧中特征点与三维地图中匹配的地图点对应的第三视觉约束关系；
确定第三视觉约束关系小于或等于预设值的多个地图点；
从当前帧的历史帧中，确定跟踪到多个地图点的共视关键帧，并根据共视关键帧的所有特征点对应的地图点，构建局部三维地图；
将局部三维地图中的地图点投影至当前帧，确定当前帧中特征点与局部三维地图中地图点对应的第四视觉约束关系；获取当前帧的上一帧中特征点与局部三维地图中地图点对应的第五视觉约束关系；根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系；获取上一帧之前的历史帧对上一帧的先验约束关系；
利用第四视觉约束关系、第五视觉约束关系、惯性约束关系及先验约束关系，更新视觉惯导设备的当前位姿。
第二方面，本申请实施例提供了一种视觉惯导设备的位姿跟踪装置，该装置包括：
获取模块，用于获取图像采集单元采集的当前帧，以及惯性测量单元采集的当前惯性数据；
计算模块，用于将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系；获取当前帧的上一帧中特征点与三维地图中地图点对应的第二视觉约束关系；根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系；获取上一帧之前的历史帧对上一帧的先验约束关系；
预估模块，用于利用第一视觉约束关系、第二视觉约束关系、惯性约束关系及先验约束关系，确定视觉惯导设备的当前位姿。
可选的，该装置还包括：
统计模块，用于统计当前帧中特征点的数目；
计算模块，具体用于：
若统计的数目小于预设阈值，则将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系；获取当前帧的上一帧中特征点与三维地图中地图点对应的第二视觉约束关系；根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系；获取上一帧之前的历史帧对上一帧的先验约束关系。
可选的，计算模块在用于将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系时，具体用于：
获取视觉惯导设备的当前跟踪状态；
根据当前跟踪状态，估计引导匹配窗口的大小；
将三维地图中的地图点投影至当前帧，得到地图点对应的投影像素点；
将当前帧中投影像素点周围符合上述大小的引导匹配窗口内的特征点与地图点进行匹配，并根据匹配结果，确定当前帧中特征点与地图点对应的第一视觉约束关系。
可选的，当前跟踪状态包括跟踪率及丢失时间，跟踪率为当前帧跟踪到三维地图中地图点的数目与上一帧跟踪到三维地图中地图点的数目的比值。
可选的，计算模块在用于根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系时，具体用于：
根据当前惯性数据，利用预积分策略，将当前惯性数据转换为当前帧与上一帧之间的惯性约束关系。
可选的，计算模块在用于获取上一帧之前的历史帧对上一帧的先验约束关系时，具体用于：
获取边缘化时刻上一帧的状态值、边缘化时刻上一帧位姿的信息矩阵以及上一帧的实时状态值；
根据边缘化时刻上一帧的状态值、边缘化时刻上一帧位姿的信息矩阵以及上一帧的实时状态值，确定上一帧之前的历史帧对上一帧的先验约束关系。
可选的，该装置还包括：
投影模块，用于根据当前位姿，将三维地图中与当前帧的特征点匹配的地图点重新投影至当前帧，计算当前帧中特征点与三维地图中匹配的地图点对应的第三视觉约束关系；
确定模块，用于确定第三视觉约束关系小于或等于预设值的多个地图点；
构建模块，用于从当前帧的历史帧中，确定跟踪到多个地图点的共视关键帧，并根据共视关键帧的所有特征点对应的地图点，构建局部三维地图；
计算模块，还用于将局部三维地图中的地图点投影至当前帧，确定当前帧中特征点与局部三维地图中地图点对应的第四视觉约束关系；获取当前帧的上一帧中特征点与局部三维地图中地图点对应的第五视觉约束关系；根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系；获取上一帧之前的历史帧对上一帧的先验约束关系；
预估模块，还用于利用第四视觉约束关系、第五视觉约束关系、惯性约束关系及先验约束关系，更新视觉惯导设备的当前位姿。
第三方面，本申请实施例提供了一种视觉惯导设备，包括图像采集单元、惯性测量单元、处理器和存储器，其中，
图像采集单元，用于采集视频帧，并将采集到的当前帧发送至处理器；
惯性测量单元，用于采集惯性数据，并将采集到的当前惯性数据发送至处理器；
存储器存储有能够被处理器执行的机器可执行指令，机器可执行指令由处理器加载并执行，以实现本申请实施例第一方面所提供的方法。
第四方面，本申请实施例提供了一种机器可读存储介质，机器可读存储介质内存储有机器可执行指令，机器可执行指令被处理器执行时实现本申请实施例第一方面所提供的方法。
本申请实施例提供的一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备，获取图像采集单元采集的当前帧以及惯性测量单元采集的当前惯性数据，将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系，获取当前帧的上一帧中特征点与三维地图中地图点对应的第二视觉约束关系，根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系，获取上一帧之前的历史帧对上一帧的先验约束关系，利用第一视觉约束关系、第二视觉约束关系、惯性约束关系及先验约束关系，确定视觉惯导设备的当前位姿。
由于在纹理稀疏的特殊场景下，当前帧的视觉约束关系相对较弱，上一帧对应的设备位姿会受惯性约束关系影响较大，在本申请实施例中，除了当前帧中特征点与三维地图中地图点对应的第一视觉约束关系、当前帧与上一帧之间的惯性约束关系和上一帧之前的历史帧对上一帧的先验约束关系以外，还利用了上一帧中特征点与三维地图中地图点对应的第二视觉约束关系，以增大上一帧历史观测的实际影响，令其在当前帧优化时的波动较小，能够减小稀疏纹理下当前帧可能存在的错误的视觉约束关系对上一帧对应设备位姿的影响，从而提高了纹理稀疏的特殊场景下视觉惯导设备的位姿跟踪精度。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本申请一实施例的视觉惯导设备的位姿跟踪方法的流程示意图；
图2a为传统的视觉惯导设备的位姿跟踪方法中优化因子结构示意图；
图2b为本申请实施例的视觉惯导设备的位姿跟踪方法中优化因子结构示意图；
图3为传统的地图点匹配特征点的示意图；
图4为本申请另一实施例的视觉惯导设备的位姿跟踪方法的流程示意图；
图5为本申请再一实施例的视觉惯导设备的位姿跟踪方法的流程示意图；
图6为本申请实施例的视觉惯导设备位姿跟踪的流程示意图；
图7为本申请实施例的视觉惯导设备位姿跟踪流程中跟踪部分的流程示意图；
图8为本申请实施例的视觉惯导设备的位姿跟踪装置的结构示意图；
图9为本申请实施例的视觉惯导设备的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
为了提高纹理稀疏的特殊场景下视觉惯导设备的位姿跟踪精度，本申请实施例提供了一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备。下面，首先对本申请实施例所提供的位姿跟踪方法进行介绍。
本申请实施例所提供的位姿跟踪方法的执行主体为视觉惯导设备，该视觉惯导设备包括图像采集单元、IMU和处理器，本申请实施例所提供的位姿跟踪方法主要是由视觉惯导设备中的处理器执行实现。上述视觉惯导设备可以为移动机器人、手机、无人机、无人驾驶汽车等。实现本申请实施例所提供的位姿跟踪方法的方式可以为设置于视觉惯导设备的处理器中的软件、硬件电路和逻辑电路中的至少一种方式。
如图1所示，本申请实施例所提供的一种位姿跟踪方法，可以包括如下步骤。
S101，获取图像采集单元采集的当前帧，以及惯性测量单元采集的当前惯性数据。
S102，将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系。
S103，获取当前帧的上一帧中特征点与三维地图中地图点对应的第二视觉约束关系。
S104，根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系。
S105，获取上一帧之前的历史帧对上一帧的先验约束关系。
S106，利用第一视觉约束关系、第二视觉约束关系、惯性约束关系及先验约束关系，确定视觉惯导设备的当前位姿。
由于在纹理稀疏的特殊场景下，当前帧的视觉约束关系相对较弱，上一帧对应的设备位姿会受惯性约束关系影响较大，在本申请实施例中，除了当前帧中特征点与三维地图中地图点对应的第一视觉约束关系、当前帧与上一帧之间的惯性约束关系和上一帧之前的历史帧对上一帧的先验约束关系以外，还利用了上一帧中特征点与三维地图中地图点对应的第二视觉约束关系，以增大上一帧历史观测的实际影响，令其在当前帧优化时的波动较小，能够减小稀疏纹理下当前帧可能存在的错误的视觉约束关系对上一帧对应设备位姿的影响，从而提高了纹理稀疏的特殊场景下视觉惯导设备的位姿跟踪精度。
图1所示实施例中，图像采集单元可以是单目相机、双目相机等，这里不做具体限定。惯性测量单元包含陀螺仪和加速度计，所采集得到的惯性数据包括角速度数据和加速度数据。
对于纹理稀疏等特殊场景，当前帧的视觉约束关系相对较弱，上一帧对应的设备位姿会受惯性约束关系影响较大，因此，当前帧的视觉约束关系会通过惯性约束极大地影响到上一帧对应的设备位姿，为了应对这种情况，除了当前帧中特征点与三维地图中地图点对应的第一视觉约束关系和当前帧与上一帧之间的惯性约束关系和上一帧之前的历史帧对上一帧的先验约束关系以外，还需要获取上一帧中特征点与三维地图中地图点对应的第二视觉约束关系。
可选的，在执行S102、S103、S104和S105之前，本申请实施例所提供的方法还可以执行：统计当前帧中特征点的数目。相应的，若统计的数目小于预设阈值，则可以执行S102、S103、S104和S105。
在本申请实施例的一种实现方式中，可以在执行S102、S103、S104和S105之前，进行场景判断，判断图像采集单元采集的当前帧是否为纹理稀疏的特殊场景，视频帧的纹理稀疏程度主要由视频帧中特征点的数目决定，特征点越少则纹理越稀疏，如果当前帧的特征点数目小于预设阈值，则说明当前为纹理稀疏的场景。视频帧是否纹理稀疏也可以通过视频帧灰度、像素点的像素值等判断，这里不做赘述。
其中，第一视觉约束关系是通过将三维地图中的地图点投影至当前帧，利用地图点和当前帧的匹配关联关系计算得到；第二视觉约束关系在上一帧跟踪时，以相同的方式计算得到，因此在当前帧位姿跟踪时可以直接获取；惯性约束关系是通过对惯性测量单元采集的当前惯性数据进行预处理得到，这里的预处理主要是指预积分处理；先验约束关系是指由历史测量构建的对当前设备状态的约束关系，历史测量已经不在联合优化中，但历史测量可以转换为先验的形式保留在联合优化中，对尚在联合优化中的状态变量起到约束作用。
在得到第一视觉约束关系、第二视觉约束关系、惯性约束关系和先验约束关系后，利用这四种约束关系作为一个非线性优化问题，进行联合优化，利用优化方法，对需要优化的变量(如视觉惯导设备的位置和姿态)等统一优化，得到更为准确的当前帧的位姿估计。
如图2a所示，传统的视觉惯导设备的位姿跟踪方法主要考虑的是当前帧j的第一视觉约束关系V1、惯性约束关系I和先验约束关系P，而在纹理稀疏的场景下，上一帧对应的设备位姿会受惯性约束关系影响较大，故而当前帧的视觉约束关系会通过惯性约束极大地影响到上一帧对应的设备位姿。因此，在纹理稀疏的场景下，如图2b所示，还考虑了上一帧i的第二视觉约束关系V2，以增大上一帧视觉观测的实际影响，令其在当前帧优化时的波动较小，减小稀疏纹理下当前帧可能存在的错误的视觉约束关系对上一帧对应设备位姿的影响。
在图1所示实施例中，S104主要通过如下步骤实现：获取视觉惯导设备的当前跟踪状态；根据当前跟踪状态，估计引导匹配窗口的大小；将三维地图中的地图点投影至当前帧，得到地图点对应的投影像素点；将当前帧中投影像素点周围符合上述大小的引导匹配窗口内的特征点与地图点进行匹配，并根据匹配结果，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系。
在计算第一视觉约束关系时，首先需要根据视觉惯导设备的当前跟踪状态来确定引导匹配窗口的大小。引导匹配的意思是为了充分利用先验的运动信息，使用当前帧的预测位姿进行地图点的投影，并在投影点的一定邻域内(一般为几个像素的正方形窗口)寻找正确匹配特征点的方法。该方法不仅可以加速特征匹配，还可以更为精准地筛选出候选匹配点。
一般传统方法使用固定窗口大小，这对视觉惯导设备的位姿预测方法的精度要求较高，对于使用匀速模型或者预积分形式的位姿预测可能会出现正确匹配特征点在引导窗口外的情况，如图3所示。本申请实施例中，根据视觉惯导设备的当前跟踪状态来实时估计引导匹配窗口的大小，从而避免发生正确匹配特征点在引导窗口外的情况。
具体的，当前跟踪状态包括跟踪率及丢失时间，跟踪率为当前帧跟踪到三维地图中地图点的数目与上一帧跟踪到所述三维地图中地图点的数目的比值。
在估计出引导匹配窗口的大小后，将三维地图中的地图点投影至当前帧，得到地图点对应的投影像素点。假定当前帧经过去畸变处理，以小孔相机模型为例，相机内参为(fx,fy,cx,cy)，相机外参为Twc，三维地图的地图点位置为Pw，则投影像素点(u,v)的计算方式为：
Figure BDA0002335501280000111
将当前帧中投影像素点周围引导匹配窗口内的特征点与地图点进行匹配，匹配的过程主要是依次将引导匹配窗口内的特征点和地图点进行描述子比较，最终选择满足条件的特征点作为地图点的一个关联，这里所说的条件主要是指特征点和地图点的描述子距离小于一定的距离、特征点的描述子在金字塔某一层等条件，通过该过程，即可得到三维地图点和二维特征点的关联关系，即认为二维特征点是三维地图点在当前帧上的一个投影量测。
得到三维地图点和二维特征点的关联关系后，可以构建基于重投影误差的第一视觉约束关系。单个视觉量测的基于重投影误差的第一视觉约束关系如下：
Figure BDA0002335501280000112
其中，Q表示该视觉重投影误差的噪声协方差，
Figure BDA0002335501280000113
表示当前帧上提取的特征点像素位置，
Figure BDA0002335501280000114
表示三维地图点在当前帧上的投影像素点位置。
在图1所示实施例中，S106主要通过如下步骤实现：根据当前惯性数据，利用预积分策略，将当前惯性数据转换为当前帧与上一帧之间的惯性约束关系。
惯性约束关系是通过对当前惯性数据进行预积分得到，具体的，预积分策略所采用的公式如下：
Figure BDA0002335501280000115
其中，ΔRij、Δvij、Δpij分别表示由IMU量测预积分得到的关于上一帧i和当前帧j之间旋转、速度和位置的估计值，作为实际值的一个约束；
Figure BDA0002335501280000121
分别代表预积分量测ΔRij、Δvij、Δpij对于陀螺仪零漂
Figure BDA0002335501280000122
的一阶雅可比；∑表示由IMU量测预积分得到的协方差矩阵。
在图1所示实施例中，S107主要通过如下步骤实现：获取边缘化时刻上一帧的状态值、边缘化时刻上一帧位姿的信息矩阵以及上一帧的实时状态值；根据边缘化时刻上一帧的状态值、边缘化时刻上一帧位姿的信息矩阵以及上一帧的实时状态值，确定上一帧之前的历史帧对上一帧的先验约束关系。
先验是由历史测量构建的对当前设备状态的约束，历史测量已经不在联合优化中，但该测量同转换为先验的形式保留在联合优化中，对尚在联合优化中的状态变量起到约束作用。边缘化是为了减小图优化的计算量，将联合优化窗口内待优化变量的数目剔除但将其与尚留在窗口内的变量之间的约束信息保留。具体的，先验约束关系如下式：
Figure BDA0002335501280000123
其中，
Figure BDA0002335501280000124
都是边缘化时刻上一帧i的状态值，而
Figure BDA0002335501280000125
Figure BDA0002335501280000126
则是上一帧i的实时状态值，H为边缘化时刻上一帧位姿的信息矩阵。
因此，可以得到视觉惯导设备的联合优化函数，如下式：
argmin(ePrior+eIMU+∑eVis j+∑eVis i) (5)
基于上述联合优化函数，进行优化计算，即可得到视觉惯导设备的当前位姿。
基于图1所示实施例，本申请实施例还提供了一种位姿跟踪方法，如图4所示，可以包括如下步骤。
S401，获取图像采集单元采集的当前帧，以及惯性测量单元采集的当前惯性数据。
S402，将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系。
S403，获取当前帧的上一帧中特征点与三维地图中地图点对应的第二视觉约束关系。
S404，根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系。
S405，获取上一帧之前的历史帧对上一帧的先验约束关系。
S406，利用第一视觉约束关系、第二视觉约束关系、惯性约束关系及先验约束关系，确定视觉惯导设备的当前位姿。
S407，根据当前位姿，将三维地图中与当前帧的特征点匹配的地图点重新投影至当前帧，计算当前帧中特征点与三维地图中匹配的地图点对应的第三视觉约束关系。
在预测出视觉惯导设备的当前位姿之后，基于当前位姿重新进行三维地图点的投影，本次投影只需要对上次匹配的地图点进行投影，由于位姿有变化，则投影后计算得到的第三视觉约束关系也有不同。
S408，确定第三视觉约束关系小于或等于预设值的多个地图点。
得到的第三视觉约束关系有大有小，第三视觉约束关系的值过大，则说明重投影误差较大，应删除这些误差较大的地图点，例如，上一次匹配时匹配到200个地图点，而这200个地图点中有50个进行重投影得到的第三视觉约束关系值过大，则滤除掉这50个地图点，保留剩下的第三视觉约束关系小于或等于预设值的150个地图点。
S409，从当前帧的历史帧中，确定跟踪到多个地图点的共视关键帧，并根据共视关键帧的所有特征点对应的地图点，构建局部三维地图。
当前帧的历史帧中，会有多个历史帧跟踪到这150个地图点，从中找到能够跟踪到这150个地图点的共视关键帧，然后根据这些共视关键帧的所有特征点对应的地图点可以构建出一个局部三维地图。
S410，将局部三维地图中的地图点投影至当前帧，确定当前帧中特征点与局部三维地图中地图点对应的第四视觉约束关系。
S411，获取当前帧的上一帧中特征点与局部三维地图中地图点对应的第五视觉约束关系。
S412，根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系。
S413，获取上一帧之前的历史帧对上一帧的先验约束关系。
依据构建出的局部三维地图，重新进行当前帧跟踪的步骤，得到当前帧中特征点与局部三维地图中地图点对应的第四视觉约束关系、当前帧的上一帧中特征点与局部三维地图中地图点对应的第五视觉约束关系、当前帧与上一帧之间的惯性约束关系和上一帧之前的历史帧对上一帧的先验约束关系。
S414，利用第四视觉约束关系、第五视觉约束关系、惯性约束关系及先验约束关系，更新视觉惯导设备的当前位姿。
由于进行过一次当前帧对应的位姿跟踪，所跟踪的地图点会认为是较为准确的地图点，因此构建出的局部三维地图被认为是更为精准的三维地图，因此在构建出的局部三维地图的基础上，再进行一次当前帧对应的位姿跟踪，所得到的当前位姿更为准确，具有较高的鲁棒性。当然，为了更进一步的提高鲁棒性，还可以再一次进行局部三维地图的更新、当前帧对应的位姿跟踪，具体步骤与图4所示实施例相近，即为多次迭代，这里不再赘述。
为了便于理解，下面对本申请实施例提供的位姿跟踪方法进行说明，如图5所示。在采集到视觉图像数据和IMU数据后，对视觉图像数据进行特征点提取和描述，并根据特征点判断是否纹理稀疏，如果是，则构建当前帧视觉约束和上一帧视觉约束，根据当前帧视觉约束得到当前帧视觉因子(即上述实施例中的第一视觉约束关系)，根据上一帧视觉约束得到上一帧视觉因子(即上述实施例中的第二视觉约束关系)，如果否，则只构建当前帧视觉约束，根据当前帧视觉约束得到当前帧视觉因子(即上述实施例中的第一视觉约束关系)；并且不论是否纹理稀疏，都对IMU数据进行预积分得到IMU因子(即上述实施例中的惯性约束关系)、获取先验因子(即上述实施例中的先验约束关系)。
利用当前帧视觉因子、上一帧视觉因子、IMU因子和先验因子，进行视觉IMU联合优化，得到视觉惯导设备的当前位姿，基于当前位姿进行局部三维地图优化，返回纹理稀疏判断的步骤，基于优化的局部三维地图重新构建当前帧视觉约束，重新进行一次当前帧跟踪，更新视觉惯导设备的当前位姿。
概括来讲，视觉惯导设备位姿跟踪的流程主要包括数据预处理、跟踪、局部建图、全局建图几个步骤，如图6所示。本申请的跟踪部分的流程如图7所示，主要包括如下步骤：
第一步：引导窗口的估计。
首先需要根据视觉惯导设备的当前跟踪状态来确定引导匹配窗口的大小。根据视觉惯导设备的当前跟踪状态来实时估计引导匹配窗口的大小，避免发生正确匹配特征点在引导窗口外的情况。
第二步：投影匹配。
将三维地图中的地图点投影至当前帧，得到地图点对应的投影像素点，将当前帧中投影像素点周围引导匹配窗口内的特征点与地图点进行匹配，匹配的过程主要是依次将引导匹配窗口内的特征点与地图点进行描述子比较，最终选择满足条件的特征点作为三维地图中地图点的一个关联，通过该过程，即可得到三维地图点和二维特征点的关联关系，即认为二维特征点是三维地图点在当前帧上的一个投影量测。
第三步：滑窗优化。
得到三维地图点和二维特征点的关联关系后，可以构建基于重投影误差的视觉约束因子。此外，由预积分策略可以得到IMU量测构建的约束因子，还有上一帧之前的历史帧边缘化留下的对上一帧对应设备位姿的先验约束因子。这些约束因子的具体确定过程见上述实施例，这里不再赘述。
再添加上一帧视觉因子，得到联合优化函数，如公式(5)所示。基于该联合优化函数可以估计出视觉惯导设备的当前位姿。
第四步：共视图维护。
在经过第三步的跟踪优化后，对第二步得到的匹配地图点进行外点(基于当前位姿重新进行三维地图点的投影，得到的视觉约束因子大于预设值的地图点)滤除，再根据保留的地图点找到相应的共视关键帧，并构建局部三维地图，进行下一次的当前帧跟踪局部三维地图的步骤。
对于数据预处理(主要包括对视觉图像数据的特征点进行提取和描述、对IMU数据的预积分)、局部建图、全局建图几个部分均与传统的视觉图像跟踪方法相同，这里不再赘述。
相应于上述方法实施例，本申请实施例提供了一种视觉惯导设备的位姿跟踪装置，如图8所示，该装置可以包括：
获取模块810，用于获取图像采集单元采集的当前帧，以及惯性测量单元采集的当前惯性数据；
计算模块820，用于将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系；获取当前帧的上一帧中特征点与三维地图中地图点对应的第二视觉约束关系；根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系；获取上一帧之前的历史帧对上一帧的先验约束关系；
预估模块830，用于利用第一视觉约束关系、第二视觉约束关系、惯性约束关系及先验约束关系，确定视觉惯导设备的当前位姿。
可选的，该装置还可以包括：
统计模块，用于统计当前帧中特征点的数目；
计算模块820，具体可以用于：若统计的数目小于预设阈值，则将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系；获取当前帧的上一帧中特征点与三维地图中地图点对应的第二视觉约束关系；根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系；获取上一帧之前的历史帧对上一帧的先验约束关系。
可选的，计算模块820在用于将三维地图中的地图点投影至当前帧，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系时，具体可以用于：
获取视觉惯导设备的当前跟踪状态；
根据当前跟踪状态，估计引导匹配窗口的大小；
将三维地图中的地图点投影至当前帧，得到地图点对应的投影像素点；
对投影像素点和当前帧中的特征点进行匹配，并根据匹配结果，确定当前帧中特征点与三维地图中地图点对应的第一视觉约束关系。
可选的，当前跟踪状态包括跟踪率及丢失时间，跟踪率为当前帧跟踪到三维地图中地图点的数目与上一帧跟踪到三维地图中地图点的数目的比值。
可选的，计算模块820在用于根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系时，具体可以用于：
根据当前惯性数据，利用预积分策略，将当前惯性数据转换为当前帧与上一帧之间的惯性约束关系。
可选的，计算模块820在用于获取上一帧之前的历史帧对上一帧的先验约束关系时，具体可以用于：
获取边缘化时刻上一帧的状态值、边缘化时刻上一帧位姿的信息矩阵以及上一帧的实时状态值；
根据边缘化时刻上一帧的状态值、边缘化时刻上一帧位姿的信息矩阵以及上一帧的实时状态值，确定上一帧之前的历史帧对上一帧的先验约束关系。
可选的，该装置还可以包括：
投影模块，用于根据当前位姿，将三维地图中与当前帧的特征点匹配的地图点重新投影至当前帧，计算当前帧中特征点与三维地图中匹配的地图点对应的第三视觉约束关系；
确定模块，用于确定第三视觉约束关系小于或等于预设值的多个地图点；
构建模块，用于从当前帧的历史帧中，确定跟踪到多个地图点的共视关键帧，并根据共视关键帧的所有特征点对应的地图点，构建局部三维地图；
计算模块820，还可以用于将局部三维地图中的地图点投影至当前帧，确定当前帧中特征点与局部三维地图中地图点对应的第四视觉约束关系；获取当前帧的上一帧中特征点与局部三维地图中地图点对应的第五视觉约束关系；根据当前惯性数据，确定当前帧与上一帧之间的惯性约束关系；获取上一帧之前的历史帧对上一帧的先验约束关系；
预估模块830，还可以用于利用第四视觉约束关系、第五视觉约束关系、惯性约束关系及先验约束关系，更新视觉惯导设备的当前位姿。
由于在纹理稀疏的特殊场景下，当前帧的视觉约束关系相对较弱，上一帧对应的设备位姿会受惯性约束关系影响较大，在本申请实施例中，除了当前帧中特征点与三维地图中地图点对应的第一视觉约束关系、当前帧与上一帧之间的惯性约束关系和上一帧之前的历史帧对上一帧的先验约束关系以外，还利用了上一帧中特征点与三维地图中地图点对应的第二视觉约束关系，以增大上一帧历史观测的实际影响，令其在当前帧优化时的波动较小，能够减小稀疏纹理下当前帧可能存在的错误的视觉约束关系对上一帧对应设备位姿的影响，从而提高了纹理稀疏的特殊场景下视觉惯导设备的位姿跟踪精度。
本申请实施例还提供了一种视觉惯导设备，如图9所示，包括图像采集单元901、惯性测量单元902、处理器903和存储器904，其中，
图像采集单元901，用于采集视频帧，并将采集到的当前帧发送至处理器903；
惯性测量单元902，用于采集惯性数据，并将采集到的当前惯性数据发送至处理器903；
存储器904存储有能够被处理器903执行的机器可执行指令，机器可执行指令由处理器903加载并执行，以实现本申请实施例所提供的视觉惯导设备的位姿跟踪方法。
上述存储器可以包括RAM(Random Access Memory，随机存取存储器)，也可以包括NVM(Non-volatile Memory，非易失性存储器)，例如至少一个磁盘存储器。可选的，存储器还可以是至少一个位于远离前述处理器的存储装置。
上述处理器可以是通用处理器，包括CPU(Central Processing Unit，中央处理器)、NP(Network Processor，网络处理器)等；还可以是DSP(Digital Signal Processor，数字信号处理器)、ASIC(Application Specific Integrated Circuit，专用集成电路)、FPGA(Field-Programmable Gate Array，现场可编程门阵列)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
图像采集单元901、惯性测量单元902、处理器903和存储器904之间可以通过有线连接或者无线连接的方式进行数据传输，并且视觉惯导设备与其他设备之间可以通过有线通信接口或者无线通信接口进行通信。图9所示的仅为通过总线进行数据传输的示例，不作为具体连接方式的限定。图像采集单元可以是图像传感器，例如双目相机、TOF(Time ofFlight，时间飞行)相机，激光扫描仪等。
本申请实施例中，处理器通过读取存储器中存储的机器可执行指令，并通过加载和执行机器可执行指令，能够实现：由于在纹理稀疏的特殊场景下，当前帧的视觉约束关系相对较弱，上一帧对应的设备位姿会受惯性约束关系影响较大，在本申请实施例中，除了当前帧中特征点与三维地图中地图点对应的第一视觉约束关系、当前帧与上一帧之间的惯性约束关系和上一帧之前的历史帧对上一帧的先验约束关系以外，还利用了上一帧中特征点与三维地图中地图点对应的第二视觉约束关系，以增大上一帧历史观测的实际影响，令其在当前帧优化时的波动较小，能够减小稀疏纹理下当前帧可能存在的错误的视觉约束关系对上一帧对应设备位姿的影响，从而提高了纹理稀疏的特殊场景下视觉惯导设备的位姿跟踪精度。
另外，本申请实施例还提供了一种机器可读存储介质，机器可读存储介质内存储有机器可执行指令，机器可执行指令被处理器执行时实现本申请实施例所提供的视觉惯导设备的位姿跟踪方法。
本申请实施例中，机器可读存储介质存储有在运行时执行本申请实施例所提供的视觉惯导设备的位姿跟踪方法的机器可执行指令，因此能够实现：由于在纹理稀疏的特殊场景下，当前帧的视觉约束关系相对较弱，上一帧对应的设备位姿会受惯性约束关系影响较大，在本申请实施例中，除了当前帧中特征点与三维地图中地图点对应的第一视觉约束关系、当前帧与上一帧之间的惯性约束关系和上一帧之前的历史帧对上一帧的先验约束关系以外，还利用了上一帧中特征点与三维地图中地图点对应的第二视觉约束关系，以增大上一帧历史观测的实际影响，令其在当前帧优化时的波动较小，能够减小稀疏纹理下当前帧可能存在的错误的视觉约束关系对上一帧对应设备位姿的影响，从而提高了纹理稀疏的特殊场景下视觉惯导设备的位姿跟踪精度。
上述方法和装置可以进一步应用于无人驾驶设备，无人驾驶设备包含上述视觉惯导设备，视觉惯导设备包含处理器，该处理器可以基于更新的视觉惯导设备的当前位姿，确定无人驾驶设备的位姿，并基于无人驾驶设备的位姿和上述三维地图实现导航。
无人驾驶设备还可以包括行进部件，用于根据处理器的导航前进，实现自动驾驶。
这样，由于上述方案能够在纹理稀疏的场景下，仍然能够精准确定位姿，进而实现更精准的SLAM(Simultaneous Localization and Mapping，即时定位与地图构建)技术，从而使得无人驾驶设备(例如，无人送货机、无人汽车、机器人等)的自动驾驶更为安全可靠。
对于视觉惯导设备及机器可读存储介质实施例而言，由于其涉及的方法内容基本相似于前述的方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
本说明书中的各个实施例均采用相关的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于装置、视觉惯导设备及机器可读存储介质实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
以上所述仅为本申请的较佳实施例而已，并非用于限定本申请的保护范围。凡在本申请的精神和原则之内所作的任何修改、等同替换、改进等，均包含在本申请的保护范围内。

Claims (15)

1.一种视觉惯导设备的位姿跟踪方法，其特征在于，所述方法包括：
获取图像采集单元采集的当前帧，以及惯性测量单元采集的当前惯性数据；
将三维地图中的地图点投影至所述当前帧，确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系；获取所述当前帧的上一帧中特征点与所述三维地图中地图点对应的第二视觉约束关系；根据所述当前惯性数据，确定所述当前帧与所述上一帧之间的惯性约束关系；获取所述上一帧之前的历史帧对所述上一帧的先验约束关系；
利用所述第一视觉约束关系、所述第二视觉约束关系、所述惯性约束关系及所述先验约束关系，确定视觉惯导设备的当前位姿。
2.根据权利要求1所述的方法，其特征在于，在所述将三维地图中的地图点投影至所述当前帧，确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系；获取所述当前帧的上一帧中特征点与所述三维地图中地图点对应的第二视觉约束关系；根据所述当前惯性数据，确定所述当前帧与所述上一帧之间的惯性约束关系；获取所述上一帧之前的历史帧对所述上一帧的先验约束关系之前，所述方法还包括：
统计所述当前帧中特征点的数目；
所述将三维地图中的地图点投影至所述当前帧，确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系；获取所述当前帧的上一帧中特征点与所述三维地图中地图点对应的第二视觉约束关系；根据所述当前惯性数据，确定所述当前帧与所述上一帧之间的惯性约束关系；获取所述上一帧之前的历史帧对所述上一帧的先验约束关系，包括：
若所述数目小于预设阈值，则将三维地图中的地图点投影至所述当前帧，确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系；获取所述当前帧的上一帧中特征点与所述三维地图中地图点对应的第二视觉约束关系；根据所述当前惯性数据，确定所述当前帧与所述上一帧之间的惯性约束关系；获取所述上一帧之前的历史帧对所述上一帧的先验约束关系。
3.根据权利要求1所述的方法，其特征在于，所述将三维地图中的地图点投影至所述当前帧，确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系，包括：
获取所述视觉惯导设备的当前跟踪状态；
根据所述当前跟踪状态，估计引导匹配窗口的大小；
将三维地图中的地图点投影至所述当前帧，得到所述地图点对应的投影像素点；
将所述当前帧中所述投影像素点周围符合所述大小的引导匹配窗口内的特征点与所述地图点进行匹配，并根据匹配结果，确定所述当前帧中特征点与所述地图点对应的第一视觉约束关系。
4.根据权利要求3所述的方法，其特征在于，所述当前跟踪状态包括跟踪率及丢失时间，所述跟踪率为所述当前帧跟踪到所述三维地图中地图点的数目与所述上一帧跟踪到所述三维地图中地图点的数目的比值。
5.根据权利要求1所述的方法，其特征在于，所述根据所述当前惯性数据，确定所述当前帧与所述上一帧之间的惯性约束关系，包括：
根据所述当前惯性数据，利用预积分策略，将所述当前惯性数据转换为所述当前帧与所述上一帧之间的惯性约束关系。
6.根据权利要求1所述的方法，其特征在于，所述获取所述上一帧之前的历史帧对所述上一帧的先验约束关系，包括：
获取边缘化时刻所述上一帧的状态值、所述边缘化时刻上一帧位姿的信息矩阵以及所述上一帧的实时状态值；
根据所述边缘化时刻所述上一帧的状态值、所述边缘化时刻上一帧位姿的信息矩阵以及所述上一帧的实时状态值，确定所述上一帧之前的历史帧对所述上一帧的先验约束关系。
7.根据权利要求1所述的方法，其特征在于，在所述利用所述第一视觉约束关系、所述第二视觉约束关系、所述惯性约束关系及所述先验约束关系，确定视觉惯导设备的当前位姿之后，所述方法还包括：
根据所述当前位姿，将所述三维地图中与所述当前帧的特征点匹配的地图点重新投影至所述当前帧，计算所述当前帧中特征点与所述三维地图中所述匹配的地图点对应的第三视觉约束关系；
确定第三视觉约束关系小于或等于预设值的多个地图点；
从所述当前帧的历史帧中，确定跟踪到所述多个地图点的共视关键帧，并根据所述共视关键帧的所有特征点对应的地图点，构建局部三维地图；
将所述局部三维地图中的地图点投影至所述当前帧，确定所述当前帧中特征点与所述局部三维地图中地图点对应的第四视觉约束关系；获取所述当前帧的上一帧中特征点与所述局部三维地图中地图点对应的第五视觉约束关系；根据所述当前惯性数据，确定所述当前帧与所述上一帧之间的惯性约束关系；获取所述上一帧之前的历史帧对所述上一帧的先验约束关系；
利用所述第四视觉约束关系、所述第五视觉约束关系、所述惯性约束关系及所述先验约束关系，更新所述视觉惯导设备的当前位姿。
8.一种视觉惯导设备的位姿跟踪装置，其特征在于，所述装置包括：
获取模块，用于获取图像采集单元采集的当前帧，以及惯性测量单元采集的当前惯性数据；
计算模块，用于将三维地图中的地图点投影至所述当前帧，确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系；获取所述当前帧的上一帧中特征点与所述三维地图中地图点对应的第二视觉约束关系；根据所述当前惯性数据，确定所述当前帧与所述上一帧之间的惯性约束关系；获取所述上一帧之前的历史帧对所述上一帧的先验约束关系；
预估模块，用于利用所述第一视觉约束关系、所述第二视觉约束关系、所述惯性约束关系及所述先验约束关系，确定视觉惯导设备的当前位姿。
9.根据权利要求8所述的装置，其特征在于，所述装置还包括：
统计模块，用于统计所述当前帧中特征点的数目；
所述计算模块，具体用于：
若所述数目小于预设阈值，则将三维地图中的地图点投影至所述当前帧，确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系；获取所述当前帧的上一帧中特征点与所述三维地图中地图点对应的第二视觉约束关系；根据所述当前惯性数据，确定所述当前帧与所述上一帧之间的惯性约束关系；获取所述上一帧之前的历史帧对所述上一帧的先验约束关系。
10.根据权利要求8所述的装置，其特征在于，所述计算模块，在用于所述将三维地图中的地图点投影至所述当前帧，确定所述当前帧中特征点与所述三维地图中地图点对应的第一视觉约束关系时，具体用于：
获取所述视觉惯导设备的当前跟踪状态；
根据所述当前跟踪状态，估计引导匹配窗口的大小；
将三维地图中的地图点投影至所述当前帧，得到所述地图点对应的投影像素点；
将所述当前帧中所述投影像素点周围符合所述大小的引导匹配窗口内的特征点与所述地图点进行匹配，并根据匹配结果，确定所述当前帧中特征点与所述地图点对应的第一视觉约束关系。
11.根据权利要求10所述的装置，其特征在于，所述当前跟踪状态包括跟踪率及丢失时间，所述跟踪率为所述当前帧跟踪到所述三维地图中地图点的数目与所述上一帧跟踪到所述三维地图中地图点的数目的比值。
12.根据权利要求8所述的装置，其特征在于，所述计算模块，在用于所述根据所述当前惯性数据，确定所述当前帧与所述上一帧之间的惯性约束关系时，具体用于：
根据所述当前惯性数据，利用预积分策略，将所述当前惯性数据转换为所述当前帧与所述上一帧之间的惯性约束关系。
13.根据权利要求8所述的装置，其特征在于，所述计算模块，在用于所述获取所述上一帧之前的历史帧对所述上一帧的先验约束关系时，具体用于：
获取边缘化时刻所述上一帧的状态值、所述边缘化时刻上一帧位姿的信息矩阵以及所述上一帧的实时状态值；
根据所述边缘化时刻所述上一帧的状态值、所述边缘化时刻上一帧位姿的信息矩阵以及所述上一帧的实时状态值，确定所述上一帧之前的历史帧对所述上一帧的先验约束关系。
14.根据权利要求8所述的装置，其特征在于，所述装置还包括：
投影模块，用于根据所述当前位姿，将所述三维地图中与所述当前帧的特征点匹配的地图点重新投影至所述当前帧，计算所述当前帧中特征点与所述三维地图中所述匹配的地图点对应的第三视觉约束关系；
确定模块，用于确定第三视觉约束关系小于或等于预设值的多个地图点；
构建模块，用于从所述当前帧的历史帧中，确定跟踪到所述多个地图点的共视关键帧，并根据所述共视关键帧的所有特征点对应的地图点，构建局部三维地图；
所述计算模块，还用于将所述局部三维地图中的地图点投影至所述当前帧，确定所述当前帧中特征点与所述局部三维地图中地图点对应的第四视觉约束关系；获取所述当前帧的上一帧中特征点与所述局部三维地图中地图点对应的第五视觉约束关系；根据所述当前惯性数据，确定所述当前帧与所述上一帧之间的惯性约束关系；获取所述上一帧之前的历史帧对所述上一帧的先验约束关系；
所述预估模块，还用于利用所述第四视觉约束关系、所述第五视觉约束关系、所述惯性约束关系及所述先验约束关系，更新所述视觉惯导设备的当前位姿。
15.一种视觉惯导设备，其特征在于，包括图像采集单元、惯性测量单元、处理器和存储器，其中，
所述图像采集单元，用于采集视频帧，并将采集到的当前帧发送至所述处理器；
所述惯性测量单元，用于采集惯性数据，并将采集到的当前惯性数据发送至所述处理器；
所述存储器存储有能够被所述处理器执行的机器可执行指令，所述机器可执行指令由所述处理器加载并执行，以实现权利要求1-7任一项所述的方法。
CN201911354390.1A 2019-12-25 2019-12-25 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备 Active CN113034538B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911354390.1A CN113034538B (zh) 2019-12-25 2019-12-25 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911354390.1A CN113034538B (zh) 2019-12-25 2019-12-25 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备

Publications (2)

Publication Number Publication Date
CN113034538A true CN113034538A (zh) 2021-06-25
CN113034538B CN113034538B (zh) 2023-09-05

Family

ID=76458050

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911354390.1A Active CN113034538B (zh) 2019-12-25 2019-12-25 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备

Country Status (1)

Country Link
CN (1) CN113034538B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114111769A (zh) * 2021-11-15 2022-03-01 杭州海康威视数字技术股份有限公司 一种视觉惯性定位方法、装置及自动驾驶装置

Citations (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103559711A (zh) * 2013-11-05 2014-02-05 余洪山 基于三维视觉系统图像特征和三维信息的运动估计方法
US20140333741A1 (en) * 2013-05-08 2014-11-13 Regents Of The University Of Minnesota Constrained key frame localization and mapping for vision-aided inertial navigation
CN108489482A (zh) * 2018-02-13 2018-09-04 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及系统
CN108827315A (zh) * 2018-08-17 2018-11-16 华南理工大学 基于流形预积分的视觉惯性里程计位姿估计方法及装置
CN109166149A (zh) * 2018-08-13 2019-01-08 武汉大学 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN109345588A (zh) * 2018-09-20 2019-02-15 浙江工业大学 一种基于Tag的六自由度姿态估计方法
CN109816696A (zh) * 2019-02-01 2019-05-28 西安全志科技有限公司 一种机器人定位与建图方法、计算机装置及计算机可读存储介质
US20190178646A1 (en) * 2017-12-08 2019-06-13 Regents Of The University Of Minnesota Square root inverse schmidt-kalman filters for vision-aided inertial navigation and mapping
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置
CN110108258A (zh) * 2019-04-09 2019-08-09 南京航空航天大学 一种单目视觉里程计定位方法
US20190251696A1 (en) * 2018-02-12 2019-08-15 Samsung Electronics Co., Ltd. Device and method with pose estimator
CN110125928A (zh) * 2019-03-27 2019-08-16 浙江工业大学 一种基于前后帧进行特征匹配的双目惯导slam系统
CN110246147A (zh) * 2019-05-14 2019-09-17 中国科学院深圳先进技术研究院 视觉惯性里程计方法、视觉惯性里程计装置及移动设备
CN110296702A (zh) * 2019-07-30 2019-10-01 清华大学 视觉传感器与惯导紧耦合的位姿估计方法及装置
CN110322500A (zh) * 2019-06-28 2019-10-11 Oppo广东移动通信有限公司 即时定位与地图构建的优化方法及装置、介质和电子设备
CN110335316A (zh) * 2019-06-28 2019-10-15 Oppo广东移动通信有限公司 基于深度信息的位姿确定方法、装置、介质与电子设备
CN110349213A (zh) * 2019-06-28 2019-10-18 Oppo广东移动通信有限公司 基于深度信息的位姿确定方法、装置、介质与电子设备
CN110349212A (zh) * 2019-06-28 2019-10-18 Oppo广东移动通信有限公司 即时定位与地图构建的优化方法及装置、介质和电子设备
CN110375738A (zh) * 2019-06-21 2019-10-25 西安电子科技大学 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN110455301A (zh) * 2019-08-01 2019-11-15 河北工业大学 一种基于惯性测量单元的动态场景slam方法

Patent Citations (21)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20140333741A1 (en) * 2013-05-08 2014-11-13 Regents Of The University Of Minnesota Constrained key frame localization and mapping for vision-aided inertial navigation
CN103559711A (zh) * 2013-11-05 2014-02-05 余洪山 基于三维视觉系统图像特征和三维信息的运动估计方法
US20190178646A1 (en) * 2017-12-08 2019-06-13 Regents Of The University Of Minnesota Square root inverse schmidt-kalman filters for vision-aided inertial navigation and mapping
US20190251696A1 (en) * 2018-02-12 2019-08-15 Samsung Electronics Co., Ltd. Device and method with pose estimator
CN108489482A (zh) * 2018-02-13 2018-09-04 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及系统
WO2019157925A1 (zh) * 2018-02-13 2019-08-22 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及系统
CN109166149A (zh) * 2018-08-13 2019-01-08 武汉大学 一种融合双目相机与imu的定位与三维线框结构重建方法与系统
CN108827315A (zh) * 2018-08-17 2018-11-16 华南理工大学 基于流形预积分的视觉惯性里程计位姿估计方法及装置
CN109345588A (zh) * 2018-09-20 2019-02-15 浙江工业大学 一种基于Tag的六自由度姿态估计方法
CN109816696A (zh) * 2019-02-01 2019-05-28 西安全志科技有限公司 一种机器人定位与建图方法、计算机装置及计算机可读存储介质
CN110125928A (zh) * 2019-03-27 2019-08-16 浙江工业大学 一种基于前后帧进行特征匹配的双目惯导slam系统
CN110044354A (zh) * 2019-03-28 2019-07-23 东南大学 一种双目视觉室内定位与建图方法及装置
CN110108258A (zh) * 2019-04-09 2019-08-09 南京航空航天大学 一种单目视觉里程计定位方法
CN110246147A (zh) * 2019-05-14 2019-09-17 中国科学院深圳先进技术研究院 视觉惯性里程计方法、视觉惯性里程计装置及移动设备
CN110375738A (zh) * 2019-06-21 2019-10-25 西安电子科技大学 一种融合惯性测量单元的单目同步定位与建图位姿解算方法
CN110322500A (zh) * 2019-06-28 2019-10-11 Oppo广东移动通信有限公司 即时定位与地图构建的优化方法及装置、介质和电子设备
CN110335316A (zh) * 2019-06-28 2019-10-15 Oppo广东移动通信有限公司 基于深度信息的位姿确定方法、装置、介质与电子设备
CN110349213A (zh) * 2019-06-28 2019-10-18 Oppo广东移动通信有限公司 基于深度信息的位姿确定方法、装置、介质与电子设备
CN110349212A (zh) * 2019-06-28 2019-10-18 Oppo广东移动通信有限公司 即时定位与地图构建的优化方法及装置、介质和电子设备
CN110296702A (zh) * 2019-07-30 2019-10-01 清华大学 视觉传感器与惯导紧耦合的位姿估计方法及装置
CN110455301A (zh) * 2019-08-01 2019-11-15 河北工业大学 一种基于惯性测量单元的动态场景slam方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
张国良;林志林;姚二亮;徐慧;: "考虑多位姿估计约束的双目视觉里程计", 控制与决策, no. 06 *
林辉灿;吕强;王国胜;卫恒;梁冰;: "鲁棒的非线性优化的立体视觉-惯导SLAM", 机器人, no. 06 *
赵彤;刘洁瑜;李卓;: "三阶段局部双目光束法平差视觉里程计", 光电工程, no. 11 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114111769A (zh) * 2021-11-15 2022-03-01 杭州海康威视数字技术股份有限公司 一种视觉惯性定位方法、装置及自动驾驶装置

Also Published As

Publication number Publication date
CN113034538B (zh) 2023-09-05

Similar Documents

Publication Publication Date Title
CN109887057B (zh) 生成高精度地图的方法和装置
CN109991636B (zh) 基于gps、imu以及双目视觉的地图构建方法及系统
WO2020093378A1 (en) Vehicle positioning system using lidar
WO2018142900A1 (ja) 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム
US9071829B2 (en) Method and system for fusing data arising from image sensors and from motion or position sensors
CN109443348B (zh) 一种基于环视视觉和惯导融合的地下车库库位跟踪方法
CN111220153B (zh) 基于视觉拓扑节点和惯性导航的定位方法
CN108731670A (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
US10996072B2 (en) Systems and methods for updating a high-definition map
CN113903011B (zh) 一种适用于室内停车场的语义地图构建及定位方法
CN110207714A (zh) 一种确定车辆位姿的方法、车载系统及车辆
EP2175237B1 (en) System and methods for image-based navigation using line features matching
Zhang et al. Vision-aided localization for ground robots
CN111220155A (zh) 基于双目视觉惯性里程计估计位姿的方法、装置与处理器
CN114001733A (zh) 一种基于地图的一致性高效视觉惯性定位算法
CN115135963A (zh) 用于在场景地图中生成3d参考点的方法
CN114638897B (zh) 基于无重叠视域的多相机系统的初始化方法、系统及装置
CN110989619B (zh) 用于定位对象的方法、装置、设备和存储介质
Xian et al. Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach
Peng et al. Vehicle odometry with camera-lidar-IMU information fusion and factor-graph optimization
WO2020018140A1 (en) Ballistic estimnation of vehicle data
Panahandeh et al. Vision-aided inertial navigation using planar terrain features
CN113034538B (zh) 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备
CN116481522A (zh) 一种室内单目惯导slam方法及系统
CN114111769B (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