CN108615246A - 提高视觉里程计系统鲁棒性和降低算法计算消耗的方法 - Google Patents

Abstract

本发明公开了一种提高视觉里程计系统鲁棒性和降低算法计算消耗的方法，包括：步骤1：采用视觉里程计系统，通过图像传感器采集图像，获得帧p，初始化MatchBuf_p为空集合；步骤2：判定MatchBuf_p‑1是否为空，如是，设置相机位姿T_wc为0，执行步骤6，如否，执行步骤3；步骤3：计算帧p‑1与帧p之间的相对位姿T_lc；步骤4：计算帧p在地图中的相机位姿T_wc ^p，并构建MatchBuf_p；步骤5：更新FilterBuf，并判定帧p是否为关键帧；步骤6：构建新的关键帧K_new，并加入到Set_K中；步骤7：使用特征匹配初始化新的三维点。本发明基于图像传感器的实时地图构建和自定位系统，该系统在实现精确鲁棒的构图和定位的同时，减小系统计算资源消耗。

Description

提高视觉里程计系统鲁棒性和降低算法计算消耗的方法

技术领域

本发明涉及基于视觉的运动估计技术领域，具体涉及一种提高视觉里程计系统鲁棒性和降低算法计算消耗的方法。

背景技术

VO(Visual Odometry)系统是使用图像传感器作为信息采集工具，进行实时地图构建和自定位的系统。VO是指视觉里程计，视觉里程计VO的目标是根据拍摄的图像估计相机的运动，通过分析一系列图像序列，来确定机器人的朝向和位置。视觉里程计VO主要用于移动机器人的定位导航。

VO系统根据原理的不同具体可以分为以下三类：

(1)直接法。不需要提取特征点，通常使用所有像素点或者像素梯度值大于某一阈值的像素点进行图像间的匹配。该类方法不需要提取图像特征点，以及计算特征点的描述子，因而通常消耗的计算资源较小。然而因为该类方法使用像素灰度值作为图像匹配依据，因而对于环境光照变化鲁棒性差。典型方法如：LSD_SLAM(Large-ScaleDirect MonocularSLAM，基于单目直接法的大场景实时定位和地图构建)，DSO(Direct Sparse Odometry，基于直接法和稀疏点的视觉里程计)等。

(2)半直接法。该类方法需要提取特征点，不需要计算特征点的描述子。通常使用快速的特征点提取算法，例如FAST(Features from Accelerated Segment Test)算子。使用以特征点为中心的N*N像素块进行图像匹配。鲁棒性相对于直接法有一些提高，由于特征点的数量较少，因而计算效率相比于直接法也有一些提高。典型方法如：SVO(Semi-DirectVisual Odometry，基于半直接法的视觉里程计)等。

(3)间接法(即特征点法)。该类方法针对每一幅图像提取特征点，并针对每个特征点计算描述子。以描述子的相似度作为特征点匹配的依据，完成图像间的匹配。该类方法计算效率底，然而对于环境变化的鲁棒性最好。该类方法虽然计算效率低，然而鲁棒性好，所以目前应用较广泛。典型方法如：ORB_SLAM(基于ORB(Oriented FAST and Rotated BRIEF)特征的视觉定位和地图构建)等。

为了节省计算资源，同时获得较好的鲁棒性，本文所述方法将半直接法与间接法结合，完成构建基于关键帧和三维点的拓扑地图，从而同时节约计算资源，并获得更好的鲁棒性。

发明内容

本发明所要解决的技术问题是：针对现有的VO方法存在的问题，提出半直接法与间接法融合的VO系统，提供了一种提高视觉里程计系统鲁棒性和降低算法计算消耗的方法，基于图像传感器(单目，双目或者多目，目是指VO系统中的相机个数，例如单目是指VO系统中只有一个相机)的实时地图构建和自定位系统，该系统在实现精确鲁棒的构图和定位的同时，减小系统计算资源消耗。

