特征分类优化方法和SLAM定位方法及其系统和电子设备技术方案

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

【技术实现步骤摘要】
特征分类优化方法和SLAM定位方法及其系统和电子设备
本专利技术涉及SLAM
，尤其是涉及特征分类优化方法和SLAM定位方法及其系统和电子设备。
技术介绍
随着计算技术、智能技术以及传感器技术的不断提高，诸如AR/VR、无人机以及智能机器人等智能应用涌入市场，使得定位与地图构建(SimultaneousLocalizationAndMapping，简称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，包括步骤：通过特征点筛选器，从该当前观测帧的特征点中筛选出左右匹配误差小于预定误差阈值的特征点；和根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。在本专利技术的一实施例中，所述特征点筛选器为其中p1和p2分别为左右匹配特征点的坐标；t为平移量；R为旋转量。在本专利技术的一实施例中，所述预定误差阈值根据该双目相机的内参来设定。在本专利技术的一实施例中，所述步骤S130，包括步骤：计算该当前观测帧中所有的特征点的左右匹配误差；从该当前观测帧中依次筛选出该左右匹配误差最小的特征点，直至该筛选出的特征点的数量达到预定筛选数量为止；以及根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。在本专利技术的一实施例中，所述预定筛选数量为特征点的最大跟踪数的1/10。根据本专利技术的另一方面，本专利技术进一步提供了SLAM定位方法，包括步骤：对通过双目相机采集的原始图像进行前端处理，以得到当前观测帧的特征点信息；对通过惯性测量单元采集的IMU信息进行滤波器预测处理，以得到该双目相机的预测位姿和预测速度；以及通过特征分类优化方法，基于该当前观测帧的特征点信息和该双目相机的该预测位姿和该预测速度进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；其中所述特征分类优化方法包括步骤：S110：相对于该当前观测帧的特征点，确定是否存在跟踪丢失的特征点；S120：当存在该跟踪丢失的特征点时，根据该双目相机的该预测位姿和该预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的该优化位姿和该优化速度；以及S130：当不存在该跟踪丢失的特征点时，先从该当前观测帧的特征点中筛本文档来自技高网...

【技术保护点】
1.特征分类优化方法，其特征在于，包括步骤：/nS110：相对于当前观测帧的特征点，确定是否存在跟踪丢失的特征点；/nS120：当存在该跟踪丢失的特征点时，根据双目相机的预测位姿和预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；以及/nS130：当不存在该跟踪丢失的特征点时，先从该当前观测帧的特征点中筛选多个特征点，再根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。/n

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


2.如权利要求1所述的特征分类优化方法，其中，所述步骤S130，包括步骤：
通过特征点筛选器，从该当前观测帧的特征点中筛选出左右匹配误差小于预定误差阈值的特征点；和
根据该双目相机的预测位姿和预测速度，对该筛选出的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度。


3.如权利要求2所述的特征分类优化方法，其中，所述特征点筛选器为其中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.特征分类优化系统，其特征在于，包括相互可通信地连接的：
一确定模块，用于相对于当前观测帧的特征点，确定是否存在跟踪丢失的特征点；
一滤波器优化模块，用于当存在该跟踪丢失的特征点时，根据双目相机的预测位姿和预测速度，对该跟踪丢失的特征点的信息进行滤波器优化处理，以得到该双目相机的优化位姿和优化速度；以及
一特征点筛选模块，其中所述特征点筛选模块...

【专利技术属性】
技术研发人员：兰国清，周俊，蒋坤君，胡增新，
申请(专利权)人：舜宇光学浙江研究院有限公司，
类型：发明
国别省市：浙江;33