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

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

Monocular vision SLAM algorithm based on semi direct method and sliding window optimization

【技术实现步骤摘要】
基于半直接法和滑动窗口优化的单目视觉SLAM算法
本专利技术涉及计算机视觉中的同时定位与建图(SLAM)算法的
，尤其是指一种基于半直接法和滑动窗口优化的单目视觉SLAM算法。
技术介绍
随着机器人、自动驾驶、增强现实与虚拟现实等新兴科技领域在全世界范围内的蓬勃发展，其背后的关键技术同时定位与建图(SLAM)技术也受到了越来越多来自科研界与工业界的关注。同时定位与建图算法指的是，在完全未知的环境中，对运动着的机器人或传感器载体，能够不断地跟踪其运动的轨迹，同时建立起对环境地图的三维感知。上述的领域都对SLAM技术具有很高的技术要求，一个运行稳定，运算结果准确并且运算速度可以保证实时性的SLAM算法是整个应用系统高效稳定运行的基石，对整体系统的性能有着至关重要的影响。现有的主流SLAM算法按照传感器的种类可划分为基于激光发射器的激光类型，以及基于彩色摄像头或深度摄像头或两者结合等视觉类这两大类。本专利技术主要针对的是视觉领域的SLAM算法。另外，根据SLAM算法采用的对环境的描述方式的不同，SLAM算法又可分为基于稀疏分布的特征点并计算其描述子的稀疏法(以ORB-SLAM为代表)、基于全图像素信息的稠密法(以KinectFusion为代表)，基于梯度信息明显的像素集合信息的直接法(以LSD-SLAM为代表)，以及基于特征点但不计算描述子的半直接法(以SVO为代表)这几种。另外，不同的方法往往采用不同的用以消除SLAM算法运行中的累积误差的优化策略，其对SLAM算法的准确性和高效性起到非常关键的作用。上述的各种类型的方法各有各自的优点和缺点。将这些方法中的优点提取出来并加以融合，构造出运行速度快、计算准确、优化方式效率高的SLAM算法，具有非常大的研究意义和实用价值。
技术实现思路
本专利技术的目的在于克服现有单目视觉SLAM算法中运算速度与准确性难以同时兼顾的问题，提供了一种基于半直接法和滑动窗口优化的单目视觉SLAM算法，该算法只利用特征点像素亮度信息而不计算特征描述子的半直接法对运动相机进行跟踪，相比计算特征描述子的基于特征点的SLAM方法该算法避免了花费大量计算时间提取特征点描述子的缺点，具有更快的运行速度。该算法同时使用了基于滑动窗口的后端优化策略，固定了后端相机姿态优化的计算复杂度，克服了现有的SLAM算法由于算法运行过程中不断生成的包括相机姿态与地图点在内的待优化数据的不断累积而拖慢优化算法计算速度的缺点，保证了算法的始终高效运行和实时性。另外，该算法仅仅使用单个彩色摄像头，设备简单并且易于标定，相比视觉SLAM领域中常见使用的价格相对昂贵、普及率不高的深度摄像头，以及装置安装固定相对复杂、标定过程相对繁琐的双目摄像头装置，具有较大的实用价值和更加广泛的应用场景。为实现上述目的，本专利技术所提供的技术方案为：基于半直接法和滑动窗口优化的单目视觉SLAM算法，包括以下步骤：1)通过第三方的图像采集接口，将单目彩色摄像机采集到的彩色图像帧上传到电脑；2)进行算法初始化过程，建立初始的两帧之间的相机位姿变换关系，并且建立初始的地图点，同时将初始的两帧都作为关键帧，并将初始的地图点插入到地图与滑动窗口中；3)将前一帧观测到的地图点投影到当前帧中，并对两帧图像之间进行基于光度误差的捆集优化计算，得到两帧之间相机的姿态变换来跟踪相机的运动；在跟踪的过程中，根据设定的两个条件决定是否将当前帧作为新的关键帧，包括：①当前帧跟踪到的特征点与参考帧相比少于设定的比例；②设当前帧与几何距离上最相近的关键帧的几何距离为dmin，当前帧与其观测到的地图点的景深中值为dmean，dmin/dmean＜η，η为可调的阈值；若决定将当前帧作为关键帧，则需对当前帧检测新的特征点，并将这些点作为候选地图点，然后利用基于概率分布的深度滤波器算法对候选点的深度进行概率收敛；若决定生成新的关键帧，则需要同时进行以下两个步骤：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)优化得到的位姿与观测量，再次进行捆集优化，得到精确的两帧之间的姿态变换矩阵，并以此计算出当前帧相对全局坐标系下的姿态变换矩阵Tkw；3.5)若当前的帧满足成为关键帧的条件，则将当前帧作为关键帧，对当前帧中检测新的FAST特征点，并将这些特征点作为候选的地图点，在之后不断获取的图像帧中寻找到的对应点作为观测量，使用概率深度滤波器进行深度估计直至深度达到收敛；其中，使用概率深度滤波器进行深度收敛包括以下步骤：3.5.1)假设候选地图点的深度服从高斯概率分布，并根据当前地图点的最小深度将所有候选地图点的概率均值和方差统一初始化；3.5.2)仍然将最近的那一帧的关键帧作为当前帧所对应的参考帧，根据不断得到每一帧中使用极线约束进行匹配点搜索，在匹配点搜索成功后，三角化得到更加精确的深度，并更新该点的高斯概率分布的参数；3.5.3)检测当前候选地图点与极线搜索检测到的对应点的定位误差，若该定位误差小于一个像素的偏差，则认为该点对应深度已经收敛，本文档来自技高网...

