CN107610175A - 基于半直接法和滑动窗口优化的单目视觉slam算法 - Google Patents

Abstract

本发明公开了一种基于半直接法和滑动窗口优化的单目视觉SLAM算法，包括步骤：1)通过第三方的图像采集接口，将单目彩色摄像机采集到的彩色图像帧上传到电脑；2)进行算法初始化过程，建立初始的两帧之间的相机位姿变换关系，并且建立初始的地图点，同时将初始的两帧都作为关键帧，并将初始的地图点插入到地图与滑动窗口中；3)将前一帧观测到的地图点投影到当前帧中，并对两帧图像之间进行基于光度误差的捆集优化计算，得到两帧之间相机的姿态变换来跟踪相机的运动。本发明具有更快的运行速度，使用设备简单并且易于标定，具有较大的实用价值和更加广泛的应用场景。

Description

基于半直接法和滑动窗口优化的单目视觉SLAM算法

技术领域

本发明涉及计算机视觉中的同时定位与建图(SLAM)算法的技术领域，尤其是指一种基于半直接法和滑动窗口优化的单目视觉SLAM算法。

背景技术

随着机器人、自动驾驶、增强现实与虚拟现实等新兴科技领域在全世界范围内的蓬勃发展，其背后的关键技术同时定位与建图(SLAM)技术也受到了越来越多来自科研界与工业界的关注。同时定位与建图算法指的是，在完全未知的环境中，对运动着的机器人或传感器载体，能够不断地跟踪其运动的轨迹，同时建立起对环境地图的三维感知。上述的领域都对SLAM技术具有很高的技术要求，一个运行稳定，运算结果准确并且运算速度可以保证实时性的SLAM算法是整个应用系统高效稳定运行的基石，对整体系统的性能有着至关重要的影响。

现有的主流SLAM算法按照传感器的种类可划分为基于激光发射器的激光类型，以及基于彩色摄像头或深度摄像头或两者结合等视觉类这两大类。本发明主要针对的是视觉领域的SLAM算法。另外，根据SLAM算法采用的对环境的描述方式的不同，SLAM算法又可分为基于稀疏分布的特征点并计算其描述子的稀疏法(以ORB-SLAM为代表)、基于全图像素信息的稠密法(以Kinect Fusion为代表)，基于梯度信息明显的像素集合信息的直接法(以LSD-SLAM为代表)，以及基于特征点但不计算描述子的半直接法(以SVO为代表)这几种。另外，不同的方法往往采用不同的用以消除SLAM算法运行中的累积误差的优化策略，其对SLAM算法的准确性和高效性起到非常关键的作用。

上述的各种类型的方法各有各自的优点和缺点。将这些方法中的优点提取出来并加以融合，构造出运行速度快、计算准确、优化方式效率高的SLAM算法，具有非常大的研究意义和实用价值。

发明内容

本发明的目的在于克服现有单目视觉SLAM算法中运算速度与准确性难以同时兼顾的问题，提供了一种基于半直接法和滑动窗口优化的单目视觉SLAM算法，该算法只利用特征点像素亮度信息而不计算特征描述子的半直接法对运动相机进行跟踪，相比计算特征描述子的基于特征点的SLAM方法该算法避免了花费大量计算时间提取特征点描述子的缺点，具有更快的运行速度。该算法同时使用了基于滑动窗口的后端优化策略，固定了后端相机姿态优化的计算复杂度，克服了现有的SLAM算法由于算法运行过程中不断生成的包括相机姿态与地图点在内的待优化数据的不断累积而拖慢优化算法计算速度的缺点，保证了算法的始终高效运行和实时性。另外，该算法仅仅使用单个彩色摄像头，设备简单并且易于标定，相比视觉SLAM领域中常见使用的价格相对昂贵、普及率不高的深度摄像头，以及装置安装固定相对复杂、标定过程相对繁琐的双目摄像头装置，具有较大的实用价值和更加广泛的应用场景。

