基于虚拟特征点的视觉里程计运动物体干扰消除方法∗

闫 铭 王 欢 崔琼杰

（南京理工大学计算机科学与工程学院 南京 210094）

1 引言

视觉里程计（Visual Odometry［1］）是通过单个或者多个视频传感器的输入估计运载体（如车辆、机器人）自身运动的过程，由于其成本低、获取信息容易的特点，应用领域涵盖机器人、增强现实和自动驾驶等［2］。视觉里程计相比于传统的轮式里程计，视觉里程计（VO）的优势是不受车轮在不平整地面或其他不良状况下打滑的影响。在GPS 失效的情况下，视觉里程计是解决机器人在这些环境下精确自主定位问题的一种有效技术途径。

解决移动物体干扰比较常见的方法是在视觉里程计中加入一个代价函数，用来降低移动物体特征点的贡献度从而降低其对于视觉里程计位姿估计的干扰。Gutierrez［11］等比较了不同的损失函数对于位姿估计的作用，Kerl［12～13］等证明了这种方法对于场景中出现小的移动物体时的鲁棒性；在ElasticFusion［14］和Keller［15］等的方法中，当一个特征点在多帧连续图像中都被观测到才算入内点。尽管他们从不同角度对算法进行了改进，然而这些方法都没有对场景中移动元素进行检测和处理，因此当移动物体在图像中占比较大时，这些方法很难取得好的效果。另一种解决策略是检测并过滤掉移动物体。Choi 等［16］根据运动物体的深度变化不同于静态特征这一现象，统计双目图像中特征的深度置信地图来进行运动物体检测；文献［17～18］将光流或场景流计算引入视觉里程计中，通过判断特征的光流或场景流进行运动物体检测。虽然上述方法能够剔除部分动态特征，但是容易在静态特征较少时，因用于位姿估计的匹配点对太少而造成位姿估计误差大。

针对这些问题，本文提出了一种移动物体干扰情况下的视觉里程计优化方法。它通过特征匹配点对之间在图像中移动的距离，统计特征点移动距离直方图，并选取合适的阈值来区分静态场景和移动物体的特征点，从而保证了对图像中存在多个运动物体的情况的鲁棒性。同时，我们充分利用图像所提取到的移动物体上的特征点信息，依据移动物体运动的平滑性约束，通过当前图像和上一时刻图像中移动物体的特征点位置预测出下一时刻移动物体特征点的位置，在局部地图中构造下一时刻的虚拟特征点，最后，通过下一时刻图像特征点与包含虚拟特征点的局部地图的匹配关系进行位姿估计。实际场景实验表明，本文提出的方法提高了移动物体干扰下视觉里程计算法的精确性和鲁棒性。

2 系统概述

本文视觉里程计的实现基于特征点的方法。特征点的提取和匹配是特征点法视觉里程计中尤为关键的一步。常用的特征点提取技术有SUSAN［19］，Harris［20］，SIFT［21］，SURF［22］等。Rublee 等在2011 年 提 出 了ORB［23］（Oriented FAST and Rotated BRIEF）算法，它使用改进的FAST［24］算法来检测特征点，有着极快的检测速度，并且通过改进FAST角点并结合描述子，取得了良好的效果。总地来说，ORB 是质量与性能之间较好的折中。本文也将采用ORB作为我们的特征点提取算法。

本文的视觉里程计系统框架如图1 所示，主要包括特征提取与位姿估计两个部分。在特征提取阶段，我们对每一帧Fk提取ORB 特征点，并检测和区分静态场景和移动物体特征点；在位姿估计阶段，我们将当前RGB-D 图像帧提取到的特征点与局部地图建立匹配关系，通过RANSAC PnP 估计相机位姿，然后使用g2o（General Graph Optimization）［26］通过最小化重投影误差进行位姿迭代优化，最后更新局部地图点以及虚拟特征点。

图1 系统框架图

3 基于帧间特征点移动距离直方图的移动物体特征点检测

针对场景中的移动物体干扰问题，目前最为成熟、有效的鲁棒估计方法就是随机采样一致性算法（Random Sample Consensus，RANSAC）。在视觉里程计，RANSAC 对于普通的图像噪声或者图像中只存在小范围的运动物体，能够取得非常好的效果。但是如果摄像机所采集的图像中运动物体占据了很大一部分，RANSAC 算法将会失效，影响最终的运动估计。本文针对这一问题，提出了一种基于帧间特征点移动距离直方图的移动物体特征点检测方法。

