CN113012216B - 特征分类优化方法和slam定位方法及其系统和电子设备 - Google Patents

特征分类优化方法和slam定位方法及其系统和电子设备 Download PDF

Info

Publication number
CN113012216B
CN113012216B CN201911326304.6A CN201911326304A CN113012216B CN 113012216 B CN113012216 B CN 113012216B CN 201911326304 A CN201911326304 A CN 201911326304A CN 113012216 B CN113012216 B CN 113012216B
Authority
CN
China
Prior art keywords
binocular camera
feature
pose
optimized
speed
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
CN201911326304.6A
Other languages
English (en)
Other versions
CN113012216A (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.)
Sunny Optical Zhejiang Research Institute Co Ltd
Original Assignee
Sunny Optical Zhejiang Research Institute 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 Sunny Optical Zhejiang Research Institute Co Ltd filed Critical Sunny Optical Zhejiang Research Institute Co Ltd
Priority to CN201911326304.6A priority Critical patent/CN113012216B/zh
Publication of CN113012216A publication Critical patent/CN113012216A/zh
Application granted granted Critical
Publication of CN113012216B publication Critical patent/CN113012216B/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/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
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/21—Design or setup of recognition systems or techniques; Extraction of features in feature space; Blind source separation
    • G06F18/211—Selection of the most significant subset of features
    • G06F18/2115—Selection of the most significant subset of features by evaluating different subsets according to an optimisation criterion, e.g. class separability, forward selection or backward elimination
    • 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
    • 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)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Multimedia (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Image Analysis (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

一种特征分类优化方法和SLAM定位方法及其系统和电子设备。该特征分类优化方法包括步骤：S110：相对于当前观测帧的特征点，确定是否存在跟踪丢失的特征点；S120：当存在该跟踪丢失的特征点时，根据双目相机的预测位姿和预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；以及S130：当不存在该跟踪丢失的特征点时，先从该当前观测帧的特征点中筛选多个特征点，再根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。

Description

特征分类优化方法和SLAM定位方法及其系统和电子设备
技术领域
本发明涉及SLAM技术领域，尤其是涉及特征分类优化方法和SLAM定位方法及其系统和电子设备。
背景技术
随着计算技术、智能技术以及传感器技术的不断提高，诸如AR/VR、无人机以及智能机器人等智能应用涌入市场，使得定位与地图构建(Simultaneous Localization AndMapping，简称SLAM)技术也备受关注。通常情况下，SLAM问题基本上可以分为前端和后端两个部分：前端主要处理传感器获取的数据，并将其转化为相对位姿或者其他机器可以理解的形式；后端主要处理最优后验估计的问题，即位姿或地图等的最优估计。现阶段的SLAM定位技术一般采用视觉惯性里程计(VIO)的方式进行位姿估计，这种方案定位精度高、效果稳定，使得视觉惯性里程计得以广泛地应用。
目前，由于视觉惯性里程计的开源算法种类繁杂，因此根据后端的不同优化方法，可分为滤波优化方法和非线性优化方法两大类。对于滤波优化方法而言，由于滤波优化方法中的状态向量维度、协方差矩阵相对较小，因此滤波优化方法的计算量小、速度快，进而能在快速场景下实现定位；滤波优化方法中典型的开源算法有S-MSCKF，但是其定位精度较低、鲁棒性不好。而对于非线性优化方法而言，由于非线性优化方法需要维护全局地图和全局关键帧，因此非线性优化方法的计算量大，实时性差；非线性优化方法的经典代表有VIN-Mono，其虽然能够在大部分场景下运行良好，但CPU资源要求较高，实时性差。
然而，诸如S-MSCKF的滤波优化方法通常仅仅利用跟踪丢失的特征点信息(即观测信息)进行滤波器优化处理，以得到双目相机的优化位姿和优化速度；而如果不存在跟踪丢失的特征点，则直接将该双目相机的预测位姿和预测速度作为该双目相机的优化位姿和优化速度，使得现有的诸如S-MSCKF的滤波优化方法不能充分利用观测信息，造成其定位精度降低，仍然达不到诸如AR/VR等应用中的要求，并且在运动条件复杂的情况下，会出现不可恢复的飘移。
发明内容
本发明的一优势在于提供一特征分类优化方法和SLAM定位方法及其系统和电子设备，其能够提高定位精度，以便达到诸如AR/VR等应用对定位精度的要求。
本发明的另一优势在于提供一特征分类优化方法和SLAM定位方法及其系统和电子设备，其中，在本发明的一实施例中，所述特征分类优化方法能够充分利用特征点信息，有助于提高定位精度。
本发明的另一优势在于提供一特征分类优化方法和SLAM定位方法及其系统和电子设备，其中，在本发明的一实施例中，所述特征分类优化方法能够将特征点分为两种类型处理，以最大化地利用特征点信息，有助于更好地提高定位精度。
本发明的另一优势在于提供一特征分类优化方法和SLAM定位方法及其系统和电子设备，其中，在本发明的一实施例中，所述特征分类优化方法能够在不影响实时性的情况下，尽可能多地利用特征点信息，进而提高定位精度。
本发明的另一优势在于提供一特征分类优化方法和SLAM定位方法及其系统和电子设备，其中，在本发明的一实施例中，所述特征分类优化方法能够通过筛选出跟踪效果较好的几个特征点进行滤波优化，以大幅地提升定位精度。
本发明的另一优势在于提供一特征分类优化方法和SLAM定位方法及其系统和电子设备，其中，在本发明的一实施例中，所述SLAM定位方法能够针对不同的观测信息，启动不同的滑窗方法，从而进一步提高定位精度。
本发明的另一优势在于提供一特征分类优化方法和SLAM定位方法及其系统和电子设备，其中，在本发明的一实施例中，所述SLAM定位方法能够加快前端处理速度，有助于进一步提高整体实时性，满足诸如AR/VR等应用对实时性的要求。
本发明的另一优势在于提供一特征分类优化方法和SLAM定位方法及其系统和电子设备，其中，在本发明的一实施例中，所述SLAM定位方法能够结合光流跟踪方法和极线搜索与块匹配方法对前端进行处理，以减少左右目特征跟踪的误差，并加快前端处理速度，从而进一步提高整体实时性。
本发明的另一优势在于提供一特征分类优化方法和SLAM定位方法及其系统和电子设备，其中为了达到上述优势，在本发明中不需要采用复杂的结构和庞大的计算量。因此，本发明成功和有效地提供一解决方案，不只提供一特征分类优化方法和SLAM定位方法及其系统和电子设备，同时还增加了所述特征分类优化方法和SLAM定位方法及其系统和电子设备的实用性和可靠性。
为了实现上述至少一优势或其他优势和目的，本发明提供了特征分类优化方法，包括步骤：
S110：相对于当前观测帧的特征点，确定是否存在跟踪丢失的特征点；
S120：当存在该跟踪丢失的特征点时，根据双目相机的预测位姿和预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；以及
S130：当不存在该跟踪丢失的特征点时，先从该当前观测帧的特征点中筛选多个特征点，再根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。
在本发明的一实施例中，所述步骤S130，包括步骤：
通过特征点筛选器，从该当前观测帧的特征点中筛选出左右匹配误差小于预定误差阈值的特征点；和
根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。
在本发明的一实施例中，所述特征点筛选器为
Figure BDA0002328470710000031
其中p1和p2分别为左右匹配特征点的坐标；t为平移量；R为旋转量。
在本发明的一实施例中，所述预定误差阈值根据该双目相机的内参来设定。
在本发明的一实施例中，所述步骤S130，包括步骤：
计算该当前观测帧中所有的特征点的左右匹配误差；
从该当前观测帧中依次筛选出该左右匹配误差最小的特征点，直至该筛选出的特征点的数量达到预定筛选数量为止；以及
根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。
在本发明的一实施例中，所述预定筛选数量为特征点的最大跟踪数的1/10。
根据本发明的另一方面，本发明进一步提供了SLAM定位方法，包括步骤：
对通过双目相机采集的原始图像进行前端处理，以得到当前观测帧的特征点信息；
对通过惯性测量单元采集的IMU信息进行滤波器预测处理，以得到该双目相机的预测位姿和预测速度；以及
通过特征分类优化方法，基于该当前观测帧的特征点信息和该双目相机的该预测位姿和该预测速度进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；其中所述特征分类优化方法包括步骤：
S110：相对于该当前观测帧的特征点，确定是否存在跟踪丢失的特征点；
S120：当存在该跟踪丢失的特征点时，根据该双目相机的该预测位姿和该预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的该优化位姿和该优化速度；以及
S130：当不存在该跟踪丢失的特征点时，先从该当前观测帧的特征点中筛选多个特征点，再根据该双目相机的该预测位姿和该预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的该优化位姿和该优化速度。
在本发明的一实施例中，所述的SLAM定位方法，进一步包括步骤：
通过滑窗处理，确定是否存在被剔除的观测帧；和
当存在该被剔除的观测帧时，则根据该双目相机的该优化位姿和该优化速度，对该被剔除的观测帧中的特征点信息进行滤波器优化处理，以得到该双目相机的输出位姿和输出速度；当不存在该被剔除的观测帧时，则直接将该双目相机的该优化位姿和该优化速度作为该双目相机的输出位姿和输出速度。
在本发明的一实施例中，所述对通过双目相机采集的原始图像进行前端处理，以得到当前观测帧的特征点信息的步骤，包括步骤：
通过光流跟踪方法对该原始图像中左目图像的特征点进行跟踪，以得到该当前观测帧中左目的特征点信息；和
根据该双目相机中的左目相机和右目相机之间的相对位姿，通过极线搜索与块匹配方法对该原始图像中右目图像的特征点进行跟踪，以得到该当前观测帧中右目的特征点信息。
在本发明的一实施例中，所述对通过双目相机采集的原始图像进行前端处理，以得到当前观测帧的特征点信息的步骤，进一步包括步骤：
判断通过该光流跟踪方法跟踪到的该左目图像的特征点的数量是否小于一特征点数量阈值，如果是，则通过特征点提取方法从该左目图像中提取出新的特征点信息，以补充到该当前观测帧中左目的特征点信息。
根据本发明的另一方面，本发明进一步提供了特征分类优化系统，包括相互可通信地连接的：
一确定模块，用于相对于当前观测帧的特征点，确定是否存在跟踪丢失的特征点；
一滤波器优化模块，用于当存在该跟踪丢失的特征点时，根据双目相机的预测位姿和预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；以及
一特征点筛选模块，其中所述特征点筛选模块用于当不存在该跟踪丢失的特征点时，先从该当前观测帧的特征点中筛选多个特征点；其中所述滤波器优化模块还用于根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。
在本发明的一实施例中，所述特征点筛选模块进一步用于通过特征点筛选器，从该当前观测帧的特征点中筛选出左右匹配误差小于预定误差阈值的特征点。
在本发明的一实施例中，所述特征点筛选模块进一步用于从该当前观测帧的特征点中依次筛选出左右匹配误差最小的特征点，直至该筛选出的特征点的数量达到预定筛选数量为止。
根据本发明的另一方面，本发明进一步提供了SLAM定位系统，用于根据通过双目相机采集的原始图像和通过惯性测量单元采集的IMU信息进行定位，其中所述SLAM定位系统包括：
一前端系统，用于对该原始图像进行前端处理，以得到当前观测帧的特征点信息；
一滤波器预测系统，用于对该IMU信息进行滤波器预测处理，以得到该双目相机的预测位姿和预测速度；
一特征分类优化系统，用于通过特征分类优化方法，基于该当前观测帧的特征点信息和该双目相机的该预测位姿和该预测速度进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；其中所述特征分类优化系统包括相互可通信地连接的：
一确定模块，用于相对于该当前观测帧的特征点，确定是否存在跟踪丢失的特征点；
一滤波器优化模块，用于当存在该跟踪丢失的特征点时，根据该双目相机的该预测位姿和该预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的该优化位姿和该优化速度；以及
一特征点筛选模块，其中所述特征点筛选模块用于当不存在该跟踪丢失的特征点时，先从该当前观测帧的特征点中筛选多个特征点；其中所述滤波器优化模块还用于根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。
在本发明的一实施例中，所述的SLAM定位系统，进一步包括一滑窗系统，其中所述滑窗系统可通信地连接于所述特征分类优化系统，并且所述滑窗系统用于通过滑窗处理，确定是否存在被剔除的观测帧；其中所述特征分类优化系统的所述滤波器优化模块还用于当存在该被剔除的观测帧时，则根据该双目相机的该优化位姿和该优化速度，对该被剔除的观测帧中的特征点信息进行滤波器优化处理，以得到该双目相机的输出位姿和输出速度；并且当不存在该被剔除的观测帧时，则直接将该双目相机的该优化位姿和该优化速度作为该双目相机的输出位姿和输出速度。
在本发明的一实施例中，所述前端系统包括相互可通信地连接的一光流跟踪模块、一极线搜索与块匹配模块以及一判断提取模块，其中所述光流跟踪模块用于通过光流跟踪方法对该原始图像中左目图像的特征点进行跟踪，以得到该当前观测帧中左目的特征点信息；其中所述极线搜索与块匹配模块用于根据该双目相机中的左目相机和右目相机之间的相对位姿，通过极线搜索与块匹配方法对该原始图像中右目图像的特征点进行跟踪，以得到该当前观测帧中右目的特征点信息；其中所述判断提取模块用于判断通过该光流跟踪方法跟踪到的该左目图像的特征点的数量是否小于一特征点数量阈值，如果是，则通过特征点提取方法从该左目图像中提取出新的特征点信息，以补充到该当前观测帧中左目的特征点信息。
根据本发明的另一方面，本发明进一步提供了电子设备，包括：
至少一处理器，用于执行指令；和
与所述至少一处理器可通信地连接的存储器，其中，所述存储器具有至少一指令，其中，所述指令被所述至少一处理器执行，以使得所述至少一处理器执行SLAM定位方法中的部分或全部步骤，其中所述SLAM定位方法包括步骤：
对通过双目相机采集的原始图像进行前端处理，以得到当前观测帧的特征点信息；
对通过惯性测量单元采集的IMU信息进行滤波器预测处理，以得到该双目相机的预测位姿和预测速度；以及
通过特征分类优化方法，基于该当前观测帧的特征点信息和该双目相机的该预测位姿和该预测速度进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；其中所述特征分类优化方法包括步骤：
S110：相对于该当前观测帧的特征点，确定是否存在跟踪丢失的特征点；
S120：当存在该跟踪丢失的特征点时，根据该双目相机的该预测位姿和该预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的该优化位姿和该优化速度；以及
S130：当不存在该跟踪丢失的特征点时，先从该当前观测帧的特征点中筛选多个特征点，再根据该双目相机的该预测位姿和该预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的该优化位姿和该优化速度。
通过对随后的描述和附图的理解，本发明进一步的目的和优势将得以充分体现。
本发明的这些和其它目的、特点和优势，通过下述的详细说明，附图和权利要求得以充分体现。
附图说明
图1是根据本发明的一实施例的特征分类优化方法的流程示意图。
图2示出了根据本发明的上述实施例的所述特征分类优化方法的步骤之一的一个示例。
图3示出了根据本发明的上述实施例的所述特征分类优化方法的所述步骤之一的另一个示例。
图4示出了根据本发明的一实施例的SLAM定位方法的流程示意图。
图5示出了根据本发明的上述实施例的所述SLAM定位方法的步骤之一的流程示意图。
图6示出了根据本发明的上述实施例的所述SLAM定位方法的一个示例。
图7示出了根据本发明的上述实施例的所述SLAM定位方法的前端处理步骤的一个示例。
图8示出了根据本发明的一实施例的特征分类优化系统的框图示意图。
图9示出了根据本发明的一实施例的SLAM定位系统的框图示意图。
图10示出了根据本发明的一实施例的电子设备的框图示意图。
具体实施方式
以下描述用于揭露本发明以使本领域技术人员能够实现本发明。以下描述中的优选实施例只作为举例，本领域技术人员可以想到其他显而易见的变型。在以下描述中界定的本发明的基本原理可以应用于其他实施方案、变形方案、改进方案、等同方案以及没有背离本发明的精神和范围的其他技术方案。
在本发明中，权利要求和说明书中术语“一”应理解为“一个或多个”，即在一个实施例，一个元件的数量可以为一个，而在另外的实施例中，该元件的数量可以为多个。除非在本发明的揭露中明确示意该元件的数量只有一个，否则术语“一”并不能理解为唯一或单一，术语“一”不能理解为对数量的限制。
在本发明的描述中，需要理解的是，属于“第一”、“第二”等仅用于描述目的，而不能理解为指示或者暗示相对重要性。本发明的描述中，需要说明的是，除非另有明确的规定和限定，属于“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接或者一体地连接；可以是机械连接，也可以是电连接；可以是直接连接，也可以是通过媒介间接连结。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或多个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
目前，现有的诸如S-MSCKF之类的基于滤波的SLAM定位方法通常采用视觉传感器(如双目相机等)与惯性测量单元(IMU)进行融合定位，但其仅仅利用跟踪丢失的特征点信息(即观测信息)进行滤波器优化处理，以得到双目相机的优化位姿和优化速度；而如果不存在跟踪丢失的特征点，则直接将该双目相机的预测位姿和预测速度作为该双目相机的优化位姿和优化速度。然而，跟踪丢失的特征点(简称丢失点)的占比通常很小，而能够被跟踪的特征点(简称跟踪点)的占比则很大，而这些特征点信息中有着丰富的约束条件，现有的诸如S-MSCKF的滤波优化方法较少地甚至没有利用特征点信息，造成其定位精度降低，无法达到诸如AR/VR等应用的要求。因此，为了解决上述问题，本发明提出一种特征分类优化方法和SLAM定位方法及其系统和电子设备。
示意性方法
参考说明书附图之图1至图3所示，根据本发明的一实施例的一种特征分类优化方法被阐明。具体地，如图1所示，所述特征分类优化方法包括步骤：
S110：相对于当前观测帧的特征点，确定是否存在跟踪丢失的特征点；
S120：当存在所述跟踪丢失的特征点时，根据双目相机的预测位姿和预测速度，对所述跟踪丢失的特征点的信息进行滤波器优化处理，以得到所述双目相机的优化位姿和优化速度；以及
S130：当不存在所述跟踪丢失的特征点时，先从所述当前观测帧的特征点中筛选多个特征点，再根据所述双目相机的预测位姿和预测速度，对筛选出的所述特征点的信息进行滤波器优化处理，以得到所述双目相机的优化位姿和优化速度。
值得注意的是，本发明的所述特征分类优化方法将特征点分为跟踪丢失的特征点(即丢失点)和被跟踪的特征点(即跟踪点)两种类型进行处理，以最大化地利用特征点信息，而这些特征点信息中又存在着丰富的约束条件，本发明的所述特征分类优化方法利用这些特征点信息(包括丢失点的观测信息和跟踪点的观测信息)进行滤波器优化，有助于提高定位精度。换言之，当存在所述跟踪丢失的特征点时，本发明会对这些跟踪丢失的特征点地信息进行滤波器优化处理，以获得精度较高的优化位姿和优化速度；而当不存在所述跟踪丢失的特征点信息，即所述当前观测帧出现的特征点跟踪率小于100％时，本发明仍然会选取预定数量的特征点进行滤波器优化处理，使得所述双目相机的优化位姿和优化速度的精度仍能够得以提升，从而提升整体定位精度。
可以理解的是，本发明的所述双目相机的预测位姿和预测速度可以但不限于通过对经由惯性测量单元采集的IMU信息进行滤波器预测处理而获得；而本发明的所述当前观测帧的特征点可以通过对经由双目相机采集的原始图像进行前端处理而获得，本发明对此不再赘述。
更具体地，根据本发明的上述实施例，在所述特征分类优化方法的所述步骤S110中，根据所述当前观测帧的特征点的信息进行地图构建，以确定是否存在所述跟踪丢失的特征点。当然，在本发明的其他示例中，所述特征分类优化方法也可以在通过对经由双目相机采集的原始图像进行前端处理时，确定是否存在所述跟踪丢失的特征点。
值得一提的是，当不存在所述跟踪丢失的特征点时，本发明的所述特征分类优化方法会从所述当前观测帧的特征点中筛选出的特征点以进行滤波器优化，从而得到精度较高的优化位姿和优化速度。但由于所述当前观测帧中特征点的左右匹配误差大小不一，因此为了尽可能多地提升定位精度，在本发明的所述特征分类优化方法的所述步骤S130中：从所述当前观测帧的特征点中优选地筛选出左右匹配误差较小的特征点，也就是说，从所述当前观测帧的特征点中优选地筛选出跟踪效果较好的特征点。
示例性地，在本发明的一示例中，如图2所示，所述特征分类优化方法的所述步骤S130，可以包括步骤：
S131：通过特征点筛选器，从所述当前观测帧的特征点中筛选出左右匹配误差小于预定误差阈值的特征点；和
S132：根据所述双目相机的预测位姿和预测速度，对筛选出的特征点的信息进行滤波器优化处理，以得到所述双目相机的优化位姿和优化速度。
值得注意的是，本发明的所述特征点筛选器可以但不限于被实施为：
Figure BDA0002328470710000101
其中，p1和p2分别为左右匹配特征点的坐标；t为平移量；R为旋转量。
可以理解的是，本发明的所述特征点的左右匹配误差
Figure BDA0002328470710000102
理论上：如果p1和p2精确匹配，则/>
Figure BDA0002328470710000103
而由于存在噪声、跟踪误差等原因，则/>
Figure BDA0002328470710000104
不等于0，但/>
Figure BDA0002328470710000105
越接近0，则p1和p2之间的左右匹配度就越高。
优选地，本发明的所述预定误差阈值可以根据所述双目相机的内参来设定，以获得跟踪效果更好的特征点。例如，所述预定误差阈值可以但不限于被实施为
Figure BDA0002328470710000111
其中s为系数；cx和cy分别为所述双目相机的内参。
值得一提的是，在本发明的所述特征分类优化方法的所述步骤S131中，筛选出来的特征点的数量对系统的定位精度和实时性均有影响，当筛选出来的特征点的数量过多时，虽然定位精度提高了，但滤波器优化处理时的计算量就变大了，导致实时性降低。因此为了在提高定位精度的同时，又保证系统的实时性满足实际应用要求，在本发明的另一示例中，如图3所示，本发明的所述特征分类优化方法的所述步骤S130可以包括步骤：
S131’：计算所述当前观测帧中所有的特征点的左右匹配误差；
S132’：从所述当前观测帧中依次筛选出左右匹配误差最小的特征点，直至筛选出的特征点的数量达到预定筛选数量为止；
S133’：根据所述双目相机的预测位姿和预测速度，对筛选出的特征点的信息进行滤波器优化处理，以得到所述双目相机的优化位姿和优化速度。
优选地，本发明的所述预定筛选数量可以根据特征点的最大跟踪数来设定，以在保证实时性的情况下，尽可能多地提高定位精度。例如，本发明的所述预定筛选数量可以但不限于被实施为特征点的最大跟踪数的1/10，也就是说，当特征点的最大跟踪数为八十时，则所述预定筛选数量为八个。
值得注意的是，在本发明的其他示例中，本发明的所述特征分类优化方法也可以兼顾所述左右匹配误差和所述预定筛选数量，例如，当从所述当前观测帧中依次筛选出左右匹配误差最小的特征点时，所筛选出的特征点的左右匹配误差达到所述预定误差阈值和所筛选出的特征点的数量达到所述预定筛选数量中任一条件满足就立刻停止筛选，以便在保证系统实时性的同时，尽可能大地提高定位精度。
根据本发明的另一方面，如图4所示，本发明进一步提供了一SLAM定位方法，包括步骤：
S210：对通过双目相机采集的原始图像进行前端处理，以得到当前观测帧的特征点信息；
S220：对通过惯性测量单元采集的IMU信息进行滤波器预测处理，以得到所述双目相机的预测位姿和预测速度；以及
S230：通过上述特征分类优化方法，基于所述当前观测帧的特征点信息和所述双目相机的预测位姿和预测速度进行滤波器优化处理，以得到所述双目相机的优化位姿和优化速度。
值得注意的是，本发明的所述SLAM定位方法的所述特征分类优化方法可以通过将特征点分为丢失特征点和跟踪点两种类型处理，最大化的利用观测信息，在不影响实时性的情况下，能够利用特征点信息。也就是说，通过将特征点分类处理，相对于S-MSCKF来说，仅仅利用跟踪丢失的特征点进行优化，并不能更好的利用观测信息，如果出现特征点跟踪率为100％的情况下，就更少的或者不用观测信息，从而减小了观测信息的利用率。而在一般情况下，跟踪点占观测信息的比例很大，这些观测信息有丰富的约束条件，可以利用这些约束进行滤波优化，从而提高定位精度。相比于非线性优化的方法来说，将特征进行分类处理，能够减少计算量与内存的消耗。
此外，由于每个输入后端的观测帧均通过窗口进行保存，而当所述窗口中的观测帧的数量满足最大可存的数量(即最大窗口数)时，就需要从所述窗口中剔除若干观测帧，以完成滑窗操作。因此，为了进一步利用被剔除的观测帧中的特征点信息，以进一步提升定位精度，如图4所示，本发明的所述SLAM定位方法，进一步包括步骤：
S240：通过滑窗处理，确定是否存在被剔除的观测帧；以及
S250：当存在所述被剔除的观测帧时，根据所述双目相机的优化位姿和优化速度，对所述被剔除的观测帧中的特征点信息进行滤波器优化处理，以得到所述双目相机的输出位姿和输出速度；当不存在所述被剔除的观测帧时，直接将所述双目相机的优化位姿和优化速度作为所述双目相机的输出位姿和输出速度。
值得注意的是，在本发明的所述SLAM定位方法的所述步骤S240中，可以采用传统的滑窗方法进行滑窗处理，也可以采用分类滑窗的方法进行滑窗处理，本发明对此不作进一步限制。例如，当采用传统的滑窗方法进行滑窗处理时，只在满窗口(即所述窗口中的所述观测帧的数量等于最大窗口数)时，才会剔除若干观测帧；在非满窗口时，不会剔除任何观测帧。而当采用分类滑窗的方法进行滑窗处理时，则会根据不同的观测信息采用不同的滑窗方法，优化多帧观测对特征点的约束，提高定位精度。
示例性地，当采用分类滑窗方法进行滑窗处理时，本发明的所述SLAM定位方法的所述步骤S240可以包括步骤：确定窗口中所有的观测帧的数量是否达到所述窗口的最大窗口数；当所述观测帧的数量达到所述最大窗口数时，则根据所述窗口中最老的第一帧与最老的第二帧之间的相对位姿，从所述窗口中间隔地批量剔除预定数量的观测帧；以及当所述观测帧的数量小于所述最大窗口数时，进一步确定所述观测帧的数量是否大于一预设的帧数阈值，如果是，根据当前观测帧的特征点跟踪率，从所述窗口中选择性地剔除观测帧；如果否，则保留所述窗口中所有的所述观测帧。
值得一提的是，在对通过双目相机采集的原始图像(包括原始图像中左目图像和右目图像)进行前端处理，以得到当前观测帧的特征点信息(包括当前观测帧中左目的特征点信息和右目的特征点信息)的过程中，通常需要对所述原始图像信息进行特征点跟踪。而现有的S-MSCKF定位方法通常先采用光流跟踪方法进行左目图像的特征点跟踪，以得到所述当前观测帧中左目的特征点信息，再采用立体匹配方法进行右目图像的特征点跟踪，以得到所述当前观测帧中右目的特征点信息，从而获得所述当前观测帧的特征点信息。但是现有的S-MSCKF定位方法中获得所述当前观测帧的特征点信息的计算量较大、前端处理速度较慢，所需花费的时间也较长，尤其是左右目特征点跟踪的误差较大，导致现有的S-MSCKF定位方法的定位精度和实时性难以满足诸如AR/VR等应用的要求。
因此，为了减小左右目特征点跟踪的误差，并加快前端处理速度，如图5所示，本发明的所述SLAM定位方法的所述步骤S210，可以包括步骤：
S211：通过光流跟踪方法对所述原始图像中左目图像的特征点进行跟踪，以得到所述当前观测帧中左目的特征点信息；和
S212：根据所述双目相机中的左目相机和右目相机之间的相对位姿，通过极线搜索与块匹配方法对所述原始图像中右目图像的特征点进行跟踪，以得到所述当前观测帧中右目的特征点信息。
值得注意的是，在本发明中，对于新接收的一帧左目图像(即所述当前的原始图像中的左目图像)，采用光流跟踪方法跟踪到的左目图像的特征点数量有可能会变少，即可能会存在跟踪丢失的特征点。此时就需要对特征点进行补充，以使特征点的数量达到最大数。因此，根据本发明的上述实施例，如图5所示，本发明的所述SLAM定位方法的所述步骤S210，可以进一步包括步骤：
S213：判断通过光流跟踪方法跟踪到的所述左目图像的特征点的数量是否小于特征点数量阈值，如果是，则通过特征点提取方法从所述左目图像中提取出新的特征点信息，以补充到所述当前观测帧中左目的特征点信息。
示例性地，在本发明的一示例中，如图6所示，本发明的所述SLAM定位方法可以包括如下步骤：
步骤1：系统初始化与特征提取
将整个系统初始化，得到系统所需相机内外参以及IMU初始参数，接收视觉传感器的信息，并对其原始图像信息进行滤波，将图像建立两层金字塔，先从金字塔顶层到底层提取特征点，在最大特征点个数固定的情况下，加快特征点提取速度，再利用harris响应值进行排序，选取响应值高的特征点，输出特征。
步骤2：特征跟踪与匹配
如图7所示，首先，提取左目图像的特征，利用左右目相机相对位姿，通过极线搜索与块匹配的方法进行特征跟踪，并将跟踪的结果输入后端；然后，对新来的一帧左目图像，采用光流跟踪，得到新一帧图像的特征点；如果特征点相对较少，则通过特征提取方法提取足够的特征进行补充，满足特征点的最大跟踪数，将跟踪的特征点输入后端，完成前端处理。
步骤3：IMU的初始化与预积分，滤波器初始化
IMU的初始化采用静态初始化，确定重力加速度的方向，同时为初始化相机提供方向。IMU的数据需要进行预积分处理作为EKF的预测值，其中所述预积分方法可以但不限于采用4阶龙格库塔算法。
而滤波器的初始化，则是对滤波器的参数进行初始值的设定，特别是协方差矩阵以及系统噪声的初始值，该值对滤波精度起着很重要的作用。具体的过程如下：首先，建立连续性的IMU误差模型；其次，将矩阵F与G离散化；接着，利用上一时刻的协方差对当前时刻IMU协方差进行预测；最后，对系统的协方差预测方程进行可观一致性矫正。
步骤4：相机状态与IMU同步，并增广协方差
当有新的相机信息(即当前观测帧的特征点信息)输入后端时，需要先将IMU通过预积分对当前的位姿进行预测，并通过IMU与相机相对位姿，计算出相机的位姿，同步两个传感器的位姿。当系统增加相机时，需要对协方差进行增广。
步骤5：构建地图并对特征点进行处理
接收到视觉信息，经过前端处理后，输入后端，把其中的特征点建立成局部地图，用来约束特征点。而新来的特征点数量很多，不可能全部都用来滤波更新，因此，本发明把特征点分类处理，第一类，就是在当前帧中跟踪丢失的特征点，另一类，在当前帧并未丢失的特征点。若在当前帧中存在丢失特征点的情况，则只用这些丢失的特征点进行滤波优化，如果在当前帧出现特征点跟踪率为100％，则从当前帧特征点中选取出几个跟踪效果最好的特征点进行滤波优化。
步骤6：滑动窗口
对每个输入后端的观测帧，均用窗口保存，当这个窗口满足最大可存的数量的时候，通过滑动窗口，删除所述窗口中的若干观测帧。例如，本发明提出新的滑窗策略，针对不同的观测信息，启动不同的滑窗方法，具体操作如下：
(1)当窗口中的观测帧数量满足帧数阈值时，但小于最大窗口数时，并且出现在当前帧特征点跟踪率为100％时，即在当前帧中不存在丢失特征点的情况，从窗口内最老的第二帧开始，计算与最老的第一帧之间角度差和距离差，若满足阈值，则剔除所述最老的第二帧；依次类推，最多剔除所述观测帧的数量为所述最大窗口数的1/3。
(2)当窗口内的观测帧数满足最大窗口数时，计算最老的第一帧与最老的第二帧距离差和角度差，如果满足阈值，则剔除所述最老的第一帧；如果没有，则不剔除所述最老的第一帧。然后，再从窗口内所述最老的第二帧开始，连续等间隔地剔除十帧。
步骤7：系统更新
系统更新主要任务是先利用在预测模块得到的对当前时刻状态、协方差预测值；再利用筛选出来的特征点，构建测量模型，并通过扩展卡尔曼滤波算法，对两种信息进行滤波，得到当前时刻的估计值。值得注意的是，本发明的所述SLAM定位方法在通过EKF更新之后，得出当前的位姿估计，采用EUROC数据集测试结果表明，极大提高了定位精度。
示意性系统
参考说明书附图之图8所示，根据本发明的一实施例的一特征分类优化系统被阐明。具体地，如图8所示，所述特征分类优化系统10可以包括相互可通信地连接的一确定模块11、一滤波器优化模块12以及一特征点筛选模块13，其中所述确定模块11用于相对于当前观测帧的特征点，确定是否存在跟踪丢失的特征点；其中所述滤波器优化模块12用于当存在跟踪丢失的特征点时，根据双目相机的预测位姿和预测速度，对所述跟踪丢失的特征点的信息进行滤波器优化处理，以得到所述双目相机的优化位姿和优化速度；其中所述特征点筛选模块13用于当不存在所述跟踪丢失的特征点时，从所述当前观测帧的特征点中筛选多个特征点；其中所述滤波器优化模块12还用于根据所述双目相机的预测位姿和预测速度，对筛选出的所述特征点的信息进行滤波器优化处理，以得到所述双目相机的优化位姿和优化速度。
值得注意的是，在本发明的一示例中，所述特征分类优化系统10的所述特征点筛选模块13可以用于通过特征点筛选器，从所述当前观测帧的特征点中筛选出左右匹配误差小于预定误差阈值的特征点。
在本发明的另一示例中，所述特征分类优化系统10的所述特征点筛选模块13也可以用于从所述当前观测帧中依次筛选出左右匹配误差最小的特征点，直至所述筛选出的特征点的数量达到预定筛选数量为止。
根据本发明的另一方面，如图9所示，本发明的一实施例进一步提供了一种SLAM定位系统1。具体地，如图9所示，所述SLAM定位系统1包括所述特征分类优化系统10、一前端系统20以及一滤波器预测系统30。所述前端系统20用于对通过双目相机采集的原始图像进行前端处理，以得到当前观测帧的特征点信息。所述滤波器预测系统30用于对通过惯性测量单元采集的IMU信息进行滤波器预测处理，以得到所述双目相机的预测位姿和预测速度。所述特征分类优化系统10用于通过上述特征分类优化方法，基于所述当前观测帧的特征点信息和所述双目相机的预测位姿和预测速度进行滤波器优化处理，以得到所述双目相机的优化位姿和优化速度。
值得注意的是，如图9所示，所述特征分类优化系统10可以包括相互可通信地连接的一确定模块11、一滤波器优化模块12以及一特征点筛选模块13，其中所述确定模块11用于相对于当前观测帧的特征点，确定是否存在跟踪丢失的特征点；其中所述滤波器优化模块12用于当存在跟踪丢失的特征点时，根据双目相机的预测位姿和预测速度，对所述跟踪丢失的特征点的信息进行滤波器优化处理，以得到所述双目相机的优化位姿和优化速度；其中所述特征点筛选模块13用于当不存在所述跟踪丢失的特征点时，从所述当前观测帧的特征点中筛选多个特征点；其中所述滤波器优化模块12还用于根据所述双目相机的预测位姿和预测速度，对筛选出的所述特征点的信息进行滤波器优化处理，以得到所述双目相机的优化位姿和优化速度。
此外，如图9所示，所述SLAM定位系统1进一步包括一滑窗系统40，其中所述滑窗系统40可通信地连接于所述特征分类优化系统10，用于通过滑窗处理，确定是否存在被剔除的观测帧。所述特征分类优化系统10的所述滤波器优化模块12还可以用于当存在所述被剔除的观测帧时，根据所述双目相机的优化位姿和优化速度，对所述被剔除的观测帧中的特征点信息进行滤波器优化处理，以得到所述双目相机的输出位姿和输出速度；当不存在所述被剔除的观测帧时，直接将所述双目相机的优化位姿和优化速度作为所述双目相机的输出位姿和输出速度。
值得注意的是，在本发明的一实施例中，如图9所示，所述前端系统20可以包括相互可通信地连接的一光流跟踪模块21和一极线搜索与块匹配模块22，其中所述光流跟踪模块21用于通过光流跟踪方法对当前的原始图像中左目图像的特征点进行跟踪，以得到所述当前观测帧中左目的特征点信息；其中所述极线搜索与块匹配模块22用于根据所述双目相机中的左目相机和右目相机之间的相对位姿，通过极线搜索与块匹配方法对所述当前的原始图像中右目图像的特征点进行跟踪，以得到所述当前观测帧中右目的特征点信息。
优选地，如图9所示，所述前端系统20可以进一步包括一判断提取模块23，其中所述判断提取模块23用于判断通过光流跟踪方法追踪到的所述左目图像的特征点的数量是否小于特征点数量阈值，如果是，则通过特征点提取方法从所述当前的原始图像中的所述左目图像中提取出新的特征点信息，以补充到所述当前观测帧中左目的特征点信息。
示意性电子设备
下面，参考图10来描述根据本发明的一实施例的电子设备。如图10所示，电子设备90包括一个或多个处理器91和存储器92。
所述处理器91可以是中央处理单元(CPU)或者具有数据处理能力和/或指令执行能力的其他形式的处理单元，并且可以控制电子设备90中的其他组件以执行期望的功能。换言之，所述处理器91包括被配置成执行指令的一个或多个物理设备。例如，所述处理器91可被配置成执行作为以下各项的一部分的指令：一个或多个应用、服务、程序、例程、库、对象、组件、数据结构、或其他逻辑构造。这种指令可被实现以执行任务、实现数据类型、转换一个或多个部件的状态、实现技术效果、或以其他方式得到期望结果。
所述处理器91可包括被配置成执行软件指令的一个或多个处理器。作为补充或替换，所述处理器91可包括被配置成执行硬件或固件指令的一个或多个硬件或固件逻辑机。所述处理器91的处理器可以是单核或多核，且在其上执行的指令可被配置为串行、并行和/或分布式处理。所述处理器91的各个组件可任选地分布在两个或更多单独设备上，这些设备可以位于远程和/或被配置成进行协同处理。所述处理器91的各方面可由以云计算配置进行配置的可远程访问的联网计算设备来虚拟化和执行。
所述存储器92可以包括一个或多个计算程序产品，所述计算程序产品可以包括各种形式的计算可读存储介质，例如易失性存储器和/或非易失性存储器。所述易失性存储器例如可以包括随机存取存储器(RAM)和/或高速缓冲存储器(cache)等。所述非易失性存储器例如可以包括只读存储器(ROM)、硬盘、闪存等。在所述计算可读存储介质上可以存储一个或多个计算程序指令，所述处理器11可以运行所述程序指令，以实现上文所述的本发明的上述示意性方法中的部分步骤或全部步骤，以及/或者其他期望的功能。
换言之，所述存储器92包括被配置成保存可由所述处理器91执行以实现此处所述的方法和过程的机器可读指令的一个或多个物理设备。在实现这些方法和过程时，可以变换所述存储器92的状态(例如，保存不同的数据)。所述存储器92可以包括可移动和/或内置设备。所述存储器92可包括光学存储器(例如，CD、DVD、HD-DVD、蓝光盘等)、半导体存储器(例如，RAM、EPROM、EEPROM等)和/或磁存储器(例如，硬盘驱动器、软盘驱动器、磁带驱动器、MRAM等)等等。所述存储器92可包括易失性、非易失性、动态、静态、读/写、只读、随机存取、顺序存取、位置可寻址、文件可寻址和/或内容可寻址设备。
可以理解，所述存储器92包括一个或多个物理设备。然而，本文描述的指令的各方面可另选地通过不由物理设备在有限时长内持有的通信介质(例如，电磁信号、光信号等)来传播。所述处理器91和所述存储器92的各方面可被一起集成到一个或多个硬件逻辑组件中。这些硬件逻辑组件可包括例如现场可编程门阵列(FPGA)、程序和应用专用的集成电路(PASIC/ASIC)、程序和应用专用的标准产品(PSSP/ASSP)、片上系统(SOC)以及复杂可编程逻辑器件(CPLD)。
在一个示例中，如图10所示，所述电子设备90还可以包括输入装置93和输出装置94，这些组件通过总线系统和/或其他形式的连接机构(未示出)互连。例如，该输入装置93可以是例如用于采集图像数据或视频数据的摄像模组等等。又如，所述输入装置93可以包括诸如键盘、鼠标、触摸屏或游戏控制器之类的一个或多个用户输入设备或者与其对接。在一些实施例中，所述输入装置93可以包括所选择的自然用户输入(NUI)部件或与其对接。这种元件部分可以是集成的或外围的，并且输入动作的转导和/或处理可以在板上或板外被处理。示例NUI部件可包括用于语言和/或语音识别的话筒；用于机器视觉和/或姿势识别的红外、色彩、立体显示和/或深度相机；用于运动检测和/或意图识别的头部跟踪器、眼睛跟踪器、加速计和/或陀螺仪；以及用于评估脑部活动和/或身体运动的电场感测部件；和/或任何其他合适的传感器。
该输出装置94可以向外部输出各种信息，包括分类结果等。该输出装置94可以包括例如显示器、扬声器、打印机、以及通信网络及其所连接的远程输出设备等等。
当然，所述电子设备90还可以进一步包括所述通信装置，其中所述通信装置可被配置成将所述电子设备90与一个或多个其他计算机设备通信地耦合。所述通信装置可以包括与一个或多个不同通信协议兼容的有线和/或无线通信设备。作为非限制性示例，通信子系统可被配置成用于经由无线电话网络或者有线或无线局域网或广域网来进行通信。在一些实施例中，所述通信装置可允许所述电子设备90经由诸如因特网这样的网络将消息发送至其他设备以及/或者从其它设备接收消息。
将会理解，此处描述的配置和/或方法本质是示例性的，这些具体实施例或示例不应被视为限制性的，因为许多变体是可能的。此处描述的具体例程或方法可以表示任何数量的处理策略中的一个或多个。如此，所示和/或所述的各种动作可以以所示和/或所述顺序、以其他顺序、并行地执行，或者被省略。同样，上述过程的次序可以改变。
当然，为了简化，图10中仅示出了该电子设备90中与本发明有关的组件中的一些，省略了诸如总线、输入/输出接口等等的组件。除此之外，根据具体应用情况，电子设备90还可以包括任何其他适当的组件。
示意性计算程序产品
除了上述方法和设备以外，本发明的实施例还可以是计算程序产品，其包括计算程序指令，所述计算程序指令在被处理器运行时使得所述处理器执行本说明书上述“示例性方法”部分中描述的根据本发明各种实施例的方法中的步骤。
所述计算程序产品可以以一种或多种程序设计语言的任意组合来编写用于执行本发明实施例操作的程序代码，所述程序设计语言包括面向对象的程序设计语言，诸如Java、C++等，还包括常规的过程式程序设计语言，诸如C语言或类似的程序设计语言。程序代码可以完全地在用户计算设备上执行、部分地在用户设备上执行、作为一个独立的软件包执行、部分在用户计算设备上部分在远程计算设备上执行、或者完全在远程计算设备或服务器上执行。
此外，本发明的实施例还可以是计算可读存储介质，其上存储有计算程序指令，所述计算程序指令在被处理器运行时使得所述处理器执行本说明书上述方法中的步骤。
所述计算可读存储介质可以采用一个或多个可读介质的任意组合。可读介质可以是可读信号介质或者可读存储介质。可读存储介质例如可以包括但不限于电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。可读存储介质的更具体的例子(非穷举的列表)包括：具有一个或多个导线的电连接、便携式盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。
以上结合具体实施例描述了本发明的基本原理，但是，需要指出的是，在本发明中提及的优点、优势、效果等仅是示例而非限制，不能认为这些优点、优势、效果等是本发明的各个实施例必须具备的。另外，上述公开的具体细节仅是为了示例的作用和便于理解的作用，而非限制，上述细节并不限制本发明为必须采用上述具体的细节来实现。
本发明中涉及的器件、装置、设备、系统的方框图仅作为例示性的例子并且不意图要求或暗示必须按照方框图示出的方式进行连接、布置、配置。如本领域技术人员将认识到的，可以按任意方式连接、布置、配置这些器件、装置、设备、系统。诸如“包括”、“包含”、“具有”等等的词语是开放性词汇，指“包括但不限于”，且可与其互换使用。这里所使用的词汇“或”和“和”指词汇“和/或”，且可与其互换使用，除非上下文明确指示不是如此。这里所使用的词汇“诸如”指词组“诸如但不限于”，且可与其互换使用。
还需要指出的是，在本发明的装置、设备和方法中，各部件或各步骤是可以分解和/或重新组合的。这些分解和/或重新组合应视为本发明的等效方案。
提供所公开的方面的以上描述以使本领域的任何技术人员能够做出或者使用本发明。对这些方面的各种修改对于本领域技术人员而言是非常显而易见的，并且在此定义的一般原理可以应用于其他方面而不脱离本发明的范围。因此，本发明不意图被限制到在此示出的方面，而是按照与在此公开的原理和新颖的特征一致的最宽范围。
本领域的技术人员应理解，上述描述及附图中所示的本发明的实施例只作为举例而并不限制本发明。本发明的目的已经完整并有效地实现。本发明的功能及结构原理已在实施例中展示和说明，在没有背离所述原理下，本发明的实施方式可以有任何变形或修改。

Claims (17)

1.特征分类优化方法，其特征在于，包括步骤：
S110：相对于当前观测帧的特征点，确定是否存在跟踪丢失的特征点；
S120：当存在该跟踪丢失的特征点时，根据双目相机的预测位姿和预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；以及
S130：当不存在该跟踪丢失的特征点时，先从该当前观测帧的特征点中筛选多个特征点，再根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。
2.如权利要求1所述的特征分类优化方法，其中，所述步骤S130，包括步骤：
通过特征点筛选器，从该当前观测帧的特征点中筛选出左右匹配误差小于预定误差阈值的特征点；和
根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。
3.如权利要求2所述的特征分类优化方法，其中，所述特征点筛选器为
Figure FDA0002328470700000011
其中p1和p2分别为左右匹配特征点的坐标；t为平移量；R为旋转量。
4.如权利要求3所述的特征分类优化方法，其中，所述预定误差阈值根据该双目相机的内参来设定。
5.如权利要求1所述的特征分类优化方法，其中，所述步骤S130，包括步骤：
计算该当前观测帧中所有的特征点的左右匹配误差；
从该当前观测帧中依次筛选出该左右匹配误差最小的特征点，直至该筛选出的特征点的数量达到预定筛选数量为止；以及
根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。
6.如权利要求5所述的特征分类优化方法，其中，所述预定筛选数量为特征点的最大跟踪数的1/10。
7.SLAM定位方法，其特征在于，包括步骤：
对通过双目相机采集的原始图像进行前端处理，以得到当前观测帧的特征点信息；
对通过惯性测量单元采集的IMU信息进行滤波器预测处理，以得到该双目相机的预测位姿和预测速度；以及
通过特征分类优化方法，基于该当前观测帧的特征点信息和该双目相机的该预测位姿和该预测速度进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；其中所述特征分类优化方法包括步骤：
S110：相对于该当前观测帧的特征点，确定是否存在跟踪丢失的特征点；
S120：当存在该跟踪丢失的特征点时，根据该双目相机的该预测位姿和该预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的该优化位姿和该优化速度；以及
S130：当不存在该跟踪丢失的特征点时，先从该当前观测帧的特征点中筛选多个特征点，再根据该双目相机的该预测位姿和该预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的该优化位姿和该优化速度。
8.如权利要求7所述的SLAM定位方法，进一步包括步骤：
通过滑窗处理，确定是否存在被剔除的观测帧；和
当存在该被剔除的观测帧时，则根据该双目相机的该优化位姿和该优化速度，对该被剔除的观测帧中的特征点信息进行滤波器优化处理，以得到该双目相机的输出位姿和输出速度；当不存在该被剔除的观测帧时，则直接将该双目相机的该优化位姿和该优化速度作为该双目相机的输出位姿和输出速度。
9.如权利要求7或8所述的SLAM定位方法，其中，所述对通过双目相机采集的原始图像进行前端处理，以得到当前观测帧的特征点信息的步骤，包括步骤：
通过光流跟踪方法对该原始图像中左目图像的特征点进行跟踪，以得到该当前观测帧中左目的特征点信息；和
根据该双目相机中的左目相机和右目相机之间的相对位姿，通过极线搜索与块匹配方法对该原始图像中右目图像的特征点进行跟踪，以得到该当前观测帧中右目的特征点信息。
10.如权利要求9所述的SLAM定位方法，其中，所述对通过双目相机采集的原始图像进行前端处理，以得到当前观测帧的特征点信息的步骤，进一步包括步骤：
判断通过该光流跟踪方法跟踪到的该左目图像的特征点的数量是否小于一特征点数量阈值，如果是，则通过特征点提取方法从该左目图像中提取出新的特征点信息，以补充到该当前观测帧中左目的特征点信息。
11.特征分类优化系统，其特征在于，包括相互可通信地连接的：
一确定模块，用于相对于当前观测帧的特征点，确定是否存在跟踪丢失的特征点；
一滤波器优化模块，用于当存在该跟踪丢失的特征点时，根据双目相机的预测位姿和预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；以及
一特征点筛选模块，其中所述特征点筛选模块用于当不存在该跟踪丢失的特征点时，先从该当前观测帧的特征点中筛选多个特征点；其中所述滤波器优化模块还用于根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。
12.如权利要求11所述的特征分类优化系统，其中，所述特征点筛选模块进一步用于通过特征点筛选器，从该当前观测帧的特征点中筛选出左右匹配误差小于预定误差阈值的特征点。
13.如权利要求11所述的特征分类优化系统，其中，所述特征点筛选模块进一步用于从该当前观测帧的特征点中依次筛选出左右匹配误差最小的特征点，直至该筛选出的特征点的数量达到预定筛选数量为止。
14.SLAM定位系统，用于根据通过双目相机采集的原始图像和通过惯性测量单元采集的IMU信息进行定位，其特征在于，其中所述SLAM定位系统包括：
一前端系统，用于对该原始图像进行前端处理，以得到当前观测帧的特征点信息；
一滤波器预测系统，用于对该IMU信息进行滤波器预测处理，以得到该双目相机的预测位姿和预测速度；
一特征分类优化系统，用于通过特征分类优化方法，基于该当前观测帧的特征点信息和该双目相机的该预测位姿和该预测速度进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；其中所述特征分类优化系统包括相互可通信地连接的：
一确定模块，用于相对于该当前观测帧的特征点，确定是否存在跟踪丢失的特征点；
一滤波器优化模块，用于当存在该跟踪丢失的特征点时，根据该双目相机的该预测位姿和该预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的该优化位姿和该优化速度；以及
一特征点筛选模块，其中所述特征点筛选模块用于当不存在该跟踪丢失的特征点时，先从该当前观测帧的特征点中筛选多个特征点；其中所述滤波器优化模块还用于根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。
15.如权利要求14所述的SLAM定位系统，进一步包括一滑窗系统，其中所述滑窗系统可通信地连接于所述特征分类优化系统，并且所述滑窗系统用于通过滑窗处理，确定是否存在被剔除的观测帧；其中所述特征分类优化系统的所述滤波器优化模块还用于当存在该被剔除的观测帧时，则根据该双目相机的该优化位姿和该优化速度，对该被剔除的观测帧中的特征点信息进行滤波器优化处理，以得到该双目相机的输出位姿和输出速度；并且当不存在该被剔除的观测帧时，则直接将该双目相机的该优化位姿和该优化速度作为该双目相机的输出位姿和输出速度。
16.如权利要求14或15所述的SLAM定位系统，其中，所述前端系统包括相互可通信地连接的一光流跟踪模块、一极线搜索与块匹配模块以及一判断提取模块，其中所述光流跟踪模块用于通过光流跟踪方法对该原始图像中左目图像的特征点进行跟踪，以得到该当前观测帧中左目的特征点信息；其中所述极线搜索与块匹配模块用于根据该双目相机中的左目相机和右目相机之间的相对位姿，通过极线搜索与块匹配方法对该原始图像中右目图像的特征点进行跟踪，以得到该当前观测帧中右目的特征点信息；其中所述判断提取模块用于判断通过该光流跟踪方法跟踪到的该左目图像的特征点的数量是否小于一特征点数量阈值，如果是，则通过特征点提取方法从该左目图像中提取出新的特征点信息，以补充到该当前观测帧中左目的特征点信息。
17.电子设备，其特征在于，包括：
至少一处理器，用于执行指令；和
与所述至少一处理器可通信地连接的存储器，其中，所述存储器具有至少一指令，其中，所述指令被所述至少一处理器执行，以使得所述至少一处理器执行SLAM定位方法中的部分或全部步骤，其中所述SLAM定位方法包括步骤：
对通过双目相机采集的原始图像进行前端处理，以得到当前观测帧的特征点信息；
对通过惯性测量单元采集的IMU信息进行滤波器预测处理，以得到该双目相机的预测位姿和预测速度；以及
通过特征分类优化方法，基于该当前观测帧的特征点信息和该双目相机的该预测位姿和该预测速度进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；其中所述特征分类优化方法包括步骤：
S110：相对于该当前观测帧的特征点，确定是否存在跟踪丢失的特征点；
S120：当存在该跟踪丢失的特征点时，根据该双目相机的该预测位姿和该预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的该优化位姿和该优化速度；以及
S130：当不存在该跟踪丢失的特征点时，先从该当前观测帧的特征点中筛选多个特征点，再根据该双目相机的该预测位姿和该预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的该优化位姿和该优化速度。
CN201911326304.6A 2019-12-20 2019-12-20 特征分类优化方法和slam定位方法及其系统和电子设备 Active CN113012216B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911326304.6A CN113012216B (zh) 2019-12-20 2019-12-20 特征分类优化方法和slam定位方法及其系统和电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911326304.6A CN113012216B (zh) 2019-12-20 2019-12-20 特征分类优化方法和slam定位方法及其系统和电子设备

Publications (2)

Publication Number Publication Date
CN113012216A CN113012216A (zh) 2021-06-22
CN113012216B true CN113012216B (zh) 2023-07-07

Family

ID=76382690

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911326304.6A Active CN113012216B (zh) 2019-12-20 2019-12-20 特征分类优化方法和slam定位方法及其系统和电子设备

Country Status (1)

Country Link
CN (1) CN113012216B (zh)

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105513092A (zh) * 2015-11-26 2016-04-20 北京理工大学 一种用于目标跟踪的模板特征选择方法
JP2016152027A (ja) * 2015-02-19 2016-08-22 株式会社リコー 画像処理装置、画像処理方法およびプログラム
CN108470354A (zh) * 2018-03-23 2018-08-31 云南大学 视频目标跟踪方法、装置和实现装置
WO2019068222A1 (en) * 2017-10-06 2019-04-11 Qualcomm Incorporated SIMULTANEOUS RELOCATION AND RESETTING OF VSLAM
CN109816696A (zh) * 2019-02-01 2019-05-28 西安全志科技有限公司 一种机器人定位与建图方法、计算机装置及计算机可读存储介质
WO2019157925A1 (zh) * 2018-02-13 2019-08-22 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107747941B (zh) * 2017-09-29 2020-05-15 歌尔股份有限公司 一种双目视觉定位方法、装置及系统

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2016152027A (ja) * 2015-02-19 2016-08-22 株式会社リコー 画像処理装置、画像処理方法およびプログラム
CN105513092A (zh) * 2015-11-26 2016-04-20 北京理工大学 一种用于目标跟踪的模板特征选择方法
WO2019068222A1 (en) * 2017-10-06 2019-04-11 Qualcomm Incorporated SIMULTANEOUS RELOCATION AND RESETTING OF VSLAM
WO2019157925A1 (zh) * 2018-02-13 2019-08-22 视辰信息科技(上海)有限公司 视觉惯性里程计的实现方法及系统
CN108470354A (zh) * 2018-03-23 2018-08-31 云南大学 视频目标跟踪方法、装置和实现装置
CN109816696A (zh) * 2019-02-01 2019-05-28 西安全志科技有限公司 一种机器人定位与建图方法、计算机装置及计算机可读存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight;Ke Sun;《 IEEE Robotics and Automation Letters》;第3卷(第2期);965-972 *

Also Published As

Publication number Publication date
CN113012216A (zh) 2021-06-22

Similar Documents

Publication Publication Date Title
US11816585B2 (en) Machine learning models operating at different frequencies for autonomous vehicles
US11313684B2 (en) Collaborative navigation and mapping
Mancini et al. Toward domain independence for learning-based monocular depth estimation
US9811732B2 (en) Systems and methods for object tracking
JP2019207220A (ja) 動的な交通参加者の除去による位置推定と地図生成の安定した同時実行
BR102017026251A2 (pt) Método e sistema de reconhecimento de dados de sensor utilizando o enriquecimento de dados para o processo de aprendizagem
CN110660102B (zh) 基于人工智能的说话人识别方法及装置、系统
US20190197425A1 (en) Deep convolutional factor analyzer
CN113012054B (zh) 基于抠图的样本增强方法和训练方法及其系统和电子设备
CN113011231B (zh) 分类滑窗方法和slam定位方法及其系统和电子设备
CN115860102B (zh) 一种自动驾驶感知模型的预训练方法、装置、设备和介质
CN106612385B (zh) 视频检测方法和视频检测装置
CN114612774A (zh) 目标检测及其模型构建方法、电子设备和存储介质
CN113129249A (zh) 基于深度视频的空间平面检测方法及其系统和电子设备
CN113095228B (zh) 图像中的目标检测方法、装置及计算机可读存储介质
CN113012216B (zh) 特征分类优化方法和slam定位方法及其系统和电子设备
US20220205803A1 (en) Intelligent object tracing system utilizing 3d map reconstruction for virtual assistance
CN113569912A (zh) 车辆识别方法、装置、电子设备及存储介质
CN112561956A (zh) 视频目标跟踪方法、装置、电子设备及存储介质
WO2020125965A1 (en) Device and method for detecting user activity by parallelized classification
CN114445549A (zh) 基于slam的三维稠密面元建图方法及其系统和电子设备
US11501571B2 (en) Depth processing
CN113129333B (zh) 多目标实时跟踪方法及其系统和电子设备
CN113091767B (zh) 滤波式后端回环校正方法及其系统和电子设备
CN111829533B (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
EE01 Entry into force of recordation of patent licensing contract

Application publication date: 20210622

Assignee: Zhejiang Shunwei Technology Co.,Ltd.

Assignor: SUNNY OPTICAL (ZHEJIANG) RESEARCH INSTITUTE Co.,Ltd.

Contract record no.: X2024330000055

Denomination of invention: Optimization methods for feature classification and SLAM localization, as well as their systems and electronic devices

Granted publication date: 20230707

License type: Common License

Record date: 20240515