为实现上述目的，本发明所提供的技术方案为：基于半直接法和滑动窗口优化的单目视觉SLAM算法，包括以下步骤：

1)通过第三方的图像采集接口，将单目彩色摄像机采集到的彩色图像帧上传到电脑；

2)进行算法初始化过程，建立初始的两帧之间的相机位姿变换关系，并且建立初始的地图点，同时将初始的两帧都作为关键帧，并将初始的地图点插入到地图与滑动窗口中；

3)将前一帧观测到的地图点投影到当前帧中，并对两帧图像之间进行基于光度误差的捆集优化计算，得到两帧之间相机的姿态变换来跟踪相机的运动；在跟踪的过程中，根据设定的两个条件决定是否将当前帧作为新的关键帧，包括：①当前帧跟踪到的特征点与参考帧相比少于设定的比例；②设当前帧与几何距离上最相近的关键帧的几何距离为d_min，当前帧与其观测到的地图点的景深中值为d_mean，d_min/d_mean＜η，η为可调的阈值；若决定将当前帧作为关键帧，则需对当前帧检测新的特征点，并将这些点作为候选地图点，然后利用基于概率分布的深度滤波器算法对候选点的深度进行概率收敛；若决定生成新的关键帧，则需要同时进行以下两个步骤：

a、将该关键帧插入到滑动窗口的最前面，对滑动窗口中的关键帧进行变量边缘化的计算以及使用捆集优化算法优化得到校正的位姿，并使用校正后的位姿对误差累积的造成的漂移进行校正；

b、对该关键帧提取特征点和描述子，并插入到基于词袋模型的关键帧数据库中，同时检索关键帧数据库，进行图像相似性匹配，以判定是否检测到回环。

所述步骤2)包括以下步骤：

2.1)基于摄像机初始化时对准的是平面物体的假设，对初始的两帧中的第一帧进行FAST特征点的提取，保存下这些特征点的图像坐标，并使用光流法在初始的第二帧中，对第一帧的提取的特征点进行跟踪，获得第一帧中跟踪到的特征点在第二帧中的图像坐标，从而建立起第一帧与第二帧的匹配点对，并对这些点进行三角化得到三维的初始地图点；

2.2)根据步骤2.1)获得的匹配点对，计算第一帧与第二帧之间的单应性矩阵，对根据重投影误差是否小于某个阈值而从匹配点钟筛选出正确的内点，之后根据这些内点对之前获得的单应性矩阵进行矩阵分解计算获得姿态变换矩阵结果，将对应着最多内点数目的姿态变换矩阵作为最佳的姿态变换矩阵，以此来作为最终的两帧之间的姿态变换矩阵；

2.3)根据步骤2.2)得到的姿态变换进一步筛选内点：若此时内点数目小于某个阈值，则判定初始化失败，继续读入下一帧并返回步骤2.1)，若此时内点数目大于某个阈值，则认为初始化成功，此时将经过筛选后的内点所对应的三维地图点作为初始的地图点插入到地图中，并将初始化阶段的两帧作为关键帧，并插入到滑动窗口中。

所述步骤3)包括以下步骤：

3.1)根据匀速运动模型，初步预测新的一帧的位姿变换，将上一帧观测到的地图点使用初步预测的位姿变换矩阵投影到当前帧中；

3.2)对前一帧观测到的地图点的坐标点像素值与该点投影到当前帧的坐标点像素值构造基于光度误差的优化方程，并利用高斯牛顿法进行优化，得到两帧之间的姿态变换矩阵；

3.3)将上一关键帧作为参考帧，利用步骤3.2)计算得到的姿态变换矩阵，反推计算得到当前帧与参考帧之间的仿射变换矩阵；对于每个在当前帧成功投影的地图点所对应的特征点所在当前帧中的像素坐标，利用仿射变换矩阵，构造出该特征点在参考帧中的像素坐标，并构造基于光度误差的优化方程，优化求解出该特征点所在当前帧中更精确的特征点位置；