当场景中有移动物体干扰时，我们提取到的特征点会有一部分落在移动物体上。我们对包含移动物体干扰场景中的图像进行特征检测，为了更加直观地进行展示，图2 中给出了几对图像帧间特征点的检测和匹配结果。

图2 包含移动物体场景下的特征点检测及匹配结果

可以注意到，虽然相机自身的运动使静态场景和移动物体在图像中的位置都是变化的，但是由于移动物体自身的运动，使得静态场景中的特征点和移动物体上的特征点在图像中的移动距离是不同的，如图3。根据这一现象，本文提出一种基于图像平面特征点移动距离直方图的移动物体特征点检测方法。

太湖流域综合规划经国务院批复，太湖流域水环境综合治理总体方案水利部分修编完成，太湖流域及东南诸河水中长期供求规划全面开展，流域水利规划体系基本形成。在流域水利工作中，充分发挥规划的指导约束作用，坚持统筹协调，有力促进了太湖流域治理与管理工作；加快推进流域水环境综合治理，完成太浦闸除险加固主体工程建设，新沟河延伸拓浚工程、苕溪清水入湖河道整治工程项目可研已经发改委批复，新孟河延伸拓浚工程可研报告已经中咨公司评估，其他项目前期工作有序推进。经过有关各方的共同努力，2013年太湖流域水环境综合治理成效明显。监测数据表明，太湖及河网水质进一步得到改善，太湖连续六年实现“安全度夏”。

图3 图像平面特征点移动距离图

对于连续两帧有移动物体干扰的图像kt-1和kt，分别提取ORB特征点并进行匹配。对于每一个kt中的特征点pt及其与kt-1中匹配的特征点pt-1，其在图像kt和kt-1的位置坐标分别为(ut,vt)和(ut-1,vt-1)，对图像kt中每一个特征点pt计算其在图像上移动的距离d ：

对kt中每个特征点在图像上移动的距离统计直方图，图4 给出本文实验数据集中包含移动物体图像帧的统计结果。

图4 特征点移动距离统计直方图

由图4 特征点移动距离统计结果可知，在包含移动物体的场景中，图像中静态场景和移动物体上的特征点的移动距离存在差异性。我们将区分静态背景和移动物体的移动距离阈值记为T ；对于当前图像所提取的特征点及其统计到的特征点移动距离，将特征点总数量记为N ，属于静态背景的特征点数量记为N1，属于移动物体的特征点数量记为N2，那么：

其中ω1表示属于静态背景的特征点数量占所有提取到的特征点的数量比例；ω2表示属于移动物体的特征点数量占所有提取到的特征点的数量比例。

式中μ1表示静态背景特征点的平均移动距离；μ2表示移动物体上特征点的平均移动距离；μ 表示图像中所有特征点的总平均移动距离。

g 为计算出的静态背景特征点与移动物体特征点的类间方差。将式（3）带入式（4）得到：

图5 移动物体特征点检测结果

遍历所有T 的可能取值，最终得到使得g 最大的T 作为最终阈值从而来区分静态场景特征点和移动物体上的特征点。图5 中可以看到，我们的方法可以比较好地区分出静态场景和移动物体上的特征点。

4 基于虚拟特征点的相机位姿估计

4.1 虚拟特征点生成

对于静态场景中的特征点，我们使用文献［25］的方法构造与维护局部地图，并将当前帧与地图点进行匹配，计算位姿。同时，本文中充分利用图像所提取到的移动物体上的特征点信息，利用其生成虚拟特征点加入局部地图用于视觉里程计的位姿估计。

对于当前t 时刻图像帧所提取到的移动物体上特征点集合M 中的每一个深度已知的特征点，其像素坐标为(u,v)，深度为z，将其投影为局部地图点Pt：

其中，Rcw和tcw分别为相机当前坐标系与世界坐标系变换的旋转矩阵和平移向量；(fx,fy)为相机焦距；(cx,cy)为光学投影中心坐标。

我们假设行移动物体的运动是一个连续性的过程，满足物体运动平滑约束。于是我们在局部地图中加入生成的虚拟特征点，用于下一时刻图像特征点与局部地图点的匹配。

图6 虚拟特征点生成原理图（L为局部地图点集合，R为移动物体区域特征点集合，V为根据（14）生成的虚拟特征点）

4.2 相机位姿估计

