求解视觉里程计（基于特征点法）

1 视觉里程计（VO）

视觉SLAM的基本框架已经学习，其中由两部分非常重要，那就是前端和后端。视觉前端就被成为视觉里程计（VO），其根据相邻图像的信息，估计出粗略的相机位姿，给后端提供较好的位姿初始值。

视觉里程计的实现方法分两种：
①对图像提取特征点的特征点法
②不对图像进行特征提取的直接法

基于特征点法的前端，长久以来（直到现在）被认为是视觉里程计的主流方法。它运行稳定，对光照、动态物体不敏感，是目前比较成熟的解决方案。
所以先来学习下特征点法。

2 基于特征点法的视觉里程计算法

基于特征点法的视觉里程计的工作原理是：

首先，从图像中提取特征点。特征点在相机视角发生少量变化后会保持不变。
然后，在其他图像中匹配这些相同的特征点，根据位姿估计算法估计相机的位姿以及特征点的空间位置。这些点被称为路标点，也就是图像特征。

2.1 特征点

通过对视觉里程计的工作原理的了解，我们可以知道，特征点在相机少量运动之后需要保持稳定。让计算机能在以后的图像序列中重新找到那个点，然后产生关联。这样我们才能设计算法求出两帧图像帧之间对应的相机运动。

现在已经出现了很多关于特征点的研究，比如SIFT,SURF,ORB等，他们有以下良好的性质：

1、可重复性：可以在不同的图像中找到。
2、可区别性：不同的特征点有不同的表达。
3、高效率：同一图像中，特征点数量远小于像素的数量（在能够明显表示此图像特征的情况下用更少的像素信息，这可以在后面的计算中显著提高计算效率）。
4、局部性：特征点仅可以表示一小片图像区域。

特征点由关键点和描述子两部分组成。可以这样说：关键点是指该特征点在图像里的位置，有些特征点还具有朝向、大小等信息。描述子通常是一个向量，按照某种人为设计的方式，描述了该关键点周围像素的信息。所以只要两个特征点的描述子在向量空间上距离相近，就可以认为他们是相同的特征点。

前面说现在已经有了很多著名的特征点，其中ORB特征点在视觉SLAM领域应用最多。因为ORB特征点在保持了特征子具有旋转，尺度不变性的同时，速度方面提升明显，对于实时性要求很高的 SLAM来说是一个很好的选择。

2.2 ORB特征点的提取与匹配

ORB特征点全称为“Oriented FAST and Rotated BRIEF ”，是一种改进的FAST角点。带有旋转不变性和尺度不变性。
下面看它是怎么带有旋转不变性和尺度不变性的。

---

提取 ORB 特征分为两个步骤：

1. FAST 角点提取：找出图像中的” 角点”。相较于原版的 FAST, ORB 中计算了特征点的主方向，为后续的 BRIEF 描述子增加了旋转不变特性。
2. BRIEF 描述子：对前一步提取出特征点的周围图像区域进行描述。

2.2.1 关键点与描述子

FAST角点不具有方向性，相机旋转一下就变成了别的特征点。它以固定半径3的圆选取像素点进行比较，这使得相近在远近运动时，特征点也改变了。
针对 FAST 角点不具有方向性和尺度的弱点，ORB 添加了尺度和旋转的描述。
尺度不变性由构建图像金字塔，并在金字塔的每一层上检测角点来实现。
特征的旋转是由灰度质心法实现的。

图像金字塔比较容易理解，指对图像进行不同层次的降采样，以获得不同分辨率的图像。来表示相机离特征点远近这么一个感觉。

---

灰度质心法

用灰度质心法解决其旋转问题。

1. 第1步：我们定义该区域图像的矩为：
   
   其中，$p,q$取0或1；$I(x,y)$表示在像素坐标$(x,y)$处图像的灰度值； $m_{pq}$表示图像的矩。
   在半径为$R$的圆形图像区域，沿两个坐标轴 $x,y$方向的图像矩分别为：
   圆形区域内所有像素的灰度值总和为：
   

2. 第2步：求出图像的质心为：
   

3. 第3步：然后关键点的“主方向”就可以表示为从圆形图像形心$O$ 指向质心 $C$的方向向量$\overrightarrow{OC}$，于是关键点的旋转角度记为：
   

用图表示上面这个过程就是：
这样每个关键点都带有这么一个角度信息，相机旋转时程序实时计算每个特征点的角度信息，然后旋转回去再对两个特征点进行比较，不就好比较了嘛。

特征描述子计算

$BRIEF$ 是一种二进制描述子，它的描述向量由许多个 0 和 1 组成，这里的 0 和 1 编码了关键点附近两个像素（比如说 $p$ 和 $q$）的大小关系：如果 $p$ 比$q$ 大，则取 1，反之就取 0。
$BRIEF$算法的核心思想是在关键点$P$的周围以一定模式选取$N$个点对，把这$N$个点对的比较结果组合起来作为描述子。
那么，具体的两个像素$p$、$q$该怎么选呢？
为了保持踩点固定，工程上采用特殊设计的固定的$pattern$来做。下面是几种$pattern$：

