视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点法和特征提取和匹配实践

 专栏系列文章如下：

 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第二讲-开发环境搭建_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转矩阵和Eigen库_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-旋转向量、欧拉角、四元数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第三讲-相似、仿射、射影变换和eigen程序、可视化演示_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李群与李代数基础和定义、指数和对数映射_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-李代数求导与扰动模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第四讲-Sophus实践、相似变换群与李代数_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-相机模型_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第五讲-图像和实践_goldqiu的博客-CSDN博客_global shutter

视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题_goldqiu的博客-CSDN博客

​​​​​​视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_goldqiu的博客-CSDN博客

视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合_goldqiu的博客-CSDN博客

第七章 视觉里程计 1

7.1特征点法

一个 SLAM 系统分为前端和后端,其中是视觉SLAM前端也称为视觉里程计(VO)。VO根据相邻图像的信息估计出粗略的相机运动,给后端提供较好的初始值。VO的算法主要分为两个大类:特征点法和直接法。基于特征点法的前端,被认为是视觉里程计的主流方法。它具有稳定,对光照、动态物体不敏感的优势,是目前比较成熟的解决方案。如何提取、匹配图像特征点,然后估计两帧之间的相机运动和场景结构,从而实现一个两帧间视觉里程计。这类算法也称为两视图几何(Two-view geometry)。

7.1.1 特征点

VO的核心问题是如何根据图像来估计相机运动。然而,图像本身是一个由亮度和色彩组成的矩阵,如果直接从矩阵层面考虑运动估计,将会非常困难。所以,比较方便的做法是:首先,从图像中选取比较有代表性的点。这些点在相机视角发生少量变化后会保持不变,于是能在各个图像中找到相同的点。然后,在这些点的基础上,讨论相机位姿估计问题,以及这些点的定位问题。在经典SLAM模型中称这些点为路标(Landmark)。而在视觉 SLAM 中,路标则是指图像特征(Feature)。其定义为：一组与计算任务相关的信息,计算任务取决于具体的应用。简而言之,特征是图像信息的另一种数字表达形式。一组好的特征对于在指定任务上的最终表现至关重要。数字图像在计算机中以灰度值矩阵的方式存储,所以最简单的单个图像像素也是一种“特征”。但是,在视觉里程计中希望特征点在相机运动之后保持稳定,而灰度值受光照、形变、物体材质的影响严重,在不同图像间变化非常大,不够稳定。理想的情况是,当场景和相机视角发生少量改变时,算法还能从图像中判断哪些地方是同一个点。所以,仅凭灰度值是不够的,需要对图像提取特征点。 特征点是图像里一些特别的地方，可以把图像中的角点、边缘和区块都当成图像中有代表性的地方。不过更容易精确地指出,某两幅图像中出现了同一个角点;同一 个边缘则稍微困难一些,因为沿着该边缘前进,图像局部是相似的;同一个区块则是最困难的。可以发现,图像中的角点、边缘相比于像素区块而言更加“特别”,在不同图像之间的辨识度更强。所以,一种直观的提取特征的方式就是在不同图像间辨认角点,确定它们的对应关系。在这种做法中,角点就是所谓的特征。角点的提取算法有很多,例如 Harris 角点、FAST 角点 、GFTT 角点等等。然而,在大多数应用中,单纯的角点依然不能满足很多需求。例如,从远处看上去是角点的地方,当相机走近之后,可能就不显示为角点了。或者,当旋转相机时,角点的外观会发生变化,也就不容易辨认出那是同一个角点。为此,就提出了许多更加稳定的局部图像特征,如著名的SIFT 、SURF 、ORB ,等等。相比于普通角点,这些人工设计的特征点能够拥有如下的性质:

1. 可重复性(Repeatability):相同的特征可以在不同的图像中找到。
2. 可区别性(Distinctiveness):不同的特征有不同的表达。
3. 高效率(Efficiency):同一图像中,特征点的数量应远小于像素的数量。
4. 本地性(Locality):特征仅与一小片图像区域相关。