根据当前帧图像的特征点与包含虚拟特征点的局部地图点的匹配关系，我们首先由RANSAC PnP 求得相机的位姿作为初值，然后通过最小化重投影误差的迭代优化方式估计相机位姿。考虑相机坐标系下n 个三维空间点pi，其投影的像素坐标为ui，像素位置与空间点位置的关系如下：

式中K 为相机内参矩阵。

由于噪声影响，式（9）存在误差。因此，我们构建最小二乘问题，最小化重投影误差来获得最终的相机位姿：

5 实验

5.1 实验环境和设置

本文实验所用电脑配置为：2.6GHz Intel Core i7-4720HQ CPU，内存大小为8G，实验中未使用GPU 加速，算法运行系统为Ubuntu 16.04。我们使用包含移动物体干扰的数据集对本文提出的动态场景下视觉里程计优化方法进行评估，同时，与同等条件下不运行本文提出的移动物体处理算法进行对比，来检验本文提出的视觉里程计方法在动态场景下的有效性。此外，我们将本文提出的方法与特征点法中具有代表性的ORB-SLAM［25］进行实验对比。由于ORB-SLAM是一个完整的SLAM系统，公平起见，我们关掉了ORB-SLAM 的回环检测和全局优化，只保留其视觉里程计部分（本文中称为ORB-VO）与本文方法进行实验对比。

5.2 数据集与评测指标

TUM［27］数据集是慕尼黑工业大学实验场景数据集，其中fr3 系列数据集是一个典型的动态场景数据集，提供了一系列存在不同程度移动物体干扰的室内RGB-D 数据集，经常用来评估视觉里程计算法在移动物体干扰条件下的精确性和鲁棒性。本文中使用TUM fr3 系列数据集来评估本文提出的方法在动态场景中的表现。

本文使用相对位姿误差（RPE）作为本实验的评估准则。相对位姿误差（RPE）用来衡量在一定时间间隔Δ 内估计轨迹的局部精度。因此，相对位姿误差（RPE）可以用来衡量轨迹漂移的程度。在i 时刻的相对位姿误差定义为其中Qi和Pi分别为i 时刻的真实位姿和估计位姿。另外，我们进一步计算估计位姿平移RPE和旋转RPE 的均方误差（RMSE）来评估本文提出的视觉里程计优化方法的表现。实验中Δ=1。

5.3 实验结果分析

表1 中给出walking_static 据集下的实验结果。我们将同等条件下运行本文提出的优化方法与不运行优化算法的结果进行对比，来检验本文提出的视觉里程计方法的精确性和鲁棒性。

表1 RMSE对比

图7 给出了两种方法在当前移动物体干扰场景数据集下的平移和旋转的相对位姿误差。

图7 相对位姿误差对比图

图7 中（a）、（b）可以看到，由于场景中人体移动的影响，如果不对场景中的移动物体进行处理，会导致位姿估计出现较大的偏差；加入本文提出的优化方法后，如图7 中（c）、（d），本文的方法可以有效地针对场景中存在的移动物体干扰，仍保持较为准确的位姿估计。这是由于本文的方法中考虑了场景中移动物体上的特征点，避免了这些特征点错误的与静态特征点匹配，而是通过这些特征点生成虚拟特征点与局部地图匹配，从而减少了场景中移动物体对于视里程计算法的错误影响。

与ORB-VO 的对比实验结果如表2 所示（加粗部分为误差较小者）。在低程度的动态场景中（表2 中前四个数据集），本文提出的方法与ORB-VO的位姿估计误差相当。而当场景中移动物体运动幅度较大且在图像中占比较高时（表2 中后三个数据集），本文提出的方法位姿估计效果明显优于ORB-VO。

表2 fr3各数据集RMSE对比

从以上实验中可以看到，本文提出的算法针对场景中存在移动物体干扰的情况下，很好地克服了移动物体对于视觉里程计算法的影响，且能保持较强的鲁棒性。

6 结语

如何在动态环境下仍然保持比较准确的定位和建图精度，是现有的众多SLAM 和视觉里程计方法的挑战之一。本文针对这一问题提出了一种基于虚拟特征点的视觉里程计运动物体干扰消除方法。通过对静态场景和运动物体上的特征点加以区分，并生成虚拟特征点加入局部地图用于位姿估计的方法，旨在提高动态场景下视觉里程计算法的定位和位姿估计的精确性和鲁棒性。对比实验表明，本文提出的动态场景下视觉里程计优化方法针对动态场景下存在移动物体干扰情况下的相机位姿估计有较大提升。