一种面向动态场景的点线融合语义SLAM方法

一种面向动态场景的点线融合语义slam方法
技术领域
1.本发明涉及一种面向动态场景的点线融合语义slam(simultaneous localization and mapping，同步定位与建图)方法，适用于rgbd模式下动态场景中的视觉定位领域。


背景技术：

2.目前主流的视觉slam算法均采用了静态世界假设(static worldassumption)，该假设假定了相机所处环境是绝对静止，且刚性的。但是在实际的应用环境中，机器人所处环境的动态性是无法避免的，这种情况下静态世界假设会不在成立，传统视觉slam系统的定位精度会大大降低，甚至导致整个slam系统失效。然而现有动态slam系统往往通过增加对特征点的运动特性进行二次判断，并以此为依据剔除动态点，但是该方法在低纹理场景下会损失大量特征点，造成slam系统定位和地图构建精度的下降，很容易致使整个slam系统崩溃。除此之外，在slam的实际应用场景中，走廊等弱纹理环境也是很常见的场景，而对于传统的视觉slam算法，其前端视觉里程计研究方法主要分为两大类：特征点法和直接法。其中，基于特征点法的前端视觉里程计方法，是目前视觉里程计的主流方法。特征点作为最低层次的特征，在纹理丰富的场景容易提取，因此其相关理论也比较发达，传统的基于点特征slam系统已经较为成熟。然而，在低纹理环境等复杂环境中，点特征的检测极不稳定，甚至可能暂时消失，这直接导致有效的特征匹配点对的数量远少于完成位姿估计所需匹配点的最低数量。因此在低纹理环境中，传统特征点法的视觉slam系统无法进行特征匹配，求解位姿，进而导致跟踪失败。基于直接法的前端视觉里程计，也往往依赖于图像中提取到的角点，通过对角点及其周围像素进行光流跟踪，进而实现视觉定位。这两类方法在低纹理环境中较难提取到可靠的特征，造成slam系统定位和地图构建精度下降。因此研究面向动态场景且覆盖低纹理环境下的视觉slam问题具有重要的研究意义。
3.针对面向动态场景的点线融合语义slam方法研究，目前的研究难点主要表现在以下几个方面：(1)低纹理场景中传统点特征提取困难；(2)现有的动态场景处理方式，会损失大量特征，导致位姿解算不准确；(3)现有的基于深度学习的语义分割网络对场景中的动态物体分割不完整，导致用于位姿解算的静态特征不可靠。


技术实现要素：

