基于点线特征融合的立体视觉里程计

高 翀，黄影平，赵柏淦，胡 兴

（上海理工大学光电信息与计算机工程学院，上海 200093）

引 言

视觉里程计（visual odometer, VO）利用单个或多个相机获取的图像流来估计相机的位姿参数[1]，进而推算相机的运动轨迹。目前，VO广泛应用于智能汽车、移动机器人、无人机等智能体，是智能体实现自主定位导航的一个重要方法。

根据利用的图像信息不同，可分为直接法和特征法视觉里程计。直接法[2-3]利用图像或某个子区域中所有像素的灰度信息计算相机的运动，基于帧间像素灰度不变假设，这一假设在光照改变时容易遭到破坏。特征法匹配跟踪连续图像序列中的一些显著特征，依据特征信息在帧间的位置变化来估计位姿。相对而言，使用特征法的VO系统运行稳定，计算成本低，对光照、图像噪声等不敏感。特征法VO[4-6]大多选取点作为特征进行位姿估计，典型的算法有ORB-SLAM2[5]和SOFT2[6]。ORB-SLAM2是著名的开源系统，提取ORB（oriented FAST and rotated BRIEF）点特征进行帧间匹配，由跟踪、建图及回环检测三个并行线程组成；SOFT2使用类角点作为点特征，分开求解相机旋转和平移参数，在KITTI算法排行榜中靠前。

然而，在一些弱纹理场景中，如图1所示，图像中的点特征并不丰富，存在无法提取到足够多的点特征，或者点特征分布不均的情况，这就会导致系统性能的下降乃至失效。另外，线是点的集合，相对于离散的点，帧间线特征的位置变化更具有显著性，有利于提高特征检测跟踪的鲁棒性。因此，一些基于线特征的VO系统被提出[7-9]。文献[7]率先提出在扩展卡尔曼滤波视觉定位与地图构建系统中使用线特征，他们采用了一种假设-检验的方法检测直线。文献[9]中将迭代最近点（ICP）算法调整应用于线特征中，在非线性优化过程中考虑一对多的线段匹配，提出迭代最近多条线（ICML）视觉里程计方法。该方法针对高速运动的视频序列，在相机旋转度较小时可以得到精度良好的位姿估计值。但是单纯使用线特征有其局限性，尤其在纹理度较高的场景中，检测到的线段数量减少或质量减弱时，单纯使用线特征的算法精度下降。文献[10]提出采用点线组合特征可以给算法提供更好的几何约束，有效提高系统鲁棒性，他们从RGB-D图像中提取3D点和线段，使用最大似然估计计算相机位姿。文献[11]同样将点线特征结合，通过计算线段端点的亚像素视差进行线特征匹配。文献[12]采用点线组合特征建立重投影误差模型，并且根据点线特征的检测误差协方差矩阵对点线特征的重投影误差加权，通过非线性最小化此重投影误差来估计相机位姿。文献[13]在ORB-SLAM2的基础上增加线特征，提出基于线的地图初始化方法，在帧间旋转幅度较小的假设条件下，根据连续3帧图像中的5条线间的对应关系估计相机位姿。

图 1 弱纹理场景Fig. 1 The low-textured scene

本文采用点线结合作为特征，提出了一种基于点线特征融合的立体视觉里程计算法，在算法设计上有如下创新。1）构建新颖的点线重投影误差模型作为目标函数，模型中，使用Huber核函数减小误匹配特征对优化过程的影响。使用列文伯格-马夸尔特法求解所构建的点线误差模型，得到旋转矩阵和平移向量，再利用球形线性插值技术对旋转参数进行进一步优化。2）在特征的提取与匹配阶段，对点特征施加环形匹配和Bucketing技术，对线特征施加外观几何约束及环形匹配，有效提高特征匹配的精度。进行匹配的时候施加恒速约束缩小搜索范围，匹配质量的提高增加了内点的比例，从而减少位姿估计时的迭代次数，有效提高了运行速度，实现了鲁棒性与实时性间的平衡。3）在公共数据集KITTI和EuRoC中进行实验验证，证明了本文算法在多种场景中的鲁棒性能，相较于其他点线特征的VO以及具有代表性的点特征VO，本文算法在精度方面有所提升。

1 方法总体框架

图2为本文所提基于点线特征融合的立体视觉里程计算法框图，分为特征提取和运动估计两个部分。系统输入为 k −1 帧和 k帧图像， l 和 r 分别代表左右帧图像，输出为 k −1 帧与 k 帧间的旋转矩阵和平移向量

图 2 方法总体框图Fig. 2 The overall framework of the method

