基于目标检测和动静点分离的视觉即时定位与地图构建技术

□ 来金强

宁波大学 机械工程与力学学院 浙江宁波 315211

1 研究背景

对于视觉即时定位与地图构建而言,真实环境往往存在复杂的动态现象,而很多开源方案都假设相机在一个理想的静态环境中运动。为了减少环境中动态信息的干扰,一般采用几何约束设置阈值,或使用光流法等。Wang Runzhi等[1]运用极线几何约束寻找外点,应用聚类信息进一步分离动态物体,但这一方法在高动态环境中鲁棒性丢失较为严重。艾青林等[2]提出一种改进的几何和运动约束方法,但在数据集部分序列相对于静态即时定位与地图构建,精度有所下降。

目前,产生了很多与深度学习相结合的技术方案。Bescos等[3]提出DynaSLAM,使用Mask-区域卷积神经网络[4]进行语义分割,同时使用多视图几何方法判断外点,但计算量大,且实时性差。Palazzolo等[5]提出ReFusion方案,在KinectFusion[6]稠密系统的基础上,使用纯几何方法分割动态区域,通过自适应阈值和形态学处理得到最终动态区域,但实时性下降严重。

针对动态场景下的视觉即时定位与地图构建鲁棒性丢失,采用语义分割实时性差等问题,笔者提出一种基于目标检测和动静点分离的视觉即时定位与地图构建技术。这一技术使用一阶段算法YOLO v5将图像分为框内框外区域,通过观测点的运动分离出可用静点,将特征点分类为框外静点、框内可用静点、动点,并一起纳入位姿优化。

2 视觉即时定位与地图构建系统

2.1 系统框架

笔者所提出的系统框架是在ORB-SLAM2[7]的基础上新增三个部分,如图1所示。

在图像输入前端增加YOLO v5的语义检测线程,在跟踪线程中对优化相机位姿的光束法平差进行了改进,使用滤除动态物的关键帧信息进行稠密建图。

在语义检测线程中,YOLO v5对系统输入的图像进行检测,将检测结果,即带检测框的图像返回至跟踪线程。检测结果用于区域划分。在室内环境中,一般只存在人这个动态物,因此只将人视为动态目标物。

在改进光束法平差优化中,由于YOLO v5的检测框内除了动态目标物外还存在部分静态物体和背景区域,因此为了尽可能利用已知信息,通过运动约束的方法对框内区域进行动静点分离,并对不同类型的点进行权重分配。

稠密建图线程将舍弃检测框区域内的点云信息显示,并使用优化后的位姿进行点云拼接和地图重建。

2.2 目标检测和区域划分

YOLO v5是一种实时性高的轻量型网络,使用较高性能的显卡可以在计算机端达到每秒30帧图像的处理效率,符合即时定位与地图构建系统对实时性的要求,而语义分割算法,如DynaSLAM使用的Mask-区域卷积神经网络,在计算机端的速度仅为10秒一帧图像。

语义检测线程以多线程并行的方式,对输入的图像进行动态目标物检测,并将检测结果返回至ORB-SLAM2的跟踪线程,包括检测框的类别、宽度w、高度h,以及中心坐标值(px,py)。

通过检测框的四个边界坐标,将图像划分为框内区域和框外区域。这四个坐标分别为(px±0.5w,py±0.5h)。对于框外区域的特征点,默认为静点。

2.3 随机抽样一致性算法

当动态目标物所占的图像区域较小或者图像模糊时,YOLO v5存在检测失败的可能,导致部分人身上的动点误判为静点。对此,对检测框外区域的特征点采取随机抽样一致性算法。首先选取八个分散的框外特征点,使用八点法估计基础矩阵F,再对其余点进行对极约束。

对极约束原理如图2所示,点O1、O2为相机光心,点P是物体上某个特征点,在I1、I2时刻的图像投影分别为P1、P2。面O1O2P与I1和I2的交线l1、l2记为极线,l1可表示为:

(1)

式中:x1为P1的归一化坐标。

若点P2到l2的距离D可表示为:

(2)

式中:x2为P2的归一化坐标。

由于计算所得的基础矩阵有一定误差,因此设定阈值ε,当D大于ε时,判断点P为动点。迭代k次之后,若动点数量占比小于5%,则停止。

2.4 运动规律和动静点分离

