一种基于线特征的RGB-D 视觉里程计算法

黄 平，曹 镇，黄俊杰

（哈尔滨工程大学智能科学与工程学院，哈尔滨 150001）

同步定位与地图构建（Simultaneous Localization and Mapping, SLAM）技术的目的是使机器人在未知的环境中，并且不明确自身位置的情况下可以同时进行自身定位与构建周围环境的地图，SLAM 是智能机器人实现“智能化”的关键技术[1]。SLAM 作为智能机器人领域的底层技术，伴随着计算机技术[2]和人工智能技术[3]的发展，也不断拓宽智能机器人的应用领域[4]。而视觉SLAM 的一个核心部分是使用相机作为传感器的视觉里程计。

早期的视觉里程计使用单目和双目相机，但此类相机并不能直接获取深度信息，需要根据图像中的特征及其匹配关系，利用计算机视觉中的多视图几何关系构建出场景的深度值，并且单目相机具有尺度不确定性，在使用前需要进行初始化确定尺度[5]。而RGB-D相机利用TOF（Time of Flight）原理直接获取场景中的深度信息，在视觉SLAM 中取得了广泛应用。

视觉里程计的实现往往需要从图像中提取特征，进而将特征匹配得到对应关系，而后才可以由此对应关系根据一系列解算方法得到相机的位姿等信息。因此，使用相机时的所处环境对算法的性能有着极大的影响。如Rual 等人于2015 年提出的ORB-SLAM[6]是现代SLAM 系统中非常常用的一种，整个系统围绕ORB 特征点[7]进行计算，此系统在纹理明显、点特征丰富的环境中有着极其优秀的表现。但若在点特征极少的环境中，该算法很可能无法提取出足够的特征点，进而无法解算相机位姿使系统停止工作。如图1 这样点特征较少的环境我们称之为欠点特征环境，而现实生活中，这样的场景并不少见，完善视觉里程计在此类环境下的应对方法，可以扩展视觉SLAM 系统的使用场景，提高系统的鲁棒性，因此也有着较高的实用价值。

图1 点特征较少但线特征丰富的场景Fig.1 Scenes with few point features but rich line features

线特征对欠特征的环境下的视觉里程计运行的稳定性有明显的帮助。引入线特征的SLAM 系统通常使用LSD（Line Segment Detector）提取线特征，LBD（Line Band Descriptor）描述线特征，便于帧之间的线特征匹配，再将线反投影至三维空间中用以位姿估计。

本文针对深度相机存在的深度缺失问题，增加深度图推断的图像预处理步骤，推断线特征对应的深度图像位置的深度信息，避免深度缺失；此外在位姿优化部分进行改进，利用拟合直线过程中的最佳过点，以及重投影的线段与观测线段的角度误差信息，推导误差关于位姿扰动的雅克比矩阵，在图优化时利用重投影误差优化相机位姿，拓展传统的优化方法。

1 算法框架

本节介绍基于图像线特征的视觉里程计算法框架。图2 为视觉里程计整体算法框架图。首先利用已标定好的深度相机获取彩色图像及深度图像。使用LSD 线段检测算法[8]提取彩色图像中的2D 线特征，而后确定包含线特征的各矩形区域，接着将各矩形区域映射到深度图中，针对深度缺失问题，对各矩形区域使用像素滤波方法进行深度推断。之后均匀采样2D线特征中的点，结合深度推断后的深度图反投影为3D点，利用RANSAC 方法[9]找到过直线的最佳两点拟合3D 直线，根据3D 直线的匹配关系估计相机位姿。最后，构建包含距离误差及角度误差的雅克比矩阵，利用高斯-牛顿法优化位姿。

图2 算法框架图Fig.2 Algorithm frame diagram

2 线特征的检测与匹配

本文使用LSD 线段检测算法提取图像中的线特征，线特征为灰度值变化明显的连续区域，该算法的优势是可以在线性时间内得到亚像素级的线段信息。它通过计算像素梯度以及梯度法相，找到图像亮度变化较大的一些点，当这些点位置相邻且梯度方向相近时，则构成了直线段。如图3 为实际场景中采取图片提取出的线特征信息。

图3 LSD 算法提取线特征Fig.3 LSD Algorithm to Extract Line Features

