一种融合点云和图像的同步定位和建图方法技术

本发明专利技术公开了一种融合点云和图像的同步定位和建图方法，包括以下步骤：S1：将相机在t时刻和t

【技术实现步骤摘要】
一种融合点云和图像的同步定位和建图方法


[0001]本专利技术涉及图像处理和点云处理算法
，具体是一种融合点云和图像的同步定位和建图方法。

技术介绍

[0002]环境三维地图重建，一般采用相机、雷达、惯导等传感器融合算法进行，需要同步估计传感器自身6自由度的态变换信息(3自由度旋转和3自由度平移)并构建环境的三维结构地图(一般以点云模型描述)。这种方法一般被称为视觉里程计或同步建图与定位(SLAM)算法。近些年，从单目相机到密集的RGB
‑
D相机，视觉SLAM算法的研究一直是机器人研究领域的热门话题，且得到了长足的发展。其中，三维激光雷达和单目相机结合的传感器系统作为典型实用的方案备受关注。经过预先联合标定，激光雷达可以为相机图像提供稀疏的深度数据，以恢复单目视觉SLAM的绝对尺度，并增强视觉SLAM效果。
[0003]目前，基于稀疏深度增强的视觉SLAM方案包括：特征点法视觉里程计，以及直接法视觉SLAM。特征点法的主要思路是提取图像的特征点和描述子，利用稀疏的深度进行关联或插值得到部分特征点的绝对深度值，然后通过多视角图像特征点匹配的方式估计相机位姿。然而，特征点和描述子的提取是一个耗时的过程，不同场景下，特征点和描述子的选择也会影响到匹配的效果，快速提取鲁棒性强的特征点和描述子是一个有挑战的环节。另一方面，稀疏深度点并不能保证落在图像特征点上，而插值得到的深度不准确，影响到位姿估计精度。相比特征点法，直接法基于光度一致性的假设直接在原始图像上进行优化，即假设同一个三维空间点，在不同视角下的观测具有同样的像素灰度。直接法可以只在有深度值的点上进行优化，完全不用计算关键点和描述子，既避免了特征计算的时间，又能应对特征缺失的场景。但是在实际场景中，由于光照变化、弱纹理等影响，光度一致性假设不一定能满足，此时就需要对图像光度进行标定和调整，或者用图像的梯度特征替换原始图像亮度值。

技术实现思路