4.本发明的技术解决问题是：克服现有技术不足，针对动态场景中的位姿估计问题，提供面向动态场景的点线融合语义slam方法，改善动态场景下位姿估计精度，提高低纹理场景下的系统稳定性能。
5.本发明的技术解决方案为：一种面向动态场景的点线融合语义slam方法，实现步骤如下：
6.(1)将rgbd相机采集到的rgb图像，通过语义分割网络分割出场景中的动态物体；同时对采集得到的深度图像进行聚类，得到当前图像的语义分割网络分割结果和深度图聚
类结果；
7.(2)对步骤(1)中获取的rgb图像同时提取点特征和线特征，对特征点进行完整保留不做处理，同时对线特征进行预处理，得到点特征和预处理后的线特征；
8.(3)结合步骤(1)中获得到的语义分割网络分割结果和深度图聚类结果，对步骤(2)中获取的点特征和预处理后的线特征进行筛选，将位于动态物体上的点、线特征进行剔除，得到静态点、线特征；
9.(4)通过步骤(3)中得到的静态点、线特征在动态场景下，进行比经典方法更稳定、精确的相机位姿估计。
10.相比于传统点特征的动态场景处理方法，采用点线特征改进后，系统更稳定，更精确。
11.所述步骤(1)中，将rgbd相机采集到的rgb图像，通过语义分割网络分割出场景中的动态物体；同时对采集得到的深度图像进行聚类，得到当前图像的语义分割网络分割结果和深度图聚类结果，方法如下：
12.针对rgbd相机同时获取得到的rgb图像和深度图像，同时开启两个线程处理。对于rgb图像，使用segnet语义分割网络进行语义分割，得到像素级别的语义分割结果。对于rgbd相机采集到的深度图，使用k-means算法将得到的深度图像聚类成成n个类别。在深度图聚类过程中，深度图像中深度值相近的区域会被归为同一类别，记录下深度图中每一类别的像素位置信息和对应的深度值，得到深度聚类结果。
13.所述步骤(2)中，对步骤(1)中获取的rgb图像同时提取点特征和线特征，对特征点进行完整保留不做处理，同时对线特征进行预处理，得到点特征和预处理后的线特征，方法如下：
14.对于步骤(1)中获得的rgb图像，同时提取点特征和线特征，点特征采用了经典的orb特征，线特征采用了具有速度快、无需调整参数的lsd线特征提取器提取。本发明在lsd线特征提取器基础上解决将一个长线段分割成几个短线段的问题和检测线段空间位置非常接近易产生干扰匹配的问题，进而增强了整个视觉定位系统的稳定性，方法利用线段的方向、线到线距离和端点到端点的距离三个参数来合并近线和消除短线，当两条线段端点之间的最小距离和一条线段中点到另一条线段的垂直距离均小于设定阈值5个像素则认为两个线段合并，得到一条附近没有近距离干扰线段的线段特征。
15.其次，当前技术条件下深度相机会在物体边缘处产生严重的误差，影响后续位姿估计，针对该问题，本发明采用了一种稳定快速的线特征筛选方法，在保存线特征深度信息可靠的条件下，尽可能多的保留线段特征。该方法首先计算被检测线段与非零深度值相交的像素的数量，然后采用均匀采样方法在直线上进行采样，统计采样点处的像素坐标，最后参照采样点的像素坐标，在深度图的对应位置上读取采样点的深度值。当采样点深度大于零，则认为采样点有效，否则无效。设定若线段上的采样点无效比例超过三分之二，则该线段为无效线段，将会被剔除。对于无效采样点少于二分之一的线特征，优先选择在无效采样点处断开。
16.至此完成对点特征和线特征的提取和预处理，将提取得到的特征点和预处理后的特征线分别保存。
17.所述步骤(3)中，结合步骤(1)中获得到的语义分割网络分割结果和深度图聚类结
果，对步骤(2)中获取的点特征和预处理后的线特征进行筛选，将位于动态物体上的点、线特征进行剔除，得到静态点、线特征，方法如下：
18.利用步骤(1)中获得到的语义分割网络分割结果，筛选出位于语义标签为“人”区域的特征点，并称其为初始动态点。将初始动态点，按照每个特征点的像素坐标，将特征点映射到对应深度聚类图中相应的像素坐标位置上，如果初始动态点落在某一深度聚类区域内，将该深度聚类区域标记为准动态区域。计算位于同一准动态区域内所有特征点的重投影误差，并求平均值，如果大于四个像素，则将该准动态区域称为动态区域。
19.利用动态区域在图像中像素位置的分布，对步骤(2)中保留的特征点和预处理后的特征线进行筛选。将位于动态区域的特征点剔除，检验线段特征的端点和中点检验是否位于动态区域，如果直线的两个端点和一个中点有任意一个位于动态区域，则该线特征为动态线特征，将动态线特征剔除，最终得到静态点、线特征。
20.所述步骤(4)中，通过步骤(3)中得到的静态点、线特征在动态场景下，进行比经典方法更稳定、精确的相机位姿估计，方法如下：
21.在得到可靠的静态点、线特征之后，同步开启三个线程：跟踪线程、局部建图线程和回环检测线程，其中跟踪线程主要通过跟踪局部地图中的点、线特征获得相机的位姿，局部建图线程通过新建或者删除路标点或者路标线，并结合从跟踪线程的多个位姿和回环检测的信息，对相机位姿进行统一的优化，得到更精确的相机位姿，回环检测线程主要是检测相机是否到达过先前位置，为优化提供参考。本发明主要在，跟踪线程和局部建图线程进行了改进创新，具体为：
22.通过深度图将二维点特征和线特征映射到三维空间建立初始化地图，通过最小化点、线特征的重投影误差求解相机位姿，首先将空间直线重投影到图像上，然后计算投影直线i’c
：
[0023][0024]
其中n∈r3是由直线和相机光心构成的平面π的法向量，l1，l2，l3为直线系数，k为投影矩阵，f
x
，fy，c
x
，cy为相机内参。
[0025]
然后计算投影直线i’c
和图像中的匹配线段ic的误差，将线特征的重投影误差e
l
表示为：
[0026][0027]
其中p
start
和p
end
分别为图像中线段特征的两个端点。
[0028]
点特征的重投影误差表示e
point
为：
[0029]epoint
＝z-h(t,p
point
)
[0030]
其中z代表了三维观测点p
point
在图像中的实际位置，h(t,p
point
)代表了三维空间点p
point
通过世界坐标系到像素坐标系的转换矩阵t得到的像素坐标。
[0031]
假设在第i帧图像中有j个静态特征点和k个静态线特征，同时假定点特征和线特征的观测误差服从高斯分布，得到最后的代价函数c为：
[0032][0033]
其中与表示点、线的观测协方差，ρ
p
(
·
)和ρ
l
(
·
)为鲁棒核函数，用来降低代价函数中的异常项：
[0034][0035]
其中e为点特征或线特征的观测方程，δ为可调节阈值，通过最小化代价函数，最终可求得相机的位姿。
[0036]
本发明与现有技术相比的优点在于：
[0037]
(1)本发明利用语义分割技术和空间结构性特征提取技术，针对传统视觉定位导航系统难以解决的动态场景，进行了系统上的改进，改进后，大大提高了系统在高动态场景中的位姿估计精度。
[0038]
(2)本发明相比于传统基于点特征的视觉导航系统，在低纹理场景下有更好的位姿估计精度，并且大大提高了系统的跟踪稳定性，能够很好的应对低纹理、弱纹理场景，在既有动态物体又缺少纹理的极端复杂场景的表现尤为突出，拓展了视觉定位导航技术的适用范围。
[0039]
总之，本发明采用的方法原理简洁，可达到在高动态和低纹理环境中相机实时位姿估计的目的。
附图说明
[0040]
图1为本发明面向动态场景的点线融合语义slam方法流程图。
具体实施方式
[0041]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例仅为本发明的一部分实施例，而不是全部的实施例，基于本发明中的实施例，本领域的普通技术人员在不付出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。
[0042]
如图1所示，本发明的具体实现步骤如下：
[0043]
步骤1、将rgbd相机采集到的rgb图像，通过语义分割网络分割出场景中的动态物体；同时对采集得到的深度图像进行聚类，得到当前图像的语义分割网络分割结果和深度图聚类结果。针对rgbd相机同时获取得到的rgb图像和深度图像，同时开启两个线程处理。对于rgb图像，使用segnet语义分割网络进行语义分割，得到像素级别的语义分割结果。针对rgbd相机采集到的深度图，使用k-means算法将得到的深度图聚类成n个类别。在深度图聚类过程中，深度图像中深度值相近的区域会被归为同一类别，记录下深度图中每一类别的像素位置信息和对应的深度值，得到深度聚类结果。
[0044]
步骤2、对步骤(1)中获取的rgb图像同时提取点特征和线特征，对特征点进行完整保留不做处理，同时对线特征进行预处理，得到点特征和预处理后的线特征。对于步骤(1)中获得的rgb图像，同时提取点特征和线特征，点特征采用orb特征，线特征采用具有速度快、无需调整参数的lsd线特征提取器提取。本发明在lsd线特征提取器基础上，解决将一个长线段分割成几个短线段的问题以及检测线段空间位置非常接近易产生干扰匹配的问题，进而增强整个视觉定位系统的稳定性。方法利用线段的方向、线到线距离和端点到端点的距离三个参数来合并近线和消除短线，当两条线段端点之间的最小距离和一条线段中点到另一条线段的垂直距离均小于设定阈值5个像素则认为两个线段合并，得到一条附近没有近距离干扰线段的线段特征。其次，基于当前技术条件下深度相机会在物体边缘处产生严重的误差，影响后续位姿估计的问题，本发明采用了一种稳定快速的线特征筛选方法，在保存线特征深度信息可靠的条件下，尽可能多的保留线段特征。该方法首先计算被检测线段与非零深度值相交的像素的数量，然后采用均匀采样方法在直线上进行采样，统计采样点处的像素坐标，最后，参照采样点的像素坐标，在深度图的对应位置上读取采样点的深度值，当采样点深度大于零，则认为采样点有效，否则无效。设定若线段上的采样点无效比例超过三分之二，则该线段为无效线段，将会被剔除。对于无效采样点少于二分之一的线特征，优先选择在无效采样点处断开。至此完成对点特征和线特征的提取和预处理，将提取得到的特征点和预处理后的特征线分别保存。
[0045]
步骤3、结合步骤(1)中获得到的语义分割网络分割结果和深度图聚类结果，对步骤(2)中获取的点特征和预处理后的线特征进行筛选，将位于动态物体上的点、线特征进行剔除，得到静态点、线特征。利用步骤(1)中获得到的语义分割网络分割结果，筛选出位于语义标签为“人”区域的特征点，并称其为初始动态点。将初始动态点，按照每个特征点的像素坐标，将特征点映射到对应深度聚类图中相应的像素坐标位置上，如果初始动态点落在某一深度聚类区域内，将该深度聚类区域标记为准动态区域。计算位于同一准动态区域内所有特征点的重投影误差，并求平均值，如果大于四个像素，则将该准动态区域称为动态区域。
[0046]
利用动态区域在图像中像素位置的分布，对步骤(2)中保留的特征点和预处理后的特征线进行筛选。将位于动态区域的特征点剔除，检验线段特征的端点和中点检验是否位于动态区域，如果直线的两个端点和一个中点有任意一个位于动态区域，则该线特征为动态线特征，将动态线特征剔除，最终得到静态点、线特征。
[0047]
步骤4、通过步骤(3)中得到的静态点、线特征在动态场景下，进行比经典方法更稳定、精确的相机位姿估计。在得到静态点、线特征之后，同步开启三个线程：跟踪线程、局部建图线程和回环检测线程。其中，跟踪线程通过跟踪局部地图中的点、线特征获得相机的位姿，局部建图线程通过新建或者删除路标点和路标线，并结合跟踪线程获得的多个相机位姿以及回环检测的信息，对相机位姿进行统一的优化，得到更精确的相机位姿，回环检测线程是通过检测相机是否到达过先前位置，为优化提供参考。本发明在跟踪线程和局部建图线程进行了改进创新，具体为：
[0048]
通过深度图将二维点特征和线特征映射到三维空间建立初始化地图，通过最小化点、线特征的重投影误差求解相机位姿。首先将空间直线重投影到图像上，计算投影直线i’c
：
[0049][0050]
其中n∈r3是由直线和相机光心构成的平面π的法向量，l1，l2，l3为直线系数，k为投影矩阵，f
x
，fy，c
x
，cy为相机内参。
[0051]
然后计算投影直线i’c
和图像中的匹配线段ic的误差，将线特征的重投影误差e
l
表示为：
[0052][0053]
其中p
start
和p
end
分别为图像中线段特征的两个端点。
[0054]
点特征的重投影误差e
point
表示为：
[0055]epoint
＝z-h(t,p
point
)
[0056]
其中z代表了三维观测点p
point
在图像中的实际位置，h(t,p
point
)代表了三维空间点p
point
通过世界坐标系到像素坐标系的转换矩阵t得到的像素坐标。
[0057]
假设在第i帧图像中有j个静态特征点和k个静态线特征，同时假定点特征和线特征的观测误差服从高斯分布，得到最后的代价函数为：
[0058][0059]
其中与表示点、线的观测协方差，ρ
p
(
·
)和ρ
l
(
·
)为鲁棒核函数，用来降低代价函数中的异常项：
[0060][0061]
其中e为点特征或线特征的观测方程，δ为可调节阈值，通过最小化代价函数，最终求得相机的位姿。
[0062]
尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，且应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。