为了能将线特征进行匹配，本文使用二进制描述子LBD[10]进行线特征的描述。通过计算描述子间的汉明距离采用穷举匹配法即可找到匹配关系。

如图4 即为两幅图像间线特征的匹配结果，而在本文视觉里程计实现中，我们将当前帧中二维线特征反投影成的三维直线，同样利用LBD 描述子，与局部地图中的三维直线相匹配。

图4 特征匹配示例Fig.4 Examples of feature matching

3 三维直线的拟合估计

3.1 深度图推断

本文利用线特征反投影拟合出的三维直线的匹配关系估计相机的位姿，因此，场景中采集到的二维线段变换到三维直线的估计过程非常重要，极大地影响着位姿估计的准确性。拟合过程中需要将二维信息根据RGB-D 相机采集到的深度值反投影为三维信息，而一般的RGB-D 相机会有深度缺失问题，表现为深度图中灰度值为0 的纯黑色区域，此缺陷会导致二维特征无法反投影为三维特征，因此，拟合三维直线前，我们首先进行深度图推断。

深度图推断的本质是图像的滤波，图像滤波的方式有很多：如双边滤波、均值滤波等，但双边滤波对深度的推断效果不明显，而均值滤波又无法很好地保证原图像的边缘特性，因此本文采用像素滤波的方式首先推断出深度图中缺失的像素灰度值，而后采用滑动窗方式的中值滤波降低噪声的同时保留好边缘信息。首先是像素滤波的处理，像素滤波的核心思想是使用待推断像素点周围的像素灰度值估计出待推断像素点灰度值。如图5 所示，像素滤波器往往使用两个环带，本文中使用的内环带为8 个像素点，外环带为16 个像素点。

图5 像素滤波器Fig.5 Pixel filter

将矩形待推断区域内灰度值为0 的像素定义为候选像素，以候选像素为中心使用如图5 所示的像素滤波器，该算法可表示为：

式(1)中，Dcenter为中心候选像素点推断后的灰度值，Dbefore为该点推断前已有灰度值， c1、c2分别为像素滤波器内层和外层中灰度值非零的个数，1T 、2T分别为内层和外层中灰度值非零的个数的阈值，M 为内外两层中非零灰度值的众数。因此，只有当内外两层非零灰度像素个数都超过设定阈值且候选像素原灰度值为零时才进行深度推断，并将候选像素的灰度值设定为内外两层灰度中的众数。

以上便完成了深度推断的部分。在深度推断后往往还需要进行进一步的滤波处理来降低图像中的噪声，而不能破坏原有特征，需要保留好边缘的特征，最后采用中值滤波的方式对深度图进一步处理保留边缘信息。

图6 为经过像素滤波与中值滤波后的深度图变化情况，为便于比较，将灰度值为0 的像素用红色表示，可以看出深度空洞的区域明显减少。

图6 深度推断前后对比图Fig.6 Contrast before and after depth inference

3.2 二维线段的采样与反投影

在位姿估计的过程中需要使用三维直线信息，因此二维线段的反投影过程及其重要，接下来重点阐述将二维线段反投影为三维直线的过程。

为方便讨论，我们以一条直线的三维估计为例进行讨论。彩色图像记为G，G 中的任意一2D 点坐标记为 gj∈G，gj=[ujvj]T。根据针孔相机模型，得到gj在相机坐标系下的坐标为：

式(2)即为坐标反投影公式，其中，cu,cv为相机光心坐标， fu,fv为焦距，s为尺度因子， dj为 gj所对应的深度值。

一种直观的获取三维直线的方法是使用二维线段的端点，将二维线段的两个端点反投影成为3D 端点，过两个点自然可以得到三维直线。但是这种方法有着很大的缺陷，通过式(2)可知，深度值的准确性决定着三维点估计的准确性，因此，若2D 线段端点的深度值不准确，那么估计出的3D 直线将具有很大偏差，在后续的位姿估计计算中也将带有很大的误差。因此我们不再仅仅只用二维线段的两个端点，而采用在二维线段上均匀采样多点的方式进行三维直线的估计。如图7 所示，为了减少计算量，我们仅对那些能匹配成功的二维线段进行2D 点采样，对于一条二维线段l ，采样后得到点集 P2D={ p1, p2... pn}∈l ，对于P2D中深度值为0 的2D 点直接剔除，其中n 是2D 点的个数，对于 P2D中的所有2D 点利用式(2)即可得到l对应的3D 点集合，记为 P3D={ P1, P2... Pn}。

