一种融合点云和图像的同步定位和建图方法与流程

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


背景技术：

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


技术实现要素：

4.本发明的目的在于提供一种融合点云和图像的同步定位和建图方法，以解决现有技术中的问题。
5.为实现上述目的，本发明提供如下技术方案：一种融合点云和图像的同步定位和建图方法，包括以下步骤：
6.s1：将相机在t时刻和t’时刻获取到的图像分别输入图像处理网络得到特征图金字塔1和特征图金字塔2；
7.s2：将激光雷达在t时刻获取到的点云变换到相机坐标系，并将t时刻点云、特征图金字塔1、特征图金字塔2、初始位姿逐步输入“gn”高斯牛顿层进行位姿优化求解；
8.s3：在一个序列窗口内的每连续两帧上执行步骤一、步骤二，将帧间相对位姿集合、累计的三维点云、每帧的特征图金字塔输入滑动窗口算法；
9.s4：将一段时间内的点云数据输入回环检测算法：若没有检测到回环，则不更新历史位姿；若检测到回环则更新历史帧间位姿，并利用高斯牛顿法进行优化；
10.s5：将前四个步骤求得的位姿、每帧点云、每帧图像数据输入点云拼接过程：每帧点云首先基于标定信息，从图像中获取颜色数据，得到彩色点云；利用每帧位姿信息，将彩色点云变换到全局坐标系下，并将多帧点云进行拼接叠加得到全局点云地图；对全局地图进行点云均匀采样，最终得到分布均匀的彩色点云地图。
11.优选的，所述s1中图像处理网络由自底向上特征提取模块和自顶向下特征提取模块构成。
12.优选的，所述自底向上特征提取模块包含四层卷积残差模块，每上一层，特征图空间分辨率变为下一层的1/4，特征维数变为下一层的2倍；所述自顶向下特征提取模块包含四层双线性插值上采样层，每下一层，特征图空间分辨率变为上一层的4倍，特征维数保持不变。
13.优选的，所述自顶向下特征提取模块每层的输入是两个特征的逐元素相加结果，即对应自底向上特征提取模块层经过“1x1卷积”得到的特征图，以及自顶向下特征提取模块上一层插值上采样得到的特征图；自顶向下特征提取模块每层分别经过一个“3x3卷积”得到最终的特征图金字塔。
14.优选的，所述s2中高斯牛顿层从特征图金字塔高层开始优化，并将优化得到的位姿作为特征金字塔下一层的初始位姿输入；在每个特征金字塔层，高斯牛顿层迭代计算3次，最终在空间分辨率最高的特征金字塔层优化得到最终位姿输出。
15.优选的，所述高斯牛顿层由四个步骤构成：计算残差、计算雅可比矩阵、求解增量、更新变量，给定点云中的一组点pt和pt’、特征金字塔ft和ft’、初始位姿ξ0，高斯牛顿层的迭代更新公式如下：
16.s21：计算残差，e＝(e1,e2,...,en)，其中，
17.ek＝f
t
(p
t,k
)-f
t’(p
t’,k
)∈rc×1；
18.s22：计算雅可比矩阵，其中，
[0019][0020]
s23：求解增量，δξi＝(j
t
j)-1jt
e；
[0021]
s24：更新变量，ξ
i 1
＝δξi.ξi。
[0022]
优选的，所述s3中利用高斯牛顿法对帧间相对位姿[t
12
；
…
；t
1n
；t
2n
]进行优化，得到更加平滑的帧间相对位姿，优化的目标函数为：
[0023][0024]ek
＝fi(pk)-fj(p
k'
)。
[0025]
优选的，所述s4中以一个环路[ti；tj；t
ij
]为例，优化的目标函数为：
[0026]
[0027]
与现有技术相比，本发明的有益效果是：首先将图像输入深度神经网络生成特征金字塔图，然后以多视角对应像素点的特征误差最小化为目标函数，优化求解出多视角的相机相对位姿；进一步，基于滑动窗口法，使用多帧信息做为一个窗口进行统一的位姿调整，使得求解的位姿在该窗口内具有较好的一致性；另外，在检测到回环之后，运用位姿图优化策略，进一步调整全局的相机位姿，从而得到全局一致性更强的位姿约束。
附图说明
[0028]
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：
[0029]
图1是本发明的位姿估计示意图；
[0030]
图2是本发明的帧间位姿估计网络结构图；
[0031]
图3为本发明的滑动窗口法优化示意图；
[0032]
图4是本发明的位姿图优化示意图；
[0033]
图5为本发明的点云拼接示意图；
[0034]
图6为本发明部分数据上的轨迹恢复效果图。
具体实施方式
[0035]
为使本发明实施方式的目的、技术方案和优点更加清楚，下面将结合本发明实施方式中的附图，对本发明实施方式中的技术方案进行清楚、完整地描述，显然，所描述的实施方式是本发明一部分实施方式，而不是全部的实施方式。基于本发明中的实施方式，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施方式，都属于本发明保护的范围。因此，以下对在附图中提供的本发明的实施方式的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施方式。基于本发明中的实施方式，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施方式，都属于本发明保护的范围。
[0036]
请参阅图1-5，本发明实施例中，一种融合点云和图像的同步定位和建图方法，包括以下步骤：
[0037]
s1：将相机在t时刻和t’时刻获取到的图像分别输入图像处理网络得到特征图金字塔1和特征图金字塔2；
[0038]
s2：将激光雷达在t时刻获取到的点云变换到相机坐标系，并将t时刻点云、特征图金字塔1、特征图金字塔2、初始位姿逐步输入“gn”高斯牛顿层进行位姿优化求解；
[0039]
s3：在一个序列窗口内的每连续两帧上执行步骤一、步骤二，将帧间相对位姿集合、累计的三维点云、每帧的特征图金字塔输入滑动窗口算法；
[0040]
s4：将一段时间内的点云数据输入回环检测算法：若没有检测到回环，则不更新历史位姿；若检测到回环则更新历史帧间位姿，并利用高斯牛顿法进行优化；
[0041]
s5：将前四个步骤求得的位姿、每帧点云、每帧图像数据输入点云拼接过程：每帧点云首先基于标定信息，从图像中获取颜色数据，得到彩色点云；利用每帧位姿信息，将彩色点云变换到全局坐标系下，并将多帧点云进行拼接叠加得到全局点云地图；对全局地图进行点云均匀采样，最终得到分布均匀的彩色点云地图。
[0042]
优选的，所述s1中图像处理网络由自底向上特征提取模块和自顶向下特征提取模块构成。
[0043]
优选的，所述自底向上特征提取模块包含四层卷积残差模块，每上一层，特征图空间分辨率变为下一层的1/4，特征维数变为下一层的2倍；所述自顶向下特征提取模块包含四层双线性插值上采样层，每下一层，特征图空间分辨率变为上一层的4倍，特征维数保持不变。
[0044]
优选的，所述自顶向下特征提取模块每层的输入是两个特征的逐元素相加结果，即对应自底向上特征提取模块层经过“1x1卷积”得到的特征图，以及自顶向下特征提取模块上一层插值上采样得到的特征图；自顶向下特征提取模块每层分别经过一个“3x3卷积”得到最终的特征图金字塔。
[0045]
优选的，所述s2中高斯牛顿层从特征图金字塔高层开始优化，并将优化得到的位姿作为特征金字塔下一层的初始位姿输入；在每个特征金字塔层，高斯牛顿层迭代计算3次，最终在空间分辨率最高的特征金字塔层优化得到最终位姿输出。
[0046]
优选的，所述高斯牛顿层由四个步骤构成：计算残差、计算雅可比矩阵、求解增量、更新变量，给定点云中的一组点pt和pt’、特征金字塔ft和ft’、初始位姿ξ0，高斯牛顿层的迭代更新公式如下：
[0047]
s21：计算残差，e＝(e1,e2,...,en)，其中，
[0048]ek
＝f
t
(p
t,k
)-f
t’(p
t’,k
)∈rc×1；
[0049]
s22：计算雅可比矩阵，其中，
[0050][0051]
s23：求解增量，δξi＝(j
t
j)-1jt
e；
[0052]
s24：更新变量，ξ
i 1
＝δξi.ξi。
[0053]
优选的，所述s3中利用高斯牛顿法对帧间相对位姿[t
12
；
…
；t
1n
；t
2n
]进行优化，得到更加平滑的帧间相对位姿，优化的目标函数为：
[0054][0055]ek
＝fi(pk)-fj(p
k'
)。
[0056]
优选的，所述s4中以一个环路[ti；tj；t
ij
]为例，优化的目标函数为：
[0057][0058]
在一些实际城市、乡村道路的车载数据上进行测试，该方法得到的平均平移误差不高于2％，平均旋转误差不高于0.8
°
/100m；可用于构建城市环境三维地图，恢复平台运行轨迹，在部分数据上的轨迹恢复效果如图6。
[0059]
本发明的工作原理是：首先将图像输入深度神经网络生成特征金字塔图，然后以多视角对应像素点的特征误差最小化为目标函数，优化求解出多视角的相机相对位姿；进一步，基于滑动窗口法，使用多帧信息做为一个窗口进行统一的位姿调整，使得求解的位姿
在该窗口内具有较好的一致性；另外，在检测到回环之后，运用位姿图优化策略，进一步调整全局的相机位姿，从而得到全局一致性更强的位姿约束。
[0060]
最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。