【技术保护点】
基于半直接法和滑动窗口优化的单目视觉SLAM算法，其特征在于，包括以下步骤：1)通过第三方的图像采集接口，将单目彩色摄像机采集到的彩色图像帧上传到电脑；2)进行算法初始化过程，建立初始的两帧之间的相机位姿变换关系，并且建立初始的地图点，同时将初始的两帧都作为关键帧，并将初始的地图点插入到地图与滑动窗口中；3)将前一帧观测到的地图点投影到当前帧中，并对两帧图像之间进行基于光度误差的捆集优化计算，得到两帧之间相机的姿态变换来跟踪相机的运动；在跟踪的过程中，根据设定的两个条件决定是否将当前帧作为新的关键帧，包括：①当前帧跟踪到的特征点与参考帧相比少于设定的比例；②设当前帧与几何距离上最相近的关键帧的几何距离为dmin，当前帧与其观测到的地图点的景深中值为dmean，dmin/dmean＜η，η为可调的阈值；若决定将当前帧作为关键帧，则需对当前帧检测新的特征点，并将这些点作为候选地图点，然后利用基于概率分布的深度滤波器算法对候选点的深度进行概率收敛；若决定生成新的关键帧，则需要同时进行以下两个步骤：a、将该关键帧插入到滑动窗口的最前面，对滑动窗口中的关键帧进行变量边缘化的计算以及使用捆集优化算法优化得到校正的位姿，并使用校正后的位姿对误差累积的造成的漂移进行校正；b、对该关键帧提取特征点和描述子，并插入到基于词袋模型的关键帧数据库中，同时检索关键帧数据库，进行图像相似性匹配，以判定是否检测到回环。...

【技术特征摘要】
1.基于半直接法和滑动窗口优化的单目视觉SLAM算法，其特征在于，包括以下步骤：1)通过第三方的图像采集接口，将单目彩色摄像机采集到的彩色图像帧上传到电脑；2)进行算法初始化过程，建立初始的两帧之间的相机位姿变换关系，并且建立初始的地图点，同时将初始的两帧都作为关键帧，并将初始的地图点插入到地图与滑动窗口中；3)将前一帧观测到的地图点投影到当前帧中，并对两帧图像之间进行基于光度误差的捆集优化计算，得到两帧之间相机的姿态变换来跟踪相机的运动；在跟踪的过程中，根据设定的两个条件决定是否将当前帧作为新的关键帧，包括：①当前帧跟踪到的特征点与参考帧相比少于设定的比例；②设当前帧与几何距离上最相近的关键帧的几何距离为dmin，当前帧与其观测到的地图点的景深中值为dmean，dmin/dmean＜η，η为可调的阈值；若决定将当前帧作为关键帧，则需对当前帧检测新的特征点，并将这些点作为候选地图点，然后利用基于概率分布的深度滤波器算法对候选点的深度进行概率收敛；若决定生成新的关键帧，则需要同时进行以下两个步骤：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...

【专利技术属性】
技术研发人员：青春美，黄韬，徐向民，
申请(专利权)人：华南理工大学，
类型：发明
国别省市：广东,44