图7 二维线段的采样Fig.7 Sampling of 2-D Line Segments

3.3 三维直线的RANSAC 估计

空间直线的表示方法有很多，如图8 所示，这里我们通过两个向量( u,d )表示[11]。

图8 三维直线表示Fig.8 3D straight line representation

记三维直线为L，u是L 的单位方向向量，d 的模为相机原点到空间直线的距离，d 的方向平行于由相机原点与空间直线所构成的平面的法向量n。假设m 是直线上的任意一点，则d 与u的关系如下：

其中^运算为叉乘。

综上，为了确定一条三维直线，我们需要找出的就是 P3D={ P1,P2,...

Pn}中最佳过直线两点，该过程如图9 所示，本文使用如表1 所示的RANSAC 方法拟合出三维直线。

图9 三维直线拟合过程Fig.9 3D straight line fitting process

表1 RANSAC 选择最佳过直线两点算法Tab.1 RANSAC method selects the best two points to cross the straight line

已知最佳过直线的两点自然可以按上述直线表示法得到三维直线，综上，完成了2D 线段反投影为3D 直线。

4 位姿估计

针对当前帧和参考帧（也可以是局部地图）中匹配成功的三维直线，位姿变换前( ur, dr)与变换后( uc, dc)的约束关系为：

其中R 是3 × 3旋转矩阵，t 为平移向量。首先计算旋转，由 uc=Rur计算如下目标函数：

为便于计算，将旋转矩阵R 转化为单位四元数q表示。则式(5)可以转化为：

其中， u^为u 的反对称矩阵，结合式(7)进一步得到：

接下来，计算平移，由 dc=Rdc+ uc^t 计算如下目标函数：

式(10)对t 求偏导，并令偏导为0，得到：

若式(12)满秩，则平移向量为：

最后，利用RANSAC 选取最合适的位姿。由三组匹配直线计算出初始位姿，而后计算出其他匹配线在该位姿下的投影误差，设定阈值，统计内外点个数；循环以上过程，最终选取内点数最多的一组为此两相邻帧间的位姿变换结果。

5 位姿优化

在完成初步的位姿估计后，需要对估计出的位姿进一步优化，建立观测特征与位姿的约束关系，利用图优化方法提高位姿估计的精度，本节主要论述利用线特征的图优化位姿方法，以及加入角度信息后进一步提高精度的优化方法。

5.1 利用线特征的位姿优化

本节利用匹配好的线特征信息进行位姿优化，类似于使用点特征的优化方法，使用重投影误差作为误差量，将局部地图中世界坐标系下的三维直线重投影到当前帧，得到重投影二维线段，减小该线段与当前帧观测二维线段的误差，从而达到优化位姿的目的，具体实施方式如图10 所示。

我们在3.3 节利用RANSAC方法选择出了最佳的过直线两点，过此两点构成了一条三维直线，即如图10 所示中的三维直线PQ，该直线在局部地图中的世界坐标系中。

图10 线段的重投影误差Fig.10 Re-projection Error of Line Segment

首先，将世界坐标系中的直线投影到相机坐标系中，将最佳过直线的两个3D 点投影至相机系便可以得到相机系下的直线，因此该问题转化为了3D 点的重投影过程，以P 点为例，设该空间点坐标为P=[ X , Y , Z]T， 它对应的投影相机系坐标为P'=[ X ', Y ', Z ']T，像素系坐标为p'=[ x,y]T，首先将世界系中的坐标转换为相机系下坐标，并取其前三维：

其中ξ 是李代数位姿，^是反对称矩阵运算。接下来，转换到像素坐标系：

其中，s 是缩放系数，K 是相机内参矩阵，对式(15)展开有：

其中， cx,cy为相机光心坐标， fx,fy为焦距，消去s便可得到像素系下的坐标：

