使用点线特征的直接法单目视觉里程计ShichaoYang, Scbastain Scherer翻译：薛小吉

摘要：单目相机使用的大部分视觉里程计算法都把关注点放在特征匹配或者基于像素亮度的直接匹配得到的点，却忽略了一个普遍但又十分重要的几何特征：边缘，本文提出了一种通过组合点与边缘特征的里程计算法，该算法吸收了直接法与特征点匹配法的优点，在纹理较少的环境里有不错的效果，通过增大算法的收敛域，该算法对于光照变化和快速运动的场景有更好的鲁棒性，在跟踪部分，基于概率论，通过最小化光度残差与几何误差来匹配边缘，从而恢复相机姿态，在建图部分，边缘主要用于加速算法与提高空间匹配精度，在不同的公开数据集中，我们的算法比当前主流单目视觉里程计算法取得了相当甚至更好的效果，在某些富有挑战性的纹理较少的环境，我们的算法将状态估计误差降低了超过50%。

1介绍

随着在机器人导航、三维重建以及虚拟现实等方面的应用，视觉里程计（VO）和同步定位与建图（SLAM）近年来成为了热点话题，由于不同的传感器例如RGB-D相机、立体视觉相机和激光等能够为每帧数据提供深度信息，因此状态估计与建图变得相对简单，然而，在一部分平台上例如重量受限的微型无人机，单目相机由于其尺寸小、成本低的优势被更加广泛使用，因此，本文的工作主要集中在更富挑战性的单目视觉里程计。

       目前，视觉里程计和视觉SLAM主要有两种典型方法：（1）基于特征点的方法，例如PTAM和ORB SLAM，他们依赖特征点的提取与匹配来创建稀疏的三维地图，通过最小化重投影光度残差来进行姿态估计，这两种方法各有优势，关键点的重投影光度残差方法通常对于图像的噪声和畸变移动具有更好的鲁棒性，另一方面，直接法可以利用更多图像信息同时可以创建稠密或者半稠密的地图。

       在此文中，我们利用点和边缘特征来综合了上述两种方法的优点，边缘是除了特征点外另一种重要的特征，在双目和RGB-D视觉里程计中应用广泛，但是在单目视觉里程计中受到的关注较少，边缘的提取对自然场景的变化更不敏感，举个例子，在图1所示的近似平面的环境，由于像素变化梯度较小，使用点特征的直接法可能不够鲁棒，但是我们仍然提取出许多如图中所示的蓝色标记出来的特征用于状态估计与建图，在我们的系统中，和大部分直接法视觉里程计一样，我们对关键帧中梯度变化较大的像素仍然采用半深度图表示，我们同时在每帧中还提取并匹配边缘特征，在跟踪部分，我们同时优化光度残差和匹配到的边缘几何误差，在建图部分，通过正则化，边缘特征还可以用于引导并加速深度搜素，同时还能提高深度图的质量，因此，我们提出的视觉里程计的方法可以提高状态估计的精度以及创建质量较好的半深度图，我们通过各种实验证明了上述的效果。

图1，使用我们的基于边缘特征的视觉里程计在TUM单目数据集上的跟踪与三维重建效果，上部的图片为图像梯度较小的平面场景，这种场景如果只使用最小化光度残差效果会很不理想，然而，用蓝色标记出来的边缘特征可以用于提高跟踪与建图的效果

我们的主要工作总结如下：

1、一种融合了点线特征的实时单目视觉里程计算法，尤其是能适用于低纹理的场景。

2、在跟踪与建图部分提供了一种不确定度分析方法与点线特征概率融合方法。

3、改进了基于正则化的边缘分析方法。

4、在许多数据集上的效果优于现有的直接法视觉里程计。

在如下的部分，我们讨论一些相关工作，在第三部分，我们对目标问题进行公式推导，使用点线特征的跟踪与建图分别在第四、第五部分，这其中包含了对不同观察模型的不确定度分析，在第六部分，我们和现有主流算法进行了实验比对，第七部分是结论与展望。

2相关工作

       我们的算法通过组合特征点法与直接法来利用边缘信息，接下来将从三个方面简要介绍。