特征点由关键点(Key-point)和描述子(Descriptor)两部分组成。关键点是指该特征点在图像里的位置,有些特征点还具有朝向、大小等信息。描述子通常是一个向量,按照某种人为设计的方式,描述了该关键点周围像素的信息。描述子是按照“外观相似的特征应该有相似的描述子”的原则设计的。因此,只要两个特征点的描述子在向量空间上的距离相近,就可以认为它们是同样的特征点。 SIFT(尺度不变特征变换,Scale-Invariant FeatureTransform)：充分考虑了在图像变换过程中出现的光照、尺度、旋转等变化,但随之而来的是极大的计算量。 FAST关键点则考虑适当降低精度和鲁棒性,以提升计算的速度，属于计算特别快的一种特征点(没有描述子)；而 ORB(Oriented FASTand Rotated BRIEF)特征则是改进了 FAST 检测子不具有方向性的问题,并采用速度极快的二进制描述子BRIEF,使整个图像特征提取的环节大大加速。根据作者在论文中所述测试,在同一幅图像中同时提取约 1000 个特征点的情况下,ORB 约要花费 15.3ms,SIFT 约花费 5228.7ms。由此可以看出,ORB在保持了特征子具有旋转、尺度不变性的同时,速度方面提升明显,对于实时性要求很高的SLAM来说是一个很好的选择。大部分特征提取都具有较好的并行性,可以通过 GPU 等设备来加速计算。经过GPU加速后的SIFT,就可以满足实时计算要求。但是,引入GPU将带来整个SLAM成本的提升。在目前的 SLAM方案中,ORB 是质量与性能之间较好的折中。

相关书籍和论文：

[39] E. Rosten and T. Drummond, “Machine learning for high-speed corner detection,” in Computer Vision–ECCV 2006, pp. 430–443, Springer, 2006.

[41] D. G. Lowe, “Distinctive image features from scale-invariant keypoints,” International Journal of Computer Vision, vol. 60, no. 2, pp. 91–110, 2004.

[43] E. Rublee, V. Rabaud, K. Konolige, and G. Bradski, “Orb: an efficient alternative to sift or surf,” in 2011 IEEE International Conference on Computer Vision (ICCV), pp. 2564–2571, IEEE, 2011.

[45] M. Nixon and A. S. Aguado, Feature extraction and image processing for computer vision. Academic Press, 2012.

7.1.2 ORB 特征

ORB特征也由关键点和描述子两部分组成。它的关键点称为“Oriented FAST”,是一种改进的FAST角点。它的描述子称为BRIEF(Binary Robust Independent Elementary Feature)。提取 ORB 特征分为如下两个步骤:

1. FAST 角点提取:找出图像中的“角点”。相较于原版的 FAST,ORB中计算了特征点的主方向,为后续的BRIEF描述子增加了旋转不变特性。
2. BRIEF 描述子:对前一步提取出特征点的周围图像区域进行描述。ORB 对BRIEF进行了一些改进,主要是指在 BRIEF中使用了先前计算的方向信息。

FAST 关键点

FAST 是一种角点,主要检测局部像素灰度变化明显的地方,以速度快著称。它的思想是:如果一个像素与邻域的像素差别较大(过亮或过暗),那么它更可能是角点。相比于其他角点检测算法,FAST 只需比较像素亮度的大小,十分快捷。它的检测过程如下:

1. 在图像中选取像素p,假设它的亮度为Ip。
2. 设置一个阈值T (比如,Ip 的20%)。
3. 以像素p为中心,选取半径为3的圆上的16个像素点。
4. 假如选取的圆上有连续的N个点的亮度大于 Ip + T 或小于 Ip − T,那么像素p可以被认为是特征点(N通常取12,即为 FAST-12。其他常用的 N 取值为 9 和 11,它们分别被称为FAST-9 和 FAST-11)。
5. 循环以上四步,对每一个像素执行相同的操作。