3.4)对步骤3.2)与步骤3.3)优化得到的位姿与观测量，再次进行捆集优化，得到精确的两帧之间的姿态变换矩阵，并以此计算出当前帧相对全局坐标系下的姿态变换矩阵T_kw；

3.5)若当前的帧满足成为关键帧的条件，则将当前帧作为关键帧，对当前帧中检测新的FAST特征点，并将这些特征点作为候选的地图点，在之后不断获取的图像帧中寻找到的对应点作为观测量，使用概率深度滤波器进行深度估计直至深度达到收敛；其中，使用概率深度滤波器进行深度收敛包括以下步骤：

3.5.1)假设候选地图点的深度服从高斯概率分布，并根据当前地图点的最小深度将所有候选地图点的概率均值和方差统一初始化；

3.5.2)仍然将最近的那一帧的关键帧作为当前帧所对应的参考帧，根据不断得到每一帧中使用极线约束进行匹配点搜索，在匹配点搜索成功后，三角化得到更加精确的深度，并更新该点的高斯概率分布的参数；

3.5.3)检测当前候选地图点与极线搜索检测到的对应点的定位误差，若该定位误差小于一个像素的偏差，则认为该点对应深度已经收敛，将其作为新的地图点插入到地图当中，否则，继续读入新的图像帧，继续根据步骤3.1)-3.5)得到的姿态变换矩阵T_kw与新的观测数据继续进行深度估计直至收敛。

在步骤3)中，所述步骤a包括以下步骤：

ⅰ、将该关键帧插入到滑动窗口的最前端，并判断：

①若此时滑动窗口中关键帧的数目小于N_KF，其中N_KF为可调的阈值，则直接对滑动窗口中的所有关键帧对应的姿态变量与观测到的地图点作为待优化变量，构造基于重投影误差的捆集优化方程，利用莱文伯格-马夸特(Levenberg-Marquardt)算法对上述优化变量进行迭代优化；

②若此时滑动窗口中关键帧的数目等于N_KF，则需要对关键帧中的姿态变量与观测到的地图点变量进行边缘化，并保留滑动窗口中最近插入的两帧，对其余的关键帧，计算距离得分值s(I_i)，其中I_i为滑动窗口中第i个关键帧，d(i,j)为第i帧与第j帧的欧氏距离，ε为可调的参数；选出s(I_i)最小的那一个关键帧，利用舒尔补(schur component)算法对该关键帧对应的地图点变量与姿态变量进行边缘化，然后结合边缘化后的信息，将滑动窗口中的其他的关键帧对应的姿态变量与观测到的地图点作为待优化变量，构造基于重投影误差的捆集优化方程，利用莱文伯格-马夸特(Levenberg-Marquardt)算法对上述优化变量进行迭代优化；

ⅱ、使用优化后的变量值，对优化前的变量值进行更新。

在步骤3)中，所述步骤b包括以下步骤：

ⅰ、对新生成的关键帧，提取特征点并对特征点提取ORB描述子，并插入到基于词袋模型的关键帧数据库中，并将新来的当前关键帧加入到位姿图模型中；

ⅱ、检索关键帧数据库，进行图像相似性匹配，找到若干数目的在位姿图模型中与当前关键帧不相邻的并相隔设定结点数目的，但图像特征最相近的关键帧加入到可能出现回环的候选回环帧集合；

ⅲ、对上述候选回环帧中已经与地图点相对应了的特征点与在当前关键帧中已经与地图点相对应了的特征点进行特征匹配，建立3D点与3D点之间的匹配关系，从而使用这些匹配关系来计算它们与当前关键帧之间的相似性变换，若得到的相似性变换得到所需数量的内点支持，则认为出现回环，否则，继续终止检测，等待下一个关键帧的到来，再重复步骤ⅰ至ⅲ；