本发明所述地图包括关键帧集合与三维点集合 N_k，N_M分别表示两个集合的元素数目。K₁表示关键帧集合Set_K中第1个关键帧，表示关键帧集合Set_K中第N_k个关键帧，K_ik表示关键帧集合Set_K中第ik个关键帧，关键帧K_ik包含{I,T_wc,Set_F}，包含ik时刻相机采集到的图像I，相机位姿T_wc(包含3×3旋转矩阵R_wc和3×1平移矩阵t_wc)和具有N_f个元素的特征集合F_i＝{x_i,y_i,s_i,D_i,m_i}，其中x_i,y_i表示第i个特征点在图像上的坐标，s_i表示该特征点的评分值，由特征点提取算法计算得到，D_i表示该点的描述子，由描述子计算算法得到，m_i∈{Set_M，-1}表示特征点F_i对应的三维点，m_i的取值为-1或者M∈Set_M，若m_i为-1，表示F_i在集合Set_M中不存在对应的三维点。M₁表示三维点集合Set_M中第1个三维点，M_im表示三维点集合Set_M中第im个三维点，表示三维点集合Set_M中第N_M个三维点。三维点M_im，包含{X,Y,Z,Set_Obs}，具有空间位置信息(X,Y,Z)和观测集合O_i＝{k_i,t_i}，k_i∈Set_K，O₀＝{k₀,t₀}，k₀∈Set_K，k₀为生成三维点M_im的关键帧，为生成三维点M_im的特征点，t₀为M_im在关键帧k₀中对应的特征标号，t_i为M_im在关键帧k_i中对应的特征标号，即表示M_im对应于中的第t_i个特征。所述生成是指已知k₀位姿坐标和深度由相机投影关系可以得到M_im的空间位置(X_im,Y_im,Z_im)，进而初始化三维点M_im。基于Graph的VO地图表示方法通常采用上述模式，在此不再赘述。

本发明所示MatchBuf_p为MatchBuf_p下标p表示图像序列标号，其中，Buf包含信息(x,y,m,D)，m∈Set_M，已知生成m的特征点为F={x^F,y^F,s^F,D^F,m^F}，则m即为m^F，D为D^F，x,y表示三维点m在帧p中的坐标。Buf_i包含信息(x_i,y_i,m_i,D_i)，m_i为集合Set_M中的任意元素，x_i,y_i表示三维点m_i在帧p中的坐标，m_i表示三维点，D_i表示三维点m_i的描述子。

本发明所示FilterBuf为DFilter_i即为SVO中的深度滤波器，在此不再赘述。SVO论文为现有技术，具体可参照文献“基于半直接法的单目视觉里程计”（C.Forster,M.Pizzoli,and D.Scaramuzza.SVO:Fast semidirectmonocular visual odometry.In International Conference on Robotics andAutomation(ICRA),2014.）。

本发明采用的技术方案如下：

一种提高视觉里程计系统鲁棒性和降低算法计算消耗的方法，包括以下步骤：

步骤1：采用视觉里程计系统，通过图像传感器采集图像，获得帧p，初始化MatchBuf_p为空集合，MatchBuf_p下标p表示图像序列标号；

步骤1中，图像传感器采用单目、双目或多目；

MatchBuf_p为MatchBuf_p下标p表示图像序列标号，其中，Buf_i包含信息(x_i,y_i,m_i,D_i)，m_i为三维点集合Set_M中的任意元素，x_i,y_i表示三维点m_i在帧p中的坐标，m_i表示三维点，D_i表示三维点m_i的描述子；

步骤2：判定MatchBuf_p-1是否为空，如是，设置相机位姿T_wc为0，执行步骤6；如否，执行步骤3；

步骤3：计算帧p-1与帧p之间的相对位姿T_lc；

设置T_lc初始值为0，使用Lucas-Kanade光流算法，迭代优化T_lc，使得MatchBuf_p-1中的点投影到帧p中得到的灰度值误差最小，从而得到相对位姿T_lc；

步骤3中，Lucas-Kanade光流算法可采用现有技术。

步骤4：计算帧p在地图中的相机位姿T_wc ^p，并构建MatchBuf_p；