原始的 BRIEF 描述子是不具有旋转不变性的，因此在图像发生旋转时容易丢失。而 ORB 在 FAST 特征点提取阶段计算了关键点的方向，所以可以利用方向信息，计算了旋转之后的“Steer BRIEF”特征，使 ORB 的描述子具有较好的旋转不变性。

2.2.2 特征点匹配

特征匹配就是确定当前看到的路标与之前看到的路标之间的对应关系，也就是SLAM中的数据关联问题。通过对图像与图像，或者图像与地图之间的描述子进行准确的匹配，我们可以为后续的姿态估计，优化等操作减轻大量负担。

毋庸置疑，特征匹配的准确性决定了视觉SLAM的位姿估计性能，然而由于图像特征的局部特性，误匹配的情况广泛存在，而且长期以来一直没有得到有效解决，目前已经成为视觉 SLAM 中制约性能提升的一大瓶颈。比如在ORB-SLAM中作者就花费了大量的计算和技巧去剔除误匹配。
描述子距离表示了两个特征之间的相似程度，不过在实际运用中还可以取不同的距离度量范数。对于二进制的描述子，使用汉明距离（Hamming distance）做为度量。两个二进制串之间的汉明距离，指的是它们不同位数的个数。
对应在 SLAM 中的实时性需求，基于快速近似最近邻（FLANN）算法可以实时匹配大量的特征点。可以通过$OpenCV$实现。
效果图：

2.3 特征点法估计相机位姿

上面我们进行了特征点的匹配，说解决了视觉SLAM中的数据关联问题，那这有什么用呢？—那就是求解相机运动。

如果我们有若干对这样的匹配点，就可以通过这些二维图像点的对应关系，恢复出在两帧之间相机的运动。

2.3.1 对极几何约束

注意：这里是通过两张图象的二维图像点恢复相机运动。（2D-2D）
假设我们从两张图像中，得到了一对配对好的特征点$p_1,p_2$，假定匹配完全正确，那么$p_1,p_2$就是$P$分别在两帧上的投影，如下图，这就是我们熟悉的对极几何约束。
假设相机从$O_1$运动到$O_2$，我们求解两帧之间的运动$R,t$。

在第一帧的相机坐标系下，设 $P$ 的空间位置为：

根据针孔相机模型，两个像素点 p1, p2 的像素位置为：
变成齐次坐标的形式：

等式两边乘以 $K^{-1}$,就是将像素坐标系的点反投影到归一化平面上：
由 $p_1,p_2$ 的关系不难得到：

两边同时乘以 $t$^

^这个符号之前说过是反对称的意思，也是做叉积的意思。
然后，两侧同时左乘 $x_2^T$:

注意观察左式
$t$ 和 $x_2$作叉积之后，就是一个垂直于 $x_2$ 的向量，然后再与 $x_2$ 做内积，结果是 $0$，即

再将 $p_1,p_2$ 带入，

上面两个式子都叫对极约束。
再进一步简化，定义

得到简化形式的对极约束：

定义的这两个矩阵，$E$称为本质矩阵， $F$称为基础矩阵。

很明显他两者之间就差了一个相机内参。由于我们做视觉SLAM一般会提前标定相机内参，所以我们只需要从本质矩阵$E$着手求解$R,t$即可。

关于视觉SLAM中相关的几个矩阵的自由度和秩分析，可以看我的另一篇博客。

2.3.2 八点法求解本质矩阵E,估计相机运动

我们将2.3.1中的归一化坐标设为：

根据对极约束有：

充分展开上式就可以得到带有 $E$ 的向量形式 $e$ 的等式。

将八对匹配点带入对极约束可以得到一个线性方程组。

接下来的问题是如何根据已经估得的本质矩阵 $E$，恢复出相机的运动 $R,t$。这个过程是由奇异值分解（SVD）得到的。
设$E$的 $SVD$ 分解形式为：

其中 $U,V$ 为正交阵，$\Sigma$ 为奇异值矩阵。
在 $SVD$ 分解中，对于任意一个 $E$，存在两个可能的 $t,R$ 与之对应：

其中 $R_Z(\pi /2)$ 表示沿 $Z$ 轴旋转 $90$ 度得到的旋转矩阵。同时，由于 $-E$ 和 $E$ 等价，所以对任意一个 $t$ 取负号，也会得到同样的结果。这使得通过 $SVD$ 分解可以得到四组解，分别对应下方四种情况。

很显然，只有第一种情况两个相机观察到的点在各帧坐标系中的深度值是正的。所以我们可以通过把任意一点带入解中，然后判断其深度值是否为正即可。

非常感谢您的阅读并期待您加入！

期待您加入我的微信公众号-“过千帆”，那里除了技术文章之外，还有我的读书分享同样希望能帮到您。