我们所建立的误差量是投影到像素系的点p' 和q'到当前帧观测线段lpixel_2D的距离。当位姿估计完全准确时，理论上由点p' 和q'构成的重投影线段 l3D→2D应与观测线段lpixel_2D完全重合，那么点p' 和q' 到lpixel_2D的距离应为0，因此我们选择此距离作为误差量，通过迭代优化不断减小此误差从而达到优化位姿的目的。为方便叙述，以其中一个点p' 到lpixel_2D的距离为例，点q'到lpixel_2D的距离同理。

设已知观测线lpixel_2D上的两个端点p=[ x1,y1]T，q=[ x2, y2]T。则p' 到lpixel_2D的距离：

式(18)中由于分母是常数，则误差：

误差 e1求取对位姿扰动的雅克比矩阵，这里相机位姿使用李代数ξ 表示，根据链式求导法则，有：

对于等式第一项，有：

对于第二项，有：

则得到1× 6的雅克比矩阵：

以上完成了利用线特征信息的重投影误差对位姿雅克比矩阵的推导，在实践中，调用G2O 图优化库[12]，以相机位姿为图优化顶点，以重投影误差为边构建图优化模型，利用该雅克比矩阵便可结合高斯牛顿或列文伯格-马夸尔特方法进行迭代优化。

5.2 加入角度信息的位姿优化

为进一步提高优化效果，可以在重投影的线段与观测线段中构建一个角度误差信息，如图10 所示，定义误差：

则此误差对位姿扰动的雅克比矩阵为：

由于 eang的定义是cos＜qp ,qp' ＞，当测量线段与观测线段接近重合时，＜qp,qp'＞→0o， 有cos＜qp ,qp'＞→1， 因此我们可以设观测量measurement 恒为1，则：

误差越接近0 越好，同上一小节，通过高斯牛顿或列文伯格-马夸尔特方法迭代使误差下降，从而得到更优的位姿结果。

6 实验结果及分析

本文采用一台笔记本PC 机运行和调试算法，配置为Intel 酷睿i5 四核2.80GHz，内存为8G，操作系统为Ubuntu16.04LTS 版本。本文使用TUM 公开的RGB-D 数据集，对所述视觉里程计算法采用如图11所示的RGB-D 数据集进行实验，具体序列名称为freiburg3_large_cabinet，序列内容为相机围绕一个大办公柜转了一圈，橱柜表面可选取的特征点很少，橱柜结构简单，大多是平面和直角。评测使用evo 评测工具对运行结果评测。

图11 数据集示例Fig.11 Data set example

当相机运动过快或场景中点特征稀少时，会丢失很多点特征的匹配对，导致以点特征为核心的视觉里程计无法再继续跟踪[13]，但使用线特征可以对此环境有一定抵抗性，本节对比仅使用点特征的里程计和使用本文所述线特征里程计运行的结果。

如图12 是仅使用ORB 点特征的里程计运行数据结果，其中图12(a)为相机运行的俯视图轨迹坐标，图12(b)为世界坐标系下xyz 三个方向的坐标，虚线部分皆为实际轨迹坐标投影，实线部分为计算出的相机运动坐标。该数据集的点特征较少，因此仅使用点特征的里程计没有很好地持续跟踪，由图12 可见，在17 s左右时无法继续解算出位姿结果，表示实际解算结果的蓝色线中断。在可跟踪部分，计算出的RMSE 为0.620 m。

图12 仅使用ORB 点特征VO 运行轨迹Fig.12 VO trajectory using ORB point features only

接下来使用本文所述线特征里程计运行该数据集，结果如图13 所示。

从图13 我们看到里程计并没有完全进行跟踪，在18 s 左右表示实际解算结果的蓝色线中断，由于未采用任何优化措施，导致后续连续几个帧的位姿结果较差，因不会向局部地图中添加任何线特征，导致后续帧没有能够匹配的特征，最终无法解算而导致跟踪失败，在可跟踪部分的RMSE 为0.050 m。最后使用本文线特征里程计，且加入了误差优化方法后的结果如图14 所示，此时里程计可以做到完整跟踪。

图13 本文的VO 运行轨迹（未加入优化）Fig.13 VO trajectory in article（Optimization not added）

图14 本文的VO 运行轨迹（加入优化）Fig.14 VO trajectory in article（Optimization added）