ⅳ、若检测到出现回环，则利用该步骤ⅲ计算得到的相似性变换恢复出尺度信息，利用尺度信息，对位姿图模型中回环帧对应的结点到当前关键帧对应的结点进行尺度信息的校正，校正相机运动过程中由于误差累积所造成的尺度漂移，从而校正得到真正准确的相机运动轨迹。

本发明与现有技术相比，具有如下优点与有益效果：

1、本发明使用了基于特征点的稀疏直接法的方法，直接利用特征点的像素级的颜色信息，相比起特征点法省去了提取特征描述子这步比较耗时的操作，另外对比基于全图像素的稠密直接法也大大的减少了计算量，使本发明的算法相比其他现有的算法得以大大地提速。

2、本发明使用了滑动窗口优化的策略，将待优化的状态量限制在了一个固定的范围内，同时利用边缘化(Marginalization)算法保留了被边缘化的状态变量的信息，使该状态变量虽然在被排除在优化之外的同时也能在优化过程中保留其信息，相比起现有的优化规模不可控的全局优化或者不容易控制的其他局部优化方式，使得后端对状态的优化过程更加稳定和准确，也就使得本发明相比其他现有的其他算法运行时更加的稳定以及能获得更加准确的结果。

3、本发明使用了回环检测的算法自动检测相机运动过程是否回到以往经过的位置，可以为恢复出全局地图的绝对尺度信息，克服了传统单目视觉三维算法无法恢复出绝对尺度信息的限制，能够恢复出更加准确的地图和相机运动轨迹。

4、本发明实现可只采用单个摄像头的装置，相比起其他需要用到双目摄像头或者深度摄像头的算法，具有装置简单以及标定过程相对容易的优点，同时也极大地减少了购买设备的成本要求，相比其他依赖于更加复杂和昂贵的设备的算法具有更大的实用价值和市场应用价值。

具体实施方式

下面结合具体实施例对本发明作进一步说明。

本实施例所提供的基于半直接法和滑动窗口优化的单目视觉SLAM算法，包括以下步骤：

1)通过第三方的图像采集接口，将单目彩色摄像机采集到的彩色图像帧上传到电脑。

2)进行算法初始化过程，建立初始的两帧之间的相机位姿变换关系，并且建立初始的地图点，同时将初始的两帧都作为关键帧，并将初始的地图点插入到地图与滑动窗口中；其包括以下步骤过程：

2.1)基于摄像机初始化时对准的是平面物体的假设，对初始的两帧中的第一帧进行FAST特征点的提取，保存下这些特征点的图像坐标。并使用光流法在初始的第二帧中，对第一帧的提取的特征点进行跟踪，获得第一帧中跟踪到的特征点在第二帧中的图像坐标，从而建立起第一帧与第二帧的匹配点对，并对这些点进行三角化得到三维的初始地图点。

2.2)根据步骤2.1)获得的匹配点对，计算第一帧与第二帧之间的单应性矩阵。对根据重投影误差是否小于某个阈值而从匹配点钟筛选出正确的内点(inlier)，之后根据这些内点对之前获得的单应性矩阵进行矩阵分解计算获得姿态变换矩阵结果，将对应着最多内点数目的姿态变换矩阵作为最佳的姿态变换矩阵，以此来作为最终的两帧之间的姿态变换矩阵。

2.3)根据步骤2.2)得到的姿态变换进一步筛选内点。若此时内点数目小于某个阈值，则判定初始化失败，继续读入下一帧并返回步骤2.1)。若此时内点数目大于某个阈值，则认为初始化成功。此时将经过筛选后的内点所对应的三维地图点作为初始的地图点插入到地图中，并将初始化阶段的两帧作为关键帧，并插入到滑动窗口中。