4.1由帧p-1在地图中的相机位姿T_wc ^p-1，以及步骤3中得到的帧p-1与帧p之间的相对位姿T_lc，得到帧p在地图中的相机位姿T_wc ^p：

T_wc ^p=T_wc ^p-1*T_lc

4.2对于M_i∈Set_M，判定M_i在帧p中是否可见，将M_i投影到帧p中，得到M_i在帧p中的投影坐标(x^p,y^p)，若(x^p,y^p)在帧p对应的图像I_p区域内，构建M_i在帧p中的Buf_i，生成M_i的关键帧为对应的特征为 表示M_i在图像上的坐标，表示M_i评分值，由特征点提取算法计算得到，表示M_i点的特征描述子，由描述子计算算法得到，表示对应的三维点，若为-1，表示在Set_M不存在对应的三维点；以像素为中心，提取n×n像素块，在帧p中，搜索(x_min,y_min)，使得|x_min-x^p|+|y_min-y^p|<ε，ε为设定值，并且灰度差I_Erro_r最小，如果I_Erro_r小于设定值Thres，则将加入到MatchBuf_p。

步骤5：更新FilterBuf，并判定帧p是否为关键帧。

步骤5.1：已知帧p的位姿T_wc ^p和帧p的图像I_p，使用基于半直接法的视觉里程计中（SVO:Fast Semi-Direct Monocular Visual Odometry，V.MAPPING部分）深度滤波器更新算法更新FilterBuf，该部分包含将收敛后的三维点加入到Set_M，以及删除无效的DFilter_i。

步骤5.2：判定帧p是否为关键帧。

即计算最新的关键帧K_new的位姿与当前帧p的位姿t_wc ^P之间的位姿关系。如果大于阈值dist_thres，则帧p为关键帧，继续执行下列步骤，即执行步骤6，否则返回步骤1。norm是一个函数，用于计算向量的模。

步骤6：构建新的关键帧K_new，并加入到Set_K中。

6.1提取该帧p图像的特征点和对应的描述子，本发明以FAST（Features fromAccelerated Segment Test）特征点提取算法为例，以BRIEF（BRIEF:Binary RobustIndependent Elementary Features）描述子为例，针对帧p提取相应的点和描述子。所有帧p的特征点与对应的描述子形成集合Set_F。对于任意F_i∈Set_F，初始m_i在集合Set_M中无对应点，设置为-1，得到关键帧K_new，并加入到Set_K中。

特征集合F_i={x_i,y_i,s_i,D_i,m_i}，其中x_i,y_i表示第i个特征点在图像上的坐标，s_i表示该特征点的评分值，由特征点提取算法计算得到，D_i表示该点的描述子，由描述子计算算法得到，m_i∈{Set_M，-1}表示特征点F_i对应的三维点，m_i的取值为-1或者M∈Set_M，若m_i为-1，表示F_i在集合Set_M中不存在对应的三维点；

6.2更新MatchBuf_p，所述的MatchBuf_p为N_buf为Set_Buf中元素的个数，Buf_i包含信息(x_i,y_i,m_i,D_i)，m_i为集合Set_M中的任意元素，x_i,y_i表示三维点m_i在帧p中的坐标，m_i表示三维点，D_i表示三维点m_i的描述子；

对于Buf_i∈MatchBuf_p，遍历F_i∈Set_F，在Set_F中寻找特征F_min，满足|x_i-x_min|+|y_i-y_min|≤r，其中r为给定阈值，且Dis(D_i-D_min)最小，其中Dis()用于度量两个描述子之间的相似程度，返回数值越小表示两个描述子越接近，D_i为所述Buf_i中的描述子，D_min表示F_min对应的描述子。若F_min可以找到，则更新Buf_i，x_i=x_min,y_i=y_min，D_i=D_min。并将F_min对应的m_min设置为m_i；

6.3构建新的深度滤波器。

6.3.1对于任意F_i∈Set_F,若m_i≠-1表示特征F_i已经匹配m_i∈Set_M，则i=i+1，返回步骤6.3.1，否则使用基于半直接法的视觉里程计（SVO:Fast Semi-Direct MonocularVisual Odometry，V.MAPPING部分）方法，针对特征F_i初始化DFilter_i,并将DFilter_i加入到Set_Filter中。