首先对获取的第 k 帧左右两帧图像同时提取ORB和LSD（line segment detection）特征并进行匹配，在此阶段，对ORB施加恒速、点环形匹配及Bucketing约束，对LSD进行外观几何约束及线环形匹配约束以提高特征匹配的精度及速度。然后在连续图像序列中分别跟踪点线两种特征，计算点特征及线特征端点的空间位置，将所得到的空间位置投影到下一帧图像中，构建点线重投影误差模型。最终由列文伯格-马夸尔特法求解所构建的点线误差模型得到旋转及平移参数，同时结合球形线性插值技术对旋转参数进行优化。

2 点特征及其处理

选用ORB算子[14]检测点特征。对检测到的点作环形匹配，匹配的时候采用BRIEF描述子在一定的搜索范围内进行，过程如图3所示。在左右帧与前后帧间根据的顺序进行环形匹配，匹配的终点与起点相同时匹配成功，这样的处理极大地提高了帧间匹配的精度。

图 3 环形匹配与恒速约束Fig. 3 Correspondence matching between left-right and consecutive images with a loop process(yellow part indicates the search area)

为了缩小特征匹配时的搜索范围以提高匹配速度，提出恒速约束模型。在相机帧率较高时其帧间运动变化较小，相机运动具有平滑性。假设相机在帧间匀速运动，通过重投影可以预测匹配点的粗略位置，进而以预测位置为中心进行匹配搜索。记空间点 P 在第 k 帧上的投影点为 pk，它的预测位置 pk−pre可以由下式计算：

式中： pk−1为点 P 在 k −1 帧时的世界坐标； K(·)为相机投影矩阵；为 k −1 帧的位姿变换矩阵。对于左右帧图像，在搜索区域为 δu×δv的矩形框内进行搜索匹配；对于前后两帧图像，搜索区域为半径为 δr的圆，圆心为特征点的预测位置。如图3所示，黄点为待匹配的点特征，黄色圆表示半径为 δr的前后帧间搜索区域，黄色矩形框表示 δu×δv大小的左右帧间搜索区域。

此外，采用Bucketing技术[6]将图像划分为50pixel×50pixel 大小的矩形框，并确保在每个矩形框中提取一定数目的点特征，使得所提取的 ORB特征可以均匀分布在图像中。

3 线特征及其处理

选用LSD算子[15]提取线特征，可在线性时间内得到亚像素级精度的检测结果。对提取到的LSD线特征计算LBD描述子[16]并进行匹配，同时舍弃具有不同长度及方向的线特征对，确保得到最佳匹配对，这可以有效提高线特征的匹配精度。

4 点线重投影误差模型

4.1 空间直线的几何表示

式中： Kl表示线段的投影矩阵； f 为相机焦距； cx， cy是像素坐标相对于图像坐标的偏移量； Q 表示线段的变换矩阵，包含旋转矩阵R和平移向量 t ； [·]1:3表示取前三维向量； [·]×表 示向量的反对称矩阵。

图 4 线特征重投影误差模型Fig. 4 The re-projection error model of line features

4.2 线特征的重投影误差

4.3 点线重投影误差模型

构建点线误差模型时，通常直接将重投影误差的二范数平方和作为求解的目标函数。但由于匹配过程存在误匹配，这会导致在目标函数中二范数增长速度过快，此时优化求解算法会主要集中调整错误的误差项的值，进而忽略正确的误差项的影响。通过在目标函数中增加Huber核函数，可以平衡每个误差项对优化过程的影响。因此，构建点线重投影误差模型如下：

5 实验与分析

选取公共数据集KITTI和EuRoC对本文所提算法进行精度、鲁棒性的实验论证。采用业内普遍使用的两种评判标准来评估算法的准确性：1）平均旋转误差（average relative rotational Error, ARE）和平均平移误差（Average Relative Translation Error, ATE），如文献[18]所定义；2）绝对轨迹误差（Absolute Trajectory Root-Mean-Square Error, AT-RMSE），如文献[19]所定义。

实验采用Intel（R）Core（TM）i5-7200U（2.5 GHz）4核处理器。算法在Ubuntu16.04系统中实现，基于C++和OpenCV库编写。

5.1 KITTI数据集

KITTI数据集[18]共包含22个立体图像序列，序列00～10给定了真实运动位姿，包含市区、乡村和高速公路等实际运动场景，行驶距离从500 m至5 000 m不等，帧率为10 frame/s，图像大小为1 241 pixel × 376 pixel。

使用KITTI数据集的00-10序列对本文所提算法进行测试，并与ORB-SLAM2[5]、SOFT2[6]和PLSVO[12]进行精度及实时性的对比。ORBSLAM2是著名的开源系统，采用ORB点特征；SOFT2在KITTI排行榜上基于视觉方法的里程计中排行第一，选用类角点作为特征；PLSVO是Ruben等人提出的一个结合概率方法与点线特征的视觉里程计系统。实验分别对00-10序列进行5次测试，取平均值作为最终结果。