3)将前一帧观测到的地图点投影到当前帧中，并对两帧图像之间进行基于光度误差的捆集优化计算得到两帧之间相机的姿态变换来跟踪相机的运动。在跟踪的过程中，根据设定的条件决定是否将当前帧作为新的关键帧，包括：①当前帧跟踪到的特征点与参考帧相比少于设定的比例；②设当前帧与几何距离上最相近的关键帧的几何距离为d_min，当前帧与其观测到的地图点的景深中值为d_mean，d_min/d_mean＜η，η为可调的阈值；若决定将当前帧作为关键帧，则对当前帧检测新的特征点，并将这些点作为候选地图点，并利用基于概率分布的深度滤波器算法对候选点的深度进行概率收敛；若决定生成新的关键帧，则需要同时进行以下两个步骤：

a、将该关键帧插入到滑动窗口的最前面，对滑动窗口中的关键帧进行变量边缘化(Marginalization)的计算以及使用捆集优化(Bundle Adjustment)算法优化得到校正的位姿，并使用校正后的位姿对误差累积的造成的漂移进行校正；其具体过程如下：

ⅰ、将该关键帧插入到滑动窗口的最前端，并判断：

①若此时滑动窗口中关键帧的数目小于N_KF，其中N_KF为可调的阈值，则直接对滑动窗口中的所有关键帧对应的姿态变量与观测到的地图点作为待优化变量，构造基于重投影误差的捆集优化方程，利用莱文伯格-马夸特(Levenberg-Marquardt)算法对上述优化变量进行迭代优化。

②若此时滑动窗口中关键帧的数目等于N_KF，则需要对关键帧中的姿态变量与观测到的地图点变量进行边缘化，则保留滑动窗口中最近插入的两帧，对其余的关键帧，计算距离得分值s(I_i)，其中I_i为滑动窗口中第i个关键帧，d(i,j)为第i帧与第j帧的欧氏距离，ε为可调的参数。选出s(I_i)最小的那一个关键帧，利用舒尔补(schur component)算法对该关键帧对应的地图点变量与姿态变量进行边缘化。然后结合边缘化后的信息，将滑动窗口中的其他的关键帧对应的姿态变量与观测到的地图点作为待优化变量，构造基于重投影误差的捆集优化方程，利用莱文伯格-马夸特(Levenberg-Marquardt)算法对上述优化变量进行迭代优化。

ⅱ、使用优化后的变量值，对优化前的变量值进行更新。

b、对该关键帧提取特征点和描述子，并插入到基于词袋模型(Bag of Words)的关键帧数据库中。同时检索关键帧数据库，进行图像相似性匹配，以判定是否检测到回环；其具体过程如下：

ⅰ、对新生成的关键帧，提取特征点并对特征点提取ORB描述子，并插入到基于词袋模型(Bag of Words)的关键帧数据库中。并将新来的当前关键帧加入到位姿图(PoseGraph)模型中。

ⅱ、检索关键帧数据库，进行图像相似性匹配，找到若干数目的在位姿图模型中与当前关键帧不相邻的并相隔一定结点数目的，但图像特征最相近的关键帧加入到可能出现回环的候选回环帧集合。

ⅲ、对上述候选回环帧，对回环帧中已经与地图点相对应了的特征点与在当前关键帧中已经与地图点相对应了的特征点进行特征匹配，建立3D点与3D点之间的匹配关系，从而使用这些匹配关系来计算它们与当前关键帧之间的相似性变换(SimilarityTransformation)。若得到相似性变换得到足够多的内点支持，则认为出现回环。否则，继续终止检测，等待下一个关键帧的到来，再重复步骤ⅰ至ⅲ。

ⅳ、若检测到出现回环，则利用该步骤4.3.3)计算得到的相似性变换恢复出尺度信息。利用尺度信息，对位姿图模型中回环帧对应的结点到当前关键帧对应的结点进行尺度信息的校正，校正相机运动过程中由于误差累积所造成的尺度漂移，从而校正得到真正准确的相机运动轨迹。

上述步骤3)包括以下步骤过程：