步骤7：使用特征匹配初始化新的三维点。

使用ORB_SLAM及ORB_SLAM2中的方法，将新的关键帧K_new与临近的关键帧使用特征描述子进行匹配，并使用三角测量生成新的三维点m_i，并将m_i添加到Set_M中，返回步骤1。

SVO可采用现有技术，具体可参照文献“基于半直接法的单目视觉里程计”（C.Forster,M.Pizzoli,and D.Scaramuzza.SVO:Fast semidirect monocular visualodometry.In International Conference on Robotics and Automation(ICRA),2014.）。

ORB_SLAM可采用现有技术，如具体可参照文献“ORB_SLAM:一种通用高精度单目视觉SLAM系统”（R.Mur-Artal,J.M.M.Montiel,and J.D.Tardos,“ORB-SLAM:a versatileand accurate monocular SLAM system,”IEEE Transactionson Robotics,vol.31,no.5,pp.1147–1163,2015.）。

ORB_SLAM2可采用现有技术，如具体可参照文献“ORB_SLAM2:一种可用于单目，多目和RGB-D相机的开源视觉SLAM系统”（R.Mur-Artal and J.D.Tardos,“ORB-SLAM2:anOpen-Source SLAM System for Monocular,Stereo and RGB-D Cameras,”IEEETransactionson Robotics,vol.33,no.5,pp.1255–1262,2017.）。

综上所示，由于采用了上述技术方案，本发明的有益效果是：

1、完成了基于视觉的视觉里程计系统，可以通过相机视野的变换得到相机位姿变化。

2、融合了半直接法和间接法，在降低计算资源消耗的同时提升系统的鲁棒性。

3、具有广泛的兼容性。可用于单目，双目，多目以及深度相机系统。

附图说明

图1为本发明中提高视觉里程计系统鲁棒性和降低算法计算消耗的方法的流程示意图。

具体实施方式

本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任意方式组合。

实施例一：

本实施例以机器人车载双目系统为例，并假定该双目系统以及双目系统和机器人之间已经进行标定。打开本系统，并使机器人保持运动状态。

本发明所述地图包括关键帧集合与三维点集合 N_k，N_M分别表示两个集合的元素数目。K₁表示关键帧集合Set_K中第1个关键帧，表示关键帧集合Set_K中第N_k个关键帧，K_ik表示关键帧集合Set_K中第ik个关键帧，关键帧K_ik包含{I,T_wc,Set_F}，包含ik时刻相机采集到的图像I，相机位姿T_wc（包含3×3旋转矩阵R_wc和3×1平移矩阵t_wc）和具有N_f个元素的特征集合F_i＝{x_i,y_i,s_i,D_i,m_i}，其中x_i,y_i表示第i个特征点在图像上的坐标，s_i表示该特征点的评分值，由特征点提取算法计算得到，D_i表示该点的描述子，由描述子计算算法得到，m_i∈{Set_M，-1}表示特征点F_i对应的三维点，m_i的取值为-1或者M∈Set_M，若m_i为-1，表示F_i在集合Set_M中不存在对应的三维点。M₁表示三维点集合Set_M中第1个三维点，M_im表示三维点集合Set_M中第im个三维点，表示三维点集合Set_M中第N_M个三维点。三维点M_im，包含{X,Y,Z,Set_Obs}，具有空间位置信息(X,Y,Z)和观测集合O_i＝{k_i,t_i}，k_i∈Set_K，O₀＝{k₀,t₀}，k₀∈Set_K，k₀为生成三维点M_im的关键帧，为生成三维点M_im的特征点，t₀为M_im在关键帧k₀中对应的特征标号，t_i为M_im在关键帧k_i中对应的特征标号，即表示M_im对应于中的第t_i个特征。所述生成是指已知k₀位姿坐标和深度由相机投影关系可以得到M_im的空间位置(X_im,Y_im,Z_im)，进而初始化三维点M_im。基于Graph的VO地图表示方法通常采用上述模式，在此不再赘述。