2.1特征点法视觉里程计

       基于特征点法的VO与SLAM有很多，例如LibVISO和ORB SLAM，这些方法首先提取图像特征并进行帧间的匹配与跟踪，通过解决PNP问题（根据3对匹配点，将世界坐标系下的点转化到相机坐标系）来最小化几何误差来估计相机位姿，这种方法对于图像噪声更加鲁棒并且有更大的可收敛区间，缺点是建出来的地图一般比较稀疏，另外有一种直接建图的算法依赖半深度图。

2.2直接法视觉里程计

       近年来，直接法也被广泛应用，它直接对像素灰度值进行几何优化，避免了任何特征提取的过程，因此这种方法可以在低纹理的场景下工作，并且应用在了不同的传感器上，例如RGB-D相机的DVO与单目相机的LSD SLAM，它的核心思想是对关键帧求取半深度图用于最小化光度残差，然而这是非凸函数，优化的时候依赖比较好的初始估计，目前有一种结合了直接法与特征点法的视觉里程计SVO可以用于高帧率的相机。

2.3基于边缘特征的视觉里程计

       在人造场景中，边缘是除了点之外的另一种重要的特征，边缘特征对光照变化更加鲁棒，同时相比于点能够保留更多的信息，基于线条特征的光束平差已经被应用于SLAM和SFM（structure from motion，从运动中恢复结构，侧重于建图），但这种方法特别耗计算资源，同时至少需要3帧才能进行有效优化，不使用光束平差的基于线条特征的方法最近被使用在双目相机、RGB-D相机和单目相机上，Kuse等对其邻近边缘像素进行最小化光度误差处理会因为边缘的误提取或者断裂的边缘导致误匹配，但是我们的线段匹配方法可以大大减少这种错误，一些方案仅仅对边缘的两个端点进行最小化几何误差，但是对于单目相机，由于其深度估计精度较差，因此会导致产生较大的误差。

3问题描述

3.1系统概述

       我们的算法是一种基于关键帧的单目视觉里程计框架，我们对关键帧中的大梯度像素生成半深度图，然后，每新来一帧，处理步骤分为三步：第一，对关键帧中的边缘特征进行线段匹配，第二，相机姿态跟踪，如果像素在边缘上，我们最小化像素光度残差和几何重投影误差，最后，我们通过不同的基线来更新深度图，边缘特征可以用于加速边缘像素的空间搜索并且可以通过有效的三维线条正则化来提高地图重建质量。

3.2符号记法

       灰度图为I : Ω  R2↦ R，Ω表示图像范围。

       对每个像素取逆深度的参考关键帧为D : Ω  R2↦ R+。

逆深度方差为V :Ω ↦ R+。

       相机投影函数为π : R3↦ R2，用于将相机坐标系中的3D点投影至像素平面。

       反投影函数为π-1 : (R2, R) ↦ R3，给定深度将像素点反投影至3D空间。

       当前帧与参考帧的刚体变换为T ∈ SE(3)，特殊欧氏群。

       变换矩阵T对应的李代数为ξ ∈ se(3)，ξ = (t;w)T∈R6，t∈R3为平移分量，w∈R3为旋转分量，w可以通过指数变化扩展为旋转矩阵，属于特殊正交群。

       帧间变换函数为τ : Ω1 × R × R6↦ Ω2，已知第一幅图像中的点及其深度，还有变换矩阵，先将第一幅图像中的点投影至三维空间，再根据变换矩阵将其投影至第二幅图像的像素平面。

       在三维空间中，边缘记为L，在二维图像平面中，边缘记为l，图像中的边缘像素记为M : R2↦ l，标记了该像素在边缘中的位置，大部分边缘像素M属于梯度变化较大的像素集合Ω。

4跟踪

4.1概述

       在跟踪线程中，假定深度图Dref和参考帧Iref固定，当前帧的计算通过最小化光度残差r(ξ)和线条重投影几何误差g(ξ)，这些是基于两个观察模型：光度观察与边缘位置观察，可以表示为一个非线性最小二乘问题，公式如下：

(1)

       这里光度差ri定义如下： (2)       gj是线段li上的像素xi的重投影误差，线段li在一个平面上： (3)       这里()是平面分布特征的坐标转换过程，这仅适用于帧间边缘匹配的情况，Σr和Σg分别表示两种误差的不确定度。       目标函数通过Gauss-Newton方法来进行优化，n次迭代量如下式： (4)       这里误差累计向量E由两部分组成：光度误差与几何误差，J是E的一阶导，权重W通过不确定度Σ-1计算得到，跟踪效果如图2所示。

