[发明专利]一种融合点云和图像的同步定位和建图方法

说明书

本发明公开了一种融合点云和图像的同步定位和建图方法，包括以下步骤：S1：将相机在t时刻和t’时刻获取到的图像分别输入图像处理网络得到特征图金字塔1和特征图金字塔2；S2：将激光雷达在t时刻获取到的点云变换到相机坐标系；S3：在一个序列窗口内的每连续两帧上执行步骤一、步骤二，将帧间相对位姿集合、累计的三维点云、每帧的特征图金字塔输入滑动窗口算法；S4：将一段时间内的点云数据输入回环检测算法；S5：将前四个步骤求得的位姿、每帧点云、每帧图像数据输入点云拼接过程。本发明，首先将图像输入深度神经网络生成特征金字塔图，然后以多视角对应像素点的特征误差最小化为目标函数，优化求解出多视角的相机相对位姿。

技术领域

本发明涉及图像处理和点云处理算法技术领域，具体是一种融合点云和图像的同步定位和建图方法。

背景技术

环境三维地图重建，一般采用相机、雷达、惯导等传感器融合算法进行，需要同步估计传感器自身6自由度的态变换信息(3自由度旋转和3自由度平移)并构建环境的三维结构地图(一般以点云模型描述)。这种方法一般被称为视觉里程计或同步建图与定位(SLAM)算法。近些年，从单目相机到密集的RGB-D相机，视觉SLAM算法的研究一直是机器人研究领域的热门话题，且得到了长足的发展。其中，三维激光雷达和单目相机结合的传感器系统作为典型实用的方案备受关注。经过预先联合标定，激光雷达可以为相机图像提供稀疏的深度数据，以恢复单目视觉SLAM的绝对尺度，并增强视觉SLAM效果。

目前，基于稀疏深度增强的视觉SLAM方案包括：特征点法视觉里程计，以及直接法视觉SLAM。特征点法的主要思路是提取图像的特征点和描述子，利用稀疏的深度进行关联或插值得到部分特征点的绝对深度值，然后通过多视角图像特征点匹配的方式估计相机位姿。然而，特征点和描述子的提取是一个耗时的过程，不同场景下，特征点和描述子的选择也会影响到匹配的效果，快速提取鲁棒性强的特征点和描述子是一个有挑战的环节。另一方面，稀疏深度点并不能保证落在图像特征点上，而插值得到的深度不准确，影响到位姿估计精度。相比特征点法，直接法基于光度一致性的假设直接在原始图像上进行优化，即假设同一个三维空间点，在不同视角下的观测具有同样的像素灰度。直接法可以只在有深度值的点上进行优化，完全不用计算关键点和描述子，既避免了特征计算的时间，又能应对特征缺失的场景。但是在实际场景中，由于光照变化、弱纹理等影响，光度一致性假设不一定能满足，此时就需要对图像光度进行标定和调整，或者用图像的梯度特征替换原始图像亮度值。

发明内容

本发明的目的在于提供一种融合点云和图像的同步定位和建图方法，以解决现有技术中的问题。

为实现上述目的，本发明提供如下技术方案：一种融合点云和图像的同步定位和建图方法，包括以下步骤：

S1：将相机在t时刻和t’时刻获取到的图像分别输入图像处理网络得到特征图金字塔1和特征图金字塔2；

S2：将激光雷达在t时刻获取到的点云变换到相机坐标系，并将t时刻点云、特征图金字塔1、特征图金字塔2、初始位姿逐步输入“GN”高斯牛顿层进行位姿优化求解；

S3：在一个序列窗口内的每连续两帧上执行步骤一、步骤二，将帧间相对位姿集合、累计的三维点云、每帧的特征图金字塔输入滑动窗口算法；

S4：将一段时间内的点云数据输入回环检测算法：若没有检测到回环，则不更新历史位姿；若检测到回环则更新历史帧间位姿，并利用高斯牛顿法进行优化；

S5：将前四个步骤求得的位姿、每帧点云、每帧图像数据输入点云拼接过程：每帧点云首先基于标定信息，从图像中获取颜色数据，得到彩色点云；利用每帧位姿信息，将彩色点云变换到全局坐标系下，并将多帧点云进行拼接叠加得到全局点云地图；对全局地图进行点云均匀采样，最终得到分布均匀的彩色点云地图。

优选的，所述S1中图像处理网络由自底向上特征提取模块和自顶向下特征提取模块构成。