在 FAST-12 算法中,为了更高效,可以添加一项预测试操作,以快速地排除绝大多数不是角点的像素。具体操作为,对于每个像素,直接检测邻域圆上的第 1, 5, 9, 13 个像素的亮度。只有当这 4个像素中有 3 个同时大于 Ip + T 或小于 Ip − T 时,当前像素才有可能是一个角点,否则应该直接排除。这样的预测试操作大大加速了角点检测。此外,原始的 FAST角点经常出现“扎堆”的现象。所以在第一遍检测之后，还需要用非极大值抑制（Non-maximal suppression），在一定区域内仅保留响应极大值的角点，避免角点集中的问题。

FAST特征点的计算仅仅是比较像素间亮度的差异，所以速度非常快，但它也有重复性不强，分布不均匀的缺点。此外，FAST角点不具有方向信息。同时，由于它固定取半径为3的圆，存在尺度问题：远处看着像是角点的地方，接近后看可能就不是角点了。针对FAST角点不具有方向性和尺度的弱点，ORB添加了尺度和旋转的描述。尺度不变性由构建图像金字塔，并在金字塔的每一层上检测角点来实现。而特征的旋转是由灰度质心法（Intensity Centroid）实现的。 金字塔是计算图视觉中常用的一种处理方法，如下图。

金字塔底层是原始图像。每往上一层，就对图像进行一个固定倍率的缩放，这样就有了不同分辨率的图像。较小的图像可以看成是远处看过来的场景。在特征匹配算法中可以匹配不同层上的图像，从而实现尺度不变性。 例如，如果相机在后退，那么应该能够在上一个图像金字塔的上层和下一个图像的下层中找到匹配。 在旋转方面，计算特征点附近的图像灰度质心。所谓质心是指以图像块灰度值作为权重的中心。其具体操作步骤如下：

1. 在一个小的图像块B中，定义图像块的矩为

2. 通过矩可以找到图像块的质心：

3. 连接图像块的几何中心O与质心 C，得到一个方向向量 OC，于是特征点的方向可以定义为

通过以上方法，FAST角点便具有了尺度与旋转的描述，从而大大提升了其在不同图像之间表述的鲁棒性。ORB 中，把这种改进后的FAST称为 Oriented FAST。

注：金字塔是指对图像进行不同层次的降采样，以获得不同分辨率的图像。

BRIEF 描述子

在提取 Oriented FAST 关键点后，对每个点计算其描述子。ORB使用改进的BRIEF特征描述。

BRIEF 是一种二进制描述子，其描述向量由许多个0和1组成，这里的0和1编码了关键点附近两个随机像素（比如 p 和 q）的大小关系：如果 p 比 q 大，则取 1，反之就取 0。如果取了 128 个这样的 p, q，最后就得到128维由 0、1 组成的向量。BRIEF 使用了随机选点的比较，速度非常快，而且由于使用了二进制表达，存储起来也十分方便，适用于实时的图像匹配。原始的 BRIEF描述子不具有旋转不变性，因此在图像发生旋转时容易丢失。而 ORB 在 FAST 特征点提取阶段计算了关键点的方向，所以可以利用方向信息，计算了旋转之后的“Steer BRIEF”特征使ORB 的描述子具有较好的旋转不变性。 由于考虑到了旋转和缩放，使得 ORB 在平移、旋转和缩放的变换下仍有良好的表现。同时，FAST和BREIF的组合也非常高效，使得ORB特征在实时SLAM中非常受欢迎。

7.1.3 特征匹配

特征匹配是视觉 SLAM 中极为关键的一步，解决了SLAM中的数据关联问题（data association），即确定当前看到的路标与之前看到的路标之间的对应关系。通过对图像与图像或者图像与地图之间的描述子进行准确匹配，可以为后续的姿态估计、优化等操作减轻大量负担。然而，由于图像特征的局部特性，误匹配的情况广泛存在，成为视觉SLAM中制约性能提升的一大瓶颈。部分原因是场景中经常存在大量的重复纹理，使得特征描述非常相似。在这种情况下，仅利用局部特征解决误匹配是非常困难的。