[0004]本专利技术的目的在于提供一种融合点云和图像的同步定位和建图方法，以解决现有技术中的问题。
[0005]为实现上述目的，本专利技术提供如下技术方案：一种融合点云和图像的同步定位和建图方法，包括以下步骤：
[0006]S1：将相机在t时刻和t
’
时刻获取到的图像分别输入图像处理网络得到特征图金字塔1和特征图金字塔2；
[0007]S2：将激光雷达在t时刻获取到的点云变换到相机坐标系，并将t时刻点云、特征图金字塔1、特征图金字塔2、初始位姿逐步输入“GN”高斯牛顿层进行位姿优化求解；
[0008]S3：在一个序列窗口内的每连续两帧上执行步骤一、步骤二，将帧间相对位姿集合、累计的三维点云、每帧的特征图金字塔输入滑动窗口算法；
[0009]S4：将一段时间内的点云数据输入回环检测算法：若没有检测到回环，则不更新历史位姿；若检测到回环则更新历史帧间位姿，并利用高斯牛顿法进行优化；
[0010]S5：将前四个步骤求得的位姿、每帧点云、每帧图像数据输入点云拼接过程：每帧点云首先基于标定信息，从图像中获取颜色数据，得到彩色点云；利用每帧位姿信息，将彩色点云变换到全局坐标系下，并将多帧点云进行拼接叠加得到全局点云地图；对全局地图进行点云均匀采样，最终得到分布均匀的彩色点云地图。
[0011]优选的，所述S1中图像处理网络由自底向上特征提取模块和自顶向下特征提取模块构成。
[0012]优选的，所述自底向上特征提取模块包含四层卷积残差模块，每上一层，特征图空间分辨率变为下一层的1/4，特征维数变为下一层的2倍；所述自顶向下特征提取模块包含四层双线性插值上采样层，每下一层，特征图空间分辨率变为上一层的4倍，特征维数保持不变。
[0013]优选的，所述自顶向下特征提取模块每层的输入是两个特征的逐元素相加结果，即对应自底向上特征提取模块层经过“1x1卷积”得到的特征图，以及自顶向下特征提取模块上一层插值上采样得到的特征图；自顶向下特征提取模块每层分别经过一个“3x3卷积”得到最终的特征图金字塔。
[0014]优选的，所述S2中高斯牛顿层从特征图金字塔高层开始优化，并将优化得到的位姿作为特征金字塔下一层的初始位姿输入；在每个特征金字塔层，高斯牛顿层迭代计算3次，最终在空间分辨率最高的特征金字塔层优化得到最终位姿输出。
[0015]优选的，所述高斯牛顿层由四个步骤构成：计算残差、计算雅可比矩阵、求解增量、更新变量，给定点云中的一组点pt和pt
’
、特征金字塔Ft和Ft
’
、初始位姿ξ0，高斯牛顿层的迭代更新公式如下：
[0016]S21：计算残差，e＝(e1,e2,...,e
N
)，其中，
[0017]e
k
＝F
t
(p
t,k
)
‑
F
t
’
(p
t
’
,k
)∈R
C
×1；
[0018]S22：计算雅可比矩阵，其中，
[0019][0020]S23：求解增量，δξ
i
＝(J
T
J)
‑1J
T
e；
[0021]S24：更新变量，ξ
i+1
＝δξ
i
.ξ
i
。
[0022]优选的，所述S3中利用高斯牛顿法对帧间相对位姿[T
12
；
…
；T
1N
；T
2N
]进行优化，得到更加平滑的帧间相对位姿，优化的目标函数为：
[0023][0024]e
k
＝F
i
(p
k
)
‑
F
j
(p
k'
)。
[0025]优选的，所述S4中以一个环路[T
i
；T
j
；T
ij
]为例，优化的目标函数为：
[0026][0027]与现有技术相比，本专利技术的有益效果是：首先将图像输入深度神经网络生成特征金字塔图，然后以多视角对应像素点的特征误差最小化为目标函数，优化求解出多视角的相机相对位姿；进一步，基于滑动窗口法，使用多帧信息做为一个窗口进行统一的位姿调整，使得求解的位姿在该窗口内具有较好的一致性；另外，在检测到回环之后，运用位姿图优化策略，进一步调整全局的相机位姿，从而得到全局一致性更强的位姿约束。
附图说明
[0028]附图用来提供对本专利技术的进一步理解，并且构成说明书的一部分，与本专利技术的实施例一起用于解释本专利技术，并不构成对本专利技术的限制。在附图中：
[0029]图1是本专利技术的位姿估计示意图；
[0030]图2是本专利技术的帧间位姿估计网络结构图；
[0031]图3为本专利技术的滑动窗口法优化示意图；
[0032]图4本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种融合点云和图像的同步定位和建图方法，其特征在于：包括以下步骤：S1：将相机在t时刻和t
’
时刻获取到的图像分别输入图像处理网络得到特征图金字塔1和特征图金字塔2；S2：将激光雷达在t时刻获取到的点云变换到相机坐标系，并将t时刻点云、特征图金字塔1、特征图金字塔2、初始位姿逐步输入“GN”高斯牛顿层进行位姿优化求解；S3：在一个序列窗口内的每连续两帧上执行步骤一、步骤二，将帧间相对位姿集合、累计的三维点云、每帧的特征图金字塔输入滑动窗口算法；S4：将一段时间内的点云数据输入回环检测算法：若没有检测到回环，则不更新历史位姿；若检测到回环则更新历史帧间位姿，并利用高斯牛顿法进行优化；S5：将前四个步骤求得的位姿、每帧点云、每帧图像数据输入点云拼接过程：每帧点云首先基于标定信息，从图像中获取颜色数据，得到彩色点云；利用每帧位姿信息，将彩色点云变换到全局坐标系下，并将多帧点云进行拼接叠加得到全局点云地图；对全局地图进行点云均匀采样，最终得到分布均匀的彩色点云地图。2.根据权利要求1所述的一种融合点云和图像的同步定位和建图方法，其特征在于：所述S1中图像处理网络由自底向上特征提取模块和自顶向下特征提取模块构成。3.根据权利要求2所述的一种融合点云和图像的同步定位和建图方法，其特征在于：所述自底向上特征提取模块包含四层卷积残差模块，每上一层，特征图空间分辨率变为下一层的1/4，特征维数变为下一层的2倍；所述自顶向下特征提取模块包含四层双线性插值上采样层，每下一层，特征图空间分辨率变为上一层的4倍，特征维数保持不变。4.根据权利要求3所述的一种融合点云和图像的同步定位和建图方法，其特征在于：所述自顶向下特征提取模块每层的输入是两个特征的逐元素相加结果，即对应自底向上特征提取模块层经过“1x1卷积”得到的特征图，以及自顶向下特征提取模块上一层插值上采样得到的特征图；自顶向下特征提取模块每层分别经过一个“3x3卷积”得到最终的特征图金字塔。5.根据权利要求1所述的一种融合点云和图像的同步定位和建图方法，其特征在于：所述S2中高斯牛顿层从特征图金字塔高层开始优化，并将优化得到的位姿作为特征金字塔下一层的初始位姿输入...

【专利技术属性】
技术研发人员：宋振波，
申请(专利权)人：英科森智能传感技术苏州有限公司，
类型：发明
国别省市：