由于本文使用线特征，线特征固有的结构性信息可以带来一定的精度提升，且本文在优化过程中使用拟合直线过程中最佳过点到观测直线的距离，以及引入一个角度误差，进一步优化了位姿结果。跟踪完成后计算出的RMSE 为0.058 m。

ORB-SLAM2 作为最优秀的以点特征为核心的SLAM 系统之一，本文也对其在该序列上的表现进行了测试。如图15 所示，默认参数下，ORB-SLAM2未能完整跟踪数据，在成功跟踪部分 RMSE 为0.050 m。本质上算法在两帧之间跟踪成功需要足够的特征点，增加图像中提取的特征点数量就可以提高跟踪的成功率。在ORB-SLAM2 代码配置参数文件中，OR Bextractor.iniThFAST 对提取的特征点数量有明显的影响，该参数是提取FAST 角点时的阈值，当角点值大于阈值时才会被成功提取，降低该值意味着提取的角点质量下降，数量增加。

图15 默认参数ORB-SLAM2 运行轨迹Fig.15 Default parameter ORB-SLAM2 trajectory

将该参数值从默认的20 降为15 时，ORB-SLAM2能够成功完整跟踪数据，如图16 所示。跟踪完成后计算出的RMSE 为0.159 m。

图16 降角点阈值ORB-SLAM2 运行轨迹Fig.16 Reduced corner points threshold ORB-SLAM2 trajectory

为了方便比较算法效果，将上述实验结果整合列表展示，如表2 所示，此序列在各视觉里程计的运行情况如下。可以看到加入优化的本文算法相比降角点阈值ORB-SLAM2 的轨迹估计精度提高63%，并能完整地跟踪轨迹。

表2 数据在各视觉里程计算法中的运行情况Tab.2 Operation of data in each visual odometry

图17 是两种里程计运行时的世界坐标系下xyz坐标的对比，蓝色线为本文视觉里程计方法计算出的轨迹坐标，绿色线为仅使用ORB 特征点的视觉里程计方法计算出的轨迹坐标，使用本文的视觉里程计方法，会使跟踪结果更加贴近真实轨迹，且跟踪更加长远。

图17 两种视觉里程计的位置估计对比Fig.17 Comparison of Position Estimation of Two VO

在当前实验环境下，本文视觉里程计各部分运行时间如表3 所示。可以看到算法运行的时间主要消耗在特征提取及描述和地图维护部分，平均一帧运行时间约214 ms，即为5 Hz 的运行频率，满足一般使用环境中的实时性要求。

表3 线特征VO 部分运行时间统计Tab.3 Statistics of Partial Run Time of Line Feature VO

在目前所流行的SLAM 系统中，ORB-SLAM2 是最优秀的以点特征为核心的SLAM 系统之一，无论在精度还是在实时性方面都有着极高的表现，但由于采用ORB 点特征，在欠点特征下不如线特征里程计鲁棒。本文仅使用线特征，在优化方面采用不同的方法，使用拟合直线过程中的过点，引入一个角度误差信息，拓展了线特征里程计优化方法。本节实验中，本文提出的一种基于线特征的RGB-D 视觉里程计算法表现优于ORB-SLAM2，并且满足一般环境的实时性需求。

7 结 论

本文构建了以线特征为核心的线特征视觉里程计框架。针对深度相机存在的深度缺失问题，增加深度图推断的图像预处理步骤，使三维直线的估计更加准确。使用LSD 进行线特征提取并且使用LBD 描述子进行描述以便进行特征匹配。利用RANSAC 方法，推导了二维线段反投影拟合为三维直线的过程。本文在位姿优化部分进行改进，充分使用拟合三维直线过程中的最佳过直线两点而非传统的线段端点增加可靠性，并且加入了一个角度误差信息，推导了误差关于位姿扰动的雅克比矩阵，实现图优化过程，该优化方法使得视觉里程计位姿估计结果更加准确。

使用线特征的视觉里程计相比于点特征里程计的计算量更大，特别是利用RANSAC 进行三维线段拟合时，算法复杂度较高，当线特征数量增加时，计算量增长较大，实时性并不如点特征，需要控制线特征数量保持在合理的范围，在未来有待进一步优化。