图二，在TUM frg3/cabinet_big数据集上两张图片的跟踪迭代过程，(a)为提取边缘特征的参考帧，(b)为提取边缘特征的当前帧，两幅图像大概间隔41帧（约1.4s），(c)为反向投影的像素点，随着迭代，绿色的边缘在向着正确的红色边缘靠近。

4.2跟踪不确定度分析

       在上式(1)中考虑了不同类型的误差将提高姿态估计的鲁棒性与精准度，每项误差的权重正比于由观察模型计算出的方差的逆，这里我们对几何误差方差Σg进行分析。

       在一般情况下，由输入不确定度引起的输出不确定度可以表示为： (5)       这里Jf是x的一阶导数。       在我们的算法中，如(3)式所定义，像素到线的重投影误差是关于直线lj和重投影点的函数，直线lj由其两端点的外积得到（lj=p1×p2），我们可以假设端点的不确定度满足方差为1的二维高斯分布，因此我们可以使用式(5)来计算直线lj的不确定度，这说明了较长的线具有更小的线匹配不确定度。       同样的，我们可以计算重投影点的方差，它是一个关于原像素点深度与方差的函数，最终的重投影误差协方差由两种不确定度来源构成： (6)  

5建图

5.1概述

       在建图线程中，通过在逆深度滤波框架中进行空间三角解算来更新参考帧的深度图，直线正则化可以用于提高准确度，在这一步，假设相机姿态保持不变，深度估计的损失函数定义如下： (7)       这里ri(d)是空间匹配光度误差，通过使用图像块的误差平方和来提高鲁棒性，对于直线lj，我们希望像素点重投影值空间后仍然能构成一条直线，所以Gj是边缘正则化损失，它表示边缘像素三维点到三维线的距离，正则化技术同样在其他稠密建图算法被应用，如果仅仅使用第一个ri，所有的像素相互间都独立，那么可以在极线上依次独立搜寻匹配的像素，正则项Gj是融合了一条边缘上的各个像素点的深度，因此，将问题转化为了基于二元法则的典型迭代替换优化方法，但是，这需要耗费大量的计算资源，取而代之，我们在两个阶段优化ri和Gj将更有效率。

5.2空间线段匹配

       对于不在边缘上的像素点或者是未匹配的边缘上的像素点，我们通过最小化误差平方和来穷举空间匹配点，搜索的深度区间限制为d+2σd，d为深度均值，σd为深度标准误差。       对于在已经匹配的边缘上的像素点，我们可以直接计算匹配线与极线的交点作为其重投影点，同时我们可以如图3中直接进行三角 测量来一次性计算所有的像素深度，如果当前帧的变化为R、t，那么三维线段可以表示为两个反向投影平面的交线：  (8)        这里l1和l2分别为参考帧与当前帧中的直线方程，K为相机的内参，因此对于每个像素，我们可以通过计算反向投影线段的交点来计算其深度。       对于极线与边缘平行的较差情况，我们不能通过平面相交的方法来求三维线段，取而代之的，我们通过最小化误差平方和在极线上搜索匹配的像素。

图3，线段三角测算，三维线段L为两个反向投影面的交线，对于l上的每个像素，它的空间匹配点是极线g与匹配线l’的交点，同样三角点也在三维线段L上。

5.3线段匹配不确定度分析

       这里我们分析边缘的空间匹配误差，对于参考帧中的每个边缘像素，匹配的像素为极线与匹配边缘的交点，这两条线都存在位置误差，最终会产生如图4的不一致误差，当极线与匹配的变化接近平行时，误差会变得非常大： (9)       这里θ是线l与g的夹角。       根据等式(5)的误差传递规则，我们可以计算不一致误差的方差如下： (10)       近似认为逆深度d正比于不一致误差λ，我们可以通过公式(5)来计算d的观测方差，这可以用于标准扩展卡尔曼滤波中像素深度方差的更新。

图4，线匹配存在的不一致误差，l是匹配像素点所在的边缘线，g是极线，因为位置误差的存在，最终导致的不一致误差如图所示。