对于框内区域的特征点,根据静点和动点运动规律的不同来区分。静点在任意两个时刻的速度变化差值和速度之和都接近0,动点在短时间内速度变化差的绝对值很小,远小于前后两时刻速度之和的绝对值。由此建立一个观测模型,并计算t时刻特征点的运动速度Vt为:

(3)

(4)

式中:Tcw_t为t时刻世界坐标系到相机坐标系的变换矩阵;K为相机内参矩阵;Pt为t时刻的投影坐标。

由上述运动规律,建立动点的分离条件,为:

‖Vt+1-Vt‖1<λ‖Vt+1+Vt‖1

(5)

式中:λ为工程经验值,需要大于10。

对于框内区域满足式(5)的点,视为动点,剩余的点视为可用的静点。可用的静点指在一段时间内没有体现出动点规律而被标记的点,若发生了运动现象,则需要标记为动点。因此,需要对框内的点持续观测计算,dt取前后各五个关键帧的时间差。

在TUM数据集[8]上做点的分离测试,特征点分离和标记如图3所示。框外使用随机抽样一致性算法保留后的点标记为白色,框内区域的可用静点标记为灰色,动点则标记为黑色。可以看出,这一策略能有效区分各类特征点。

2.5 改进光束法平差优化

ORB-SLAM2跟踪线程的光束法平差目标函数是最小二乘形式,为:

(6)

式中:ρ为鲁棒核函数;{R,t}为相机位姿变换矩阵;R为相机旋转矩阵;t为相机平移向量;xi为点的像素坐标系坐标;Xi为点的世界坐标系坐标;π()函数用于将相机坐标系坐标转为像素坐标系坐标;E为三阶信息矩阵。

E用于引导误差项的求解方向,可表示为:

(7)

式中:n为图像金字塔层数。

在原目标函数中加入可用静点的误差项,并在该误差项的信息矩阵上乘以因数,以降低权重分配。对于误差项,需要使用高斯-牛顿迭代法不断迭代求解,于是有:

(8)

Eη=ηE

(9)

式中:xj为检测框内保留的可用静点的像素坐标系坐标;Xj为检测框内保留的可用静点的世界坐标系坐标;Eη为保留可用静点的信息矩阵;η为权重因数,默认小于1。

3 性能测试

3.1 数据集

在公开的数据集TUM RGB-D中测试算法性能。该数据集有专门测试动态的序列Dynamic Objects,其中的fr3序列是室内动态场景,序列中的前缀sitting和walking表示场景中人物的运动形式,后缀halfsphere、rpy等表示相机的主要运动路径。数据集自带真实的轨迹文件groundtruth,可以通过两个指标对算法进行评估,分别为绝对轨迹误差和相对位姿误差。

3.2 测试结果

笔者方法和ORB-SLAM2对于序列sitting_half和walking_half的绝对轨迹误差如图4、图5所示。由图4可知,笔者方法在低动态的sitting_half序列和高动态的walking_half序列得到的轨迹均优于ORB-SLAM2。walking_half序列中,ORB-SLAM2的轨迹已经严重偏移,基本丧失了鲁棒性。

为进一步验证笔者方法的鲁棒性和精度,对fr3所有序列进行测试,每一个序列测试14次,去除精度最高和最低的四个序列后,记录绝对轨迹误差的均方根值,并与其它算法、方法对比,见表1。

表1 绝对轨迹误差均方根值对比 m

从表1中数据可以看出,笔者方法的优胜序列较其它算法、方法更多,且所有序列均保持较高的精度。

与ORB-SLAM2+YOLO v5的简单结合相比,笔者方法在各个序列中精度均有不同程度的提升,这表明笔者提出的区域划分和特征点分离方案可以有效提升轨迹精度。

最终得到的稠密点云地图如图6所示。笔者的稠密建图线程根据语义检测线程确定动态区域,只使用框外区域的点云信息,从而有效滤除动态信息,构建静态地图。

4 结论

笔者提出了一种针对动态环境的,基于目标检测和动静点分离的视觉即时定位与地图构建技术。使用YOLO v5网络进行检测,将检测结果用于图像区域划分。对检测框内的点做进一步划分后,在位姿计算中赋以相应权重。在TUM数据集的fr3序列walking前缀动态测试中,笔者方法的绝对轨迹误差相比ORB-SLAM2减小了90%以上,并且在所有fr3序列中的精度均高于YOLO v5与ORB-SLAM2的简单结合。在下一步工作中,将尝试在视觉惯性里程计系统中增加目标检测和去动态处理,通过惯性测量单位信息来提高系统鲁棒性。