3.1)根据匀速运动模型，初步预测新的一帧的位姿变换。将上一帧观测到的地图点使用初步预测的位姿变换矩阵投影到当前帧中。

3.2)对前一帧观测到的地图点的坐标点像素值与该点投影到当前帧的坐标点像素值构造基于光度误差的优化方程，并利用高斯牛顿法进行优化，得到两帧之间的姿态变换矩阵。

3.3)将上一关键帧作为参考帧，利用步骤3.2)计算得到的姿态变换矩阵，反推计算得到当前帧与参考帧之间的仿射变换矩阵。对于每个在当前帧成功投影的地图点所对应的特征点所在当前帧中的像素坐标，利用仿射变换矩阵，构造出该特征点在参考帧中的像素坐标，并构造基于光度误差的优化方程，优化求解出该特征点所在当前帧中更精确的特征点位置。

3.4)对步骤3.2)与3.3)优化得到的位姿与观测量，再次进行捆集优化，得到精确的两帧之间的姿态变换矩阵，并以此计算出当前帧相对全局坐标系下的姿态变换矩阵T_kw。

3.5)若当前的帧满足成为关键帧的条件，则将当前帧作为关键帧。对当前帧中检测新的FAST特征点，并将这些特征点作为候选的地图点，在之后不断获取的图像帧中寻找到的对应点作为观测量，使用概率深度滤波器进行深度估计直至深度达到收敛。其中，使用概率深度滤波器进行深度收敛包括以下步骤：

3.5.1)假设候选地图点的深度服从高斯概率分布，并根据当前地图点的最小深度将所有候选地图点的概率均值和方差统一初始化。

3.5.2)仍然将最近的那一帧的关键帧作为当前帧所对应的参考帧。根据不断得到每一帧中使用极线约束进行匹配点搜索，在匹配点搜索成功后，三角化得到更加精确的深度，并更新该点的高斯概率分布的参数。

3.5.3)检测当前候选地图点与极线搜索检测到的对应点的定位误差，若该定位误差小于一个像素的偏差，则认为该点对应深度已经收敛，将其作为新的地图点插入到地图当中。否则，继续读入新的图像帧，继续根据步骤3.1)-3.5)得到的姿态变换矩阵T_kw与新的观测数据继续进行深度估计直至收敛。

综上所述，本发明算法只利用特征点像素亮度信息而不计算特征描述子的半直接法对运动相机进行跟踪，相比计算特征描述子的基于特征点的SLAM方法该算法避免了花费大量计算时间提取特征点描述子的缺点，具有更快的运行速度。同时使用了基于滑动窗口的后端优化策略，固定了后端相机姿态优化的计算复杂度，克服了现有的SLAM算法由于算法运行过程中不断生成的包括相机姿态与地图点在内的待优化数据的不断累积而拖慢优化算法计算速度的缺点，保证了算法的始终高效运行和实时性。另外，本发明算法可仅仅使用单个彩色摄像头，设备简单并且易于标定，相比视觉SLAM领域中常见使用的价格相对昂贵、普及率不高的深度摄像头，以及装置安装固定相对复杂、标定过程相对繁琐的双目摄像头装置，具有较大的实用价值和更加广泛的应用场景，值得推广。

以上所述实施例只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (5)

1.基于半直接法和滑动窗口优化的单目视觉SLAM算法，其特征在于，包括以下步骤：

1)通过第三方的图像采集接口，将单目彩色摄像机采集到的彩色图像帧上传到电脑；

2)进行算法初始化过程，建立初始的两帧之间的相机位姿变换关系，并且建立初始的地图点，同时将初始的两帧都作为关键帧，并将初始的地图点插入到地图与滑动窗口中；