5.4三维线段正则化

       深度图正则化是单目相机地图重建中用于提高深度估计精度的重要手段，在上一小节利用扩展卡尔曼滤波后更新深度图后，像素平面的边缘可能与三维空间中的不一致，因此，我们需要匹配三维空间的线段并且更新像素的深度，三维加权线拟合问题最近在RGB-D线基里程计中得到了解决，它使用L-M迭代优化算法来寻找最佳三维线段，这里我们提出一种快速的解析法来求解三维加权线拟合问题。       从像素平面的边缘投影至三维空间点后，根据投影几何这些点应该在同一个平面G上，我们可以重新创建一个相机坐标系，x、y轴在平面G上，在新坐标系内的点记为p’，我们使用随机采样一致性来挑选一系列二维内点，距离判断采用马氏距离，相比于使用欧式距离，马氏距离考虑了不确定度： (11)       这里q’∈l’表明该像素点在线l’上，可以通过对q’求导并使导数为零来求解马氏距离。

图5，三维线条正则化，我们首先将点投影至三维空间中，如图中红色区域所示，粉色的椭圆为三维点的不确定区域，将三维点转换至灰色坐标系平面，X、Y轴用蓝色标记，然后我们使用RANSAC来计算加权最小二乘的解析解，而不是采用迭代优化方法。

     在RANSAC之后，我们可以找到一致性最好的点集，这就转化成了二维加权线拟合问题，我们想要求最优线段L*，通过下式： (12)    这里的δ(p’i)是沿y方向的点p’i到线L的距离，它是点到直线的近似距离但是可以构成一个闭式求解问题，将所有去均值的点坐标和原始像素协方差累加，于是得线的模型为Y = Xβ + ε，这里β是系数，ε为正态分布的噪声，在高斯噪声下最大似然估计优化方法为： (13)    我们可以将新建坐标系中的最优线段L*转换至原始坐标系中，这样就可以求解线段上像素的深度了。

6实验结果

6.1成果

       1、边缘提取与匹配：我们使用公开的线段提取算法来提高跟踪精度，我们采用了一种尺度变化的方法，建立两个金字塔层，由于线特征提取算法的不确定性，一条线有时会分裂成多条线段，所以我们明确的将两条距离和角度在阈值范围内的线段视为一条，在此之后，去掉那些非常短的线段来减小线拟合的误差，为了加快线融合的速度，每条线段的中点都标注了它的方向，从而我们可以通过中点与方向来进行线段的融合，之后对每条线计算LBD描述子，并对它们进行帧间匹配，分块方法可以用于加速匹配，最后，对直线的追踪能够找到一条边缘上所有的像素，我们发现如果将直线扩展一个像素能够使系统变得更加鲁棒和准确因为在跟踪和建图的线约束中引入了更多的像素。

       2、基于关键帧的VO：我们的方法没有采用SLAM和SFM中对点和线的光束平差法，但是可以引入来优化效果，在关键帧和当前帧中可以进行相机位姿跟踪、线匹配和空间建图。

6.2实验

       在这个部分，我们在许多的公开数据集（包括TUM RGB-D、TUM mono、ICL-NUIM）上测试了我们的算法，主要和目前主流的单目直接法SDVO和基于特征点的ORB SLAM做了比较，同时还和一些其他的在公开数据集上有测试结果的基于边缘的视觉里程计做了比较，对于ORB SLAM，我们关闭了回环检测线程，但仍然保持了全局和局部光束平差来提取累加的回环检测结果，而我们的算法和SDVO是不需要光束平差的，我们使用相对位置误差评判准则： (14)       这里Qi是位姿的真实值序列，Bi是估计的位姿，尺度与轨迹基本匹配。