本发明所示MatchBuf_p为MatchBuf_p下标p表示图像序列标号，其中，Buf包含信息(x,y,m,D)，m∈Set_M，已知生成m的特征点为F＝{x^F,y^F,s^F,D^F,m^F}，则m即为m^F，D为D^F，x,y表示三维点m在帧p中的坐标。Buf_i包含信息(x_i,y_i,m_i,D_i)，m_i为集合Set_M中的任意元素，x_i,y_i表示三维点m_i在帧p中的坐标，m_i表示三维点，D_i表示三维点m_i的描述子。

本发明所示FilterBuf为DFilter_i即为SVO中的深度滤波器，在此不再赘述。

如图1所示，一种提高视觉里程计系统鲁棒性和降低算法计算消耗的方法，包括以下步骤：

步骤1：采用视觉里程计系统，通过图像传感器采集图像，获得帧p，初始化MatchBuf_p为空集合，MatchBuf_p下标p表示图像序列标号；

步骤1中，图像传感器采用单目、双目或多目；

MatchBuf_p为MatchBuf_p下标p表示图像序列标号，其中，Buf_i包含信息(x_i,y_i,m_i,D_i)，m_i为三维点集合Set_M中的任意元素，x_i,y_i表示三维点m_i在帧p中的坐标，m_i表示三维点，D_i表示三维点m_i的描述子；

步骤2：判定MatchBuf_p-1是否为空，如是，设置相机位姿T_wc为0，执行步骤6；如否，执行步骤3；

步骤3：计算帧p-1与帧p之间的相对位姿T_lc；

设置T_lc初始值为0，使用Lucas-Kanade光流算法，迭代优化T_lc，使得MatchBuf_p-1中的点投影到帧p中得到的灰度值误差最小，从而得到相对位姿T_lc；

步骤3中，Lucas-Kanade光流算法可采用现有技术。

步骤4：计算帧p在地图中的相机位姿T_wc ^p，并构建MatchBuf_p；

4.1由帧p-1在地图中的相机位姿T_wc ^p-1，以及步骤3中得到的帧p-1与帧p之间的相对位姿T_lc，得到帧p在地图中的相机位姿T_wc ^p：

T_wc ^p=T_wc ^p-¹*T_lc

4.2对于M_i∈Set_M，判定M_i在帧p中是否可见，将M_i投影到帧p中，得到M_i在帧p中的投影坐标(x^p,y^p)，若(x^p,y^p)在图像I_p区域内，构建M_i在帧p中的Buf，生成M_i的关键帧为对应的特征为 表示M_i在图像上的坐标，表示M_i评分值，由特征点提取算法计算得到，表示M_i点的特征描述子，由描述子计算算法得到， 表示对应的三维点，若为-1，表示在Set_M不存在对应的三维点；以像素为中心，提取n×n像素块，在帧p中，搜索(x_min,y_min)，使得|x_min-x^p|+|y_min-y^p|<ε，并且灰度差I_Error最小，如果I_Error小于设定值Thres，则将加入到MatchBuf_p。

步骤5：更新FilterBuf，并判定帧p是否为关键帧。

步骤5.1：已知帧p的位姿T_wc ^p和帧p的图像，使用基于半直接法的视觉里程计中（SVO:Fast Semi-Direct Monocular Visual Odometry，V.MAPPING部分）深度滤波器更新算法更新FilterBuf，该部分包含将收敛后的三维点加入到Set_M，以及删除无效的DFilter_i。

步骤5.2：判定帧p是否为关键帧。

即计算最新的关键帧K_new的位姿与当前帧的位姿p t_wc ^P之间的位姿关系。如果大于阈值dist_thres，则帧p为关键帧，继续执行下列步骤。否则返回步骤1。

步骤6：构建新的关键帧K_new，并加入到Set_K中。

6.1提取该帧p图像的特征点和对应的描述子，本发明以FAST特征点提取算法为例，以BRIEF(BRIEF:Binary Robust Independent Elementar yFeatures)描述子为例，针对帧p提取相应的点和描述子。所有帧p的特征点与对应的描述子形成集合Set_F。对于任意F_i∈Set_F，初始m_i在集合Set_M中无对应点，设置为-1。