3)将前一帧观测到的地图点投影到当前帧中，并对两帧图像之间进行基于光度误差的捆集优化计算，得到两帧之间相机的姿态变换来跟踪相机的运动；在跟踪的过程中，根据设定的两个条件决定是否将当前帧作为新的关键帧，包括：①当前帧跟踪到的特征点与参考帧相比少于设定的比例；②设当前帧与几何距离上最相近的关键帧的几何距离为d_min，当前帧与其观测到的地图点的景深中值为d_mean，d_min/d_mean＜η，η为可调的阈值；若决定将当前帧作为关键帧，则需对当前帧检测新的特征点，并将这些点作为候选地图点，然后利用基于概率分布的深度滤波器算法对候选点的深度进行概率收敛；若决定生成新的关键帧，则需要同时进行以下两个步骤：

a、将该关键帧插入到滑动窗口的最前面，对滑动窗口中的关键帧进行变量边缘化的计算以及使用捆集优化算法优化得到校正的位姿，并使用校正后的位姿对误差累积的造成的漂移进行校正；

b、对该关键帧提取特征点和描述子，并插入到基于词袋模型的关键帧数据库中，同时检索关键帧数据库，进行图像相似性匹配，以判定是否检测到回环。

2.根据权利要求1所述的基于半直接法和滑动窗口优化的单目视觉SLAM算法，其特征在于，所述步骤2)包括以下步骤：

2.1)基于摄像机初始化时对准的是平面物体的假设，对初始的两帧中的第一帧进行FAST特征点的提取，保存下这些特征点的图像坐标，并使用光流法在初始的第二帧中，对第一帧的提取的特征点进行跟踪，获得第一帧中跟踪到的特征点在第二帧中的图像坐标，从而建立起第一帧与第二帧的匹配点对，并对这些点进行三角化得到三维的初始地图点；

2.2)根据步骤2.1)获得的匹配点对，计算第一帧与第二帧之间的单应性矩阵，对根据重投影误差是否小于某个阈值而从匹配点钟筛选出正确的内点，之后根据这些内点对之前获得的单应性矩阵进行矩阵分解计算获得姿态变换矩阵结果，将对应着最多内点数目的姿态变换矩阵作为最佳的姿态变换矩阵，以此来作为最终的两帧之间的姿态变换矩阵；

2.3)根据步骤2.2)得到的姿态变换进一步筛选内点：若此时内点数目小于某个阈值，则判定初始化失败，继续读入下一帧并返回步骤2.1)，若此时内点数目大于某个阈值，则认为初始化成功，此时将经过筛选后的内点所对应的三维地图点作为初始的地图点插入到地图中，并将初始化阶段的两帧作为关键帧，并插入到滑动窗口中。

3.根据权利要求1所述的基于半直接法和滑动窗口优化的单目视觉SLAM算法，其特征在于，所述步骤3)包括以下步骤：

3.1)根据匀速运动模型，初步预测新的一帧的位姿变换，将上一帧观测到的地图点使用初步预测的位姿变换矩阵投影到当前帧中；

3.2)对前一帧观测到的地图点的坐标点像素值与该点投影到当前帧的坐标点像素值构造基于光度误差的优化方程，并利用高斯牛顿法进行优化，得到两帧之间的姿态变换矩阵；

3.3)将上一关键帧作为参考帧，利用步骤3.2)计算得到的姿态变换矩阵，反推计算得到当前帧与参考帧之间的仿射变换矩阵；对于每个在当前帧成功投影的地图点所对应的特征点所在当前帧中的像素坐标，利用仿射变换矩阵，构造出该特征点在参考帧中的像素坐标，并构造基于光度误差的优化方程，优化求解出该特征点所在当前帧中更精确的特征点位置；

3.4)对步骤3.2)与步骤3.3)优化得到的位姿与观测量，再次进行捆集优化，得到精确的两帧之间的姿态变换矩阵，并以此计算出当前帧相对全局坐标系下的姿态变换矩阵T_kw；