图6，TUM数据集中纹理较多的示例图片，(a)fr2/desk，(b)fr3/cabinet，(c)fr3/notex-far。因为(b)、(c)中特征点较少，ORB SLAM表现更差，我们的算法仍然可以利用匹配的边缘特征来优化状态估计。

 1）定性结果：我们选择TUM mono/38用于视觉里程计和可视化建图测试，效果见图1，这个场景主要为很大边缘特征的白色平面，我们的算法可以得到较好的位姿估计并建出质量较高的地图，更多结果见附件视频。2）定量结果：我们首先在两个比较主流的数据集（TUM RGB-D：fr2/desk、fr2/xyz）上做测试，对比结果见表1，SDVO和另外两个基于边缘特征的VO的测试结果取自他们的文章，这两个场景都含有丰富的特征，特别适合基于特征点法的ORB SLAM，因为存在大量的梯度变化较大的像素，SDVO的效果也不错，这些环境里还有许多瓶子的轮廓线、叶子、小键盘，因此存在较大的线特征提取与匹配误差，我们的算法和SDVO效果比较接近，并且优于另外两个基于边缘特征的视觉里程计。 

如表2，我们同样提供了其他数据集上的测试结果，然而其他基于边缘的视觉里程计并没有测试结果，顶部的两个场景是相对简单的，在TUM mono/38中，我们仅仅对有真实值的起始部分做了测试，由于在门框和橱窗区域仍有许多角点，ORB SLAM变现最好，我们的算法优于SDVO，这是因为场景是接近平面的并且没有较大的像素梯度，所以使用最小化光度误差的SDVO效果较差，然而我们的算法可以通过对边缘特征最小化重投影误差来保证良好的效果。
表一 TUM数据集上的相对位置误差（cm/s）比较

在表二中的最后四个场景特征点较少，因此ORB SLA表现较差，但是直接法通过利用像素梯度值和边缘信息仍能正常工作，注意在ICL/officel数据集中，虽然有很多的特征点，但是当相机视野中只有白色的墙面和地面而没有区分度较高的特征时，ORB SLAM发散了，我们的算法在大多数情况下优于SDVO的原因主要有以下两点：第一，通过增加边缘特征，我们能利用更多的像素用于跟踪，一些像素点可能由于同质表面梯度变化较小，但是由于边缘特征的存在仍然能够被利用起来，第二，我们同时最小化光度误差与几何误差，可以使算法更加鲁棒，并且有更大的可收敛区域，这可以在许多其他的工作中被佐证。

为了证明优化算法存在较大的收敛域，我们从TUM fr3/cabinet_big数据集中挑选了间隔41帧的两帧图像，并在图2中展示了迭代的过程，我们能够清楚的看到绿色的重投影像素点渐渐的向红色的真实值靠拢。

6.3耗时分析

       我们在表三上注明了我们的算法在TUM fr3/cabinet_big数据集的时间用量，使用两个八叉树线提取方法，在每帧中平均有193条边缘特征，除了建图外整个跟踪线程耗时51.95ms，可以以20Hz的速度运行，时间会因为参与优化的像素点的数量而有所差异，目前来看，边缘提取和描述子的计算耗费了大部分时间，这可以通过将采样来加速，Edline边缘提取子可以加速近一半的时间，当其因为提取的边缘特征较少使得在一些有挑战的环境中状态估计的精度降低了，最近，Gomez等利用边缘跟踪取代每帧的提取与匹配来减少计算量，这是一个很好的方法。

7结论

       在本文中，我们提出了一种利用点线特征的直接法视觉里程计，我们沿着SDVO的思路继续往下做，通过利用边缘特征来提高跟踪与建图的效果，在跟踪部分，我们通过最小化光度误差与几何误差来匹配边缘特征，在建图部分，我们不需要穷举便能利用已经匹配好的边缘快速精准的进行空间匹配，我们提出了一种使用边缘特征正则化深度图的解析法，同时在跟踪与建图部分还给出了不同观察模型的不确定度分析过程。

       我们的算法综合了直接法与特征点法的优势，通过最小化边缘和几何误差，可以创建半深度图并且使状态估计更加的鲁棒和精准，在多种数据集上，我们的算法性能可以与SDVO和ORB SLAM相当甚至优于它们，使用光束平差的ORB SLAM在特征点丰富的场景下效果很棒，但在纹理较少的场景可能会无法工作，而直接法通常效果更好，我们的算法针对上述场景开发并且通过利用边缘特征取得了比SDVO更好的效果。

       在此之后，我们想通过边缘的直接对准来减少边缘提取和匹配的计算量，同时，在多种中使用光束平差可以提高准确率，我们将在单帧中通过组合点、线、面特征来利用更多的信息，从而提高算法在富有挑战的场景中的鲁棒性与精准率。