6.2更新MatchBuf_p，所述的MatchBuf_p为N_buf为Set_Buf中元素的个数，Buf_i包含信息(x_i,y_i,m_i,D_i)，m_i为集合Set_M中的任意元素，x_i,y_i表示三维点m_i在帧p中的坐标，m_i表示三维点，D_i表示三维点m_i的描述子；

对于Buf_i∈MatchBuf_p，遍历F_j∈Set_F，在Set_F中寻找特征F_min，满足|x_i-x_min|+|y_i-y_min|≤r，其中r为给定阈值，且Dis(D_i-D_min)最小，其中Dis()用于度量两个描述子之间的相似程度，返回数值越小表示两个描述子越接近，D_i为所述Buf_i中的描述子，D_min表示F_min对应的描述子。若F_min可以找到，则更新Buf_i，x_i＝x_min,y_i＝y_min，D_i＝D_min。并将F_min对应的m_min设置为m_i。

6.3构建新的深度滤波器。

6.3.1对于任意F_i∈Set_F,若m_i≠-1表示特征F_i已经匹配m_i∈Set_M，则i＝i+1，返回步骤6.3.1，否则使用基于半直接法的视觉里程计(SVO:Fast Semi-Direct MonocularVisual Odometry，V.MAPPING部分)方法，针对特征F_i初始化DFilter_i,并将DFilter_i加入到Set_Filter中。

步骤7：使用特征匹配初始化新的三维点。

使用ORB_SLAM及ORB_SLAM2中的方法，将新的关键帧K_new与临近的关键帧使用特征描述子进行匹配，并使用三角测量生成新的三维点m_i，并将m_i添加到Set_M中，返回步骤1。

SVO可采用现有技术，具体可参照文献“基于半直接法的单目视觉里程计”(C.Forster,M.Pizzoli,and D.Scaramuzza.SVO:Fast semidirect monocular visualodometry.In International Conference on Robotics and Automation(ICRA),2014.)。

ORB_SLAM可采用现有技术，如具体可参照文献“ORB_SLAM:一种通用高精度单目视觉SLAM系统”(R.Mur-Artal,J.M.M.Montiel,and J.D.Tardos,“ORB-SLAM:a versatileand accurate monocular SLAM system,”IEEE Transactionson Robotics,vol.31,no.5,pp.1147–1163,2015.)。

ORB_SLAM2可采用现有技术，如具体可参照文献“ORB_SLAM2:一种可用于单目，多目和RGB-D相机的开源视觉SLAM系统”（R.Mur-Artal and J.D.Tardos,“ORB-SLAM2:anOpen-Source SLAM System for Monocular,Stereo and RGB-D Cameras,”IEEETransactionson Robotics,vol.33,no.5,pp.1255–1262,2017.）。

Claims (5)

1.一种提高视觉里程计系统鲁棒性和降低算法计算消耗的方法，其特征在于，包括以下步骤：

步骤1：采用视觉里程计系统，通过图像传感器采集图像，获得帧p，初始化MatchBuf_p为空集合，MatchBuf_p下标p表示图像序列标号；

步骤2：判定MatchBuf_p-1是否为空，如是，设置相机位姿T_wc为0，执行步骤6，如否，执行步骤3；

步骤3：计算帧p-1与帧p之间的相对位姿T_lc；

设置T_lc初始值为0，使用Lucas-Kanade光流算法，迭代优化T_lc，使得MatchBuf_p-1中的点投影到帧p中得到的灰度值误差最小，从而得到相对位姿T_lc；

步骤4：计算帧p在地图中的相机位姿T_wc ^p，并构建MatchBuf_p；

步骤5：更新FilterBuf，并判定帧p是否为关键帧；

步骤5.1：已知帧p的位姿T_wc ^p和帧p的图像，使用基于半直接法的视觉里程计中深度滤波器更新算法更新FilterBuf，该部分包含将收敛后的三维点加入到Set_M，以及删除无效的DFilter_i；