考虑两个时刻的图像。如果在图像It中提取到特征点

在图像 It+1 中也提取到特征点xnt+1。如何寻找这两个集合元素的对应关系？最简单的特征匹配方法就是暴力匹配（Brute-Force Matcher）。即对每一个特征点 xm t 与所有的 xn t+1 测量描述子的距离，然后排序，取最近的一 个作为匹配点。描述子距离表示了两个特征之间的相似程度，不过在实际运用中还可以取不同的距离度量范数。对于浮点类型的描述子，使用欧氏距离进行度量即可。而对于二进制的描述子（比如BRIEF），往往使用汉明距离（Hamming distance）作为度量——两个二进制串之间的汉明距离，指的是其不同位数的个数。 然而，当特征点数量很大时，暴力匹配法的运算量将变得很大，特别是当想要匹配某个帧和一张地图的时候。这不符合在SLAM中的实时性需求。此时快速近似最近邻（FLANN）算法更加适合于匹配点数量极多的情况。这些匹配算法理论已经成熟，实现上集成到OpenCV了。

相关文献：

[47] M. Muja and D. G. Lowe, “Fast approximate nearest neighbors with automatic algorithm configuration.,” in VISAPP (1), pp. 331–340, 2009.

7.2 实践：特征提取和匹配

OpenCV集成了多数主流的图像特征，可以很方便地进行调用。

7.2.1 OpenCV 的 ORB 特征

调用OpenCV来提取和匹配ORB。图像位于slambook2/ch7/下的1.png 和 2.png，可以看到相机发生了微小的运动。程序演示如何提取ORB特征并进行匹配：slambook/ch7/orb_cv.cpp

首先要安装g2o，github下载20200410版本的:https://github.com/RainerKuemmerle/g2o.git

对ch7文件夹中的cmake工程进行编译，报错：

undefined reference to `vtable for fmt::v7::format_error‘

链接上fmt 库：

target_link_libraries("可执行文件" ${Sophus_LIBRARIES} fmt)

运行结果：

未筛选的匹配中带有大量的误匹配。经过一次筛选之后，匹配数量减少了许多，大多数匹配都是正确的。这里筛选的依据是汉明距离小于最小距离的两倍，这是一种工程上的经验方法。尽管在示例图像中能够筛选出正确的匹配，但仍然不能保证在所有其他图像中得到的匹配都是正确的。因此，在后面的运动估计中，还需要使用去除误匹配的算法。

7.2.2 手写 ORB 特征

代码：slambook/ch7/orb_self.cpp

运行结果：

在计算中用256位的二进制描述，即对应到8个32位的unsigned int数据，用 typedef 将它表示成 DescType。然后计算 FAST 特征点的角度，再使用该角度计算描述子。此代码中通过三角函数的原理回避了复杂的 arctan 以及 sin、cos 计算，从而达到加速的效果。在 BfMatch 函数中还使用了 SSE 指令集中的_mm_popcnt_u32 函数来计算一个 unsigned int 变量中1的个数，从而达到计算汉明距离的效果。

这个程序中，通过一些简单的算法修改，对ORB的提取加速了数倍。如果对提取特征部分进一步并行化处理，算法还可以有加速的空间。

7.2.3 计算相机运动

有了匹配好的点对后要根据点对来估计相机的运动。

1. 当相机为单目时，只知道2D的像素坐标，因而问题是根据两组2D点估计运动。该问题用对极几何来解决。
2. 当相机为双目、RGB-D 时，或者通过某种方法得到了距离信息，那么问题就是根据两组3D点估计运动。该问题通常用 ICP 来解决。
3. 如果一组为 3D，一组为 2D，即得到了一些 3D 点和它们在相机的投影位置，也能估计相机的运动。该问题通过 PnP 求解。

今天的文章视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点法和特征提取和匹配实践分享到此就结束了，感谢您的阅读。