3.5)若当前的帧满足成为关键帧的条件，则将当前帧作为关键帧，对当前帧中检测新的FAST特征点，并将这些特征点作为候选的地图点，在之后不断获取的图像帧中寻找到的对应点作为观测量，使用概率深度滤波器进行深度估计直至深度达到收敛；其中，使用概率深度滤波器进行深度收敛包括以下步骤：

3.5.1)假设候选地图点的深度服从高斯概率分布，并根据当前地图点的最小深度将所有候选地图点的概率均值和方差统一初始化；

3.5.2)仍然将最近的那一帧的关键帧作为当前帧所对应的参考帧，根据不断得到每一帧中使用极线约束进行匹配点搜索，在匹配点搜索成功后，三角化得到更加精确的深度，并更新该点的高斯概率分布的参数；

3.5.3)检测当前候选地图点与极线搜索检测到的对应点的定位误差，若该定位误差小于一个像素的偏差，则认为该点对应深度已经收敛，将其作为新的地图点插入到地图当中，否则，继续读入新的图像帧，继续根据步骤3.1)-3.5)得到的姿态变换矩阵T_kw与新的观测数据继续进行深度估计直至收敛。

4.根据权利要求1所述的基于半直接法和滑动窗口优化的单目视觉SLAM算法，其特征在于，在步骤3)中，所述步骤a包括以下步骤：

ⅰ、将该关键帧插入到滑动窗口的最前端，并判断：

①若此时滑动窗口中关键帧的数目小于N_KF，其中N_KF为可调的阈值，则直接对滑动窗口中的所有关键帧对应的姿态变量与观测到的地图点作为待优化变量，构造基于重投影误差的捆集优化方程，利用莱文伯格-马夸特算法对上述优化变量进行迭代优化；

②若此时滑动窗口中关键帧的数目等于N_KF，则需要对关键帧中的姿态变量与观测到的地图点变量进行边缘化，并保留滑动窗口中最近插入的两帧，对其余的关键帧，计算距离得分值s(I_i)，其中I_i为滑动窗口中第i个关键帧，d(i,j)为第i帧与第j帧的欧氏距离，ε为可调的参数；选出s(I_i)最小的那一个关键帧，利用舒尔补算法对该关键帧对应的地图点变量与姿态变量进行边缘化，然后结合边缘化后的信息，将滑动窗口中的其他的关键帧对应的姿态变量与观测到的地图点作为待优化变量，构造基于重投影误差的捆集优化方程，利用莱文伯格-马夸特算法对上述优化变量进行迭代优化；

ⅱ、使用优化后的变量值，对优化前的变量值进行更新。

5.根据权利要求1所述的基于半直接法和滑动窗口优化的单目视觉SLAM算法，其特征在于，在步骤3)中，所述步骤b包括以下步骤：

ⅰ、对新生成的关键帧，提取特征点并对特征点提取ORB描述子，并插入到基于词袋模型的关键帧数据库中，并将新来的当前关键帧加入到位姿图模型中；

ⅱ、检索关键帧数据库，进行图像相似性匹配，找到若干数目的在位姿图模型中与当前关键帧不相邻的并相隔设定结点数目的，但图像特征最相近的关键帧加入到可能出现回环的候选回环帧集合；

ⅲ、对上述候选回环帧中已经与地图点相对应了的特征点与在当前关键帧中已经与地图点相对应了的特征点进行特征匹配，建立3D点与3D点之间的匹配关系，从而使用这些匹配关系来计算它们与当前关键帧之间的相似性变换，若得到的相似性变换得到所需数量的内点支持，则认为出现回环，否则，继续终止检测，等待下一个关键帧的到来，再重复步骤ⅰ至ⅲ；

ⅳ、若检测到出现回环，则利用该步骤ⅲ计算得到的相似性变换恢复出尺度信息，利用尺度信息，对位姿图模型中回环帧对应的结点到当前关键帧对应的结点进行尺度信息的校正，校正相机运动过程中由于误差累积所造成的尺度漂移，从而校正得到真正准确的相机运动轨迹。