步骤5.2：判定帧p是否为关键帧；

即计算最新的关键帧K_new的位姿与当前帧p的位姿t_wc ^P之间的位姿关系，如果大于阈值dist_thres，则帧p为关键帧，执行步骤6，否则返回步骤1；

步骤6：构建新的关键帧K_new，并加入到Set_K中；

6.1提取该帧p图像的特征点和对应的描述子，所有帧p的特征点与对应的描述子形成集合Set_F，对于任意F_i∈Set_F，初始m_i在集合Set_M中无对应点，设置为-1，得到关键帧K_new，并加入到Set_K中；

6.2更新MatchBuf_p，所述的MatchBuf_p为Set_Buf N_buf为Set_Buf中元素的个数，Buf_i包含信息(x_i,y_i,m_i,D_i)，m_i为集合Set_M中的任意元素，x_i,y_i表示三维点m_i在帧p中的坐标，m_i表示三维点，D_i表示三维点m_i的描述子；

对于Buf_i∈MatchBuf_p，遍历F_i∈Set_F，在Set_F中寻找特征F_min，满足|x_i-x_min|+|y_i-y_min|≤r，其中r为给定阈值，且Dis(D_i-D_min)最小，其中Dis()用于度量两个描述子之间的相似程度，返回数值越小表示两个描述子越接近，D_i为所述Buf_i中的描述子，D_min表示F_min对应的描述子，若F_min可以找到，则更新Buf_i，x_i＝x_min,y_i＝y_min，D_i＝D_min，并将F_min对应的m_min设置为m_i；

6.3构建新的深度滤波器；

6.3.1对于任意F_i∈Set_F,若m_i≠-1表示特征F_i已经匹配m_i∈Set_M，则i＝i+1，返回步骤6.3.1，否则使用基于半直接法的视觉里程计方法，针对特征F_i初始化DFilter_i，并将DFilter_i加入到Set_Filter中；

步骤7：使用特征匹配初始化新的三维点；

将新的关键帧K_new与临近的关键帧使用特征描述子进行匹配，并使用三角测量生成新的三维点m_i，并将m_i添加到Set_M中，返回步骤1。

2.根据权利要求1所述的方法，其特征在于，步骤1中，图像传感器采用单目、双目或多目。

3.根据权利要求1所述的方法，其特征在于，步骤1中，MatchBuf_p为Set_Buf MatchBuf_p下标p表示图像序列标号，其中，Buf_i包含信息(x_i,y_i,m_i,D_i)，m_i为三维点集合Set_M中的任意元素，x_i,y_i表示三维点m_i在帧p中的坐标，m_i表示三维点，D_i表示三维点m_i的描述子。

4.根据权利要求1所述的方法，其特征在于，步骤4中，具体包括：

4.1由帧p-1在地图中的相机位姿T_wc ^p-1，以及步骤3中得到的帧p-1与帧p之间的相对位姿T_lc，得到帧p在地图中的相机位姿T_wc ^p：

T_wc ^p＝T_wc ^p-1*T_lc

4.2对于M_i∈Set_M，判定M_i在帧p中是否可见，将M_i投影到帧p中，得到M_i在帧p中的投影坐标(x^p,y^p)，若(x^p,y^p)在图像I_p区域内，构建M_i在帧p中的Buf，生成M_i的关键帧为对应的特征为 表示M_i在图像上的坐标，表示M_i评分值，由特征点提取算法计算得到，表示M_i点的特征描述子，由描述子计算算法得到， 表示对应的三维点，若为-1，表示在Set_M不存在对应的三维点；以像素为中心，提取n×n像素块，在帧p中，搜索(x_min,y_min)，使得|x_min-x^p|+|y_min-y^p|<ε，并且灰度差I_Error最小，如果I_Error小于设定值Thres，则将加入到MatchBuf_p。

5.根据权利要求1所述的方法，其特征在于，步骤6.1中，采用FAST特征点提取算法提取该帧p图像的特征点，采用以BRIEF描述子提取对应的描述子。