图5为KITTI下的点线特征提取结果。黄色小点表示提取到的ORB点特征，红色线段为提取到的LSD线特征。图6展示了这四种算法对00-10序列测试得到的ARE、ATE及AT-RMSE的对比结果。与PLSVO相比，本文算法在所有序列上表现更佳，误差值更小。与ORB-SLAM2和SOFT2相比，本文算法总体上与它们相近，在多个序列表现更佳，如序列00和序列06，这些表现效果更佳的序列多为城镇场景，富含结构化特征明显的建筑物，存在较多易于提取的线特征，此种场景下，更为明显地体现了线特征的优势。

图 5 KITTI数据集点线特征提取结果Fig. 5 Point and line feature extracting results of KITTI

图 6 KITTI 00～10序列的ARE、ATE与AT-RMSEFig. 6 The ARE, ATE and AT-RMSE of KITTI 00-10

表1展示了这四种算法对KITTI 00～10序列综合评估的ARE和ATE，本文算法的ARE为0.20(◦)/100m ，ATE为0.69%。总体上，我们的算法相较于ORB-SLAM2和PLSVO在精度上有所提升，与SOFT2的精度值相近。我们的算法全面优于PLSVO算法，证实了本系统所采用的点线误差模型以及特征匹配机制的有效性。图7所示为使用本文算法对KITTI 00和06序列的轨迹构建（蓝色实线）以及与真实轨迹（黑色虚线）的对比图。

表 1 KITTI 00～10序列的综合评估ARE和ATE对比结果Tab. 1 The comparison of ARE and ATE on KITTI 00-10

表2对比了四个系统的每帧图像的平均消耗时间。可以看到，本文算法略慢于ORB-SLAM2和SOFT2，快于PLSVO。这是因为我们同时提取点线特征，而ORB-SLAM2和SOFT2仅需提取点特征，所以本文算法略慢是合理的。但是，在特征提取与匹配阶段，施加了恒速约束限制搜索范围，施加外观几何约束和环形匹配约束提高匹配精度，有效提高了内点比例，减少优化过程中的迭代次数，因此我们的算法速度快于PLSVO。

表 2 平均每帧时耗对比Tab. 2 The comparison result of consuming time per frame

表 3 EuRoC数据集11个序列的绝对轨迹误差（AT-RMSE）对比结果Tab. 3 Comparison of AT-RMSE on EuRoC dataset

图 7 KITTI 00与06序列的轨迹重构图Fig. 7 Trajectory reconstruction of KITTI sequences 00 and 06

5.2 EuRoC数据集

EuRoC数据集[20]是由微小型飞行器（Micro Air Vehicle, MAV）在三个不同场景中采集得到的11个图像序列。根据MAV的速度、场景光照条件及纹理信息，将这11个序列分为三个难度等级，即容易、中等和困难。表3为本文算法在EuRoC数据集所有序列上的绝对轨迹误差值，并与ORB-SLAM2和SOFT2进行对比。由于PLSVO文献作者并未进行EuRoC数据集下的实验，所以这里未与之进行比对。从结果可以看出，本文所提点线误差模型在此数据集下具有良好的表现，误差控制在厘米级别。EuRoC数据集都为室内场景，室内人造场景下，存在更多易于提取的线特征，其中序列V1_01_easy、V1_03_difficult中线特征丰富，点特征数量较少且存在分布不均的情况，场景中纹理信息较弱，综合点线特征的系统比ORB-SLAM2和SOFT2表现更佳。图8为EuRoC数据集中图像的点线特征提取结果。图9为V1_01_easy和V1_03_difficult的轨迹构建以及与真实轨迹的对比。

图 8 EuRoC数据集点线特征提取结果Fig. 8 Point and line feature extracting results of EuRoC

图 9 EuRoC V1_01_easy与V1_03_difficult的轨迹重构图Fig. 9 Trajectory reconstruction of EuRoC V1_01_easy and V1_03_difficult

6 结 论

本文提出一种可在多种场景下稳定工作、基于点线特征融合的立体视觉里程计方法。在特征提取与匹配阶段，采用ORB算子提取点特征，采用LSD算子提取线特征。对ORB特征匹配采用环形匹配和Bucketing技术；对LSD线特征施加外观几何约束及环形匹配策略提高匹配的精度；匹配的时候施加恒速约束缩小搜索范围，提高匹配速度。在位姿估计阶段，提出了一种新颖的融合点线特征的点线重投影误差模型，使用Huber核函数减小误匹配特征对优化过程的影响，由此模型结合球形线性插值得到相机位姿参数。采用公共数据集KITTI及EuRoC对算法进行实验验证，并与代表性的点特征VO以及点线特征VO进行比较。实验结果证明我们的算法性能表现良好，可在多种场景下应用。