SLAM——ORB特征提取、对极约束（E\F）、单应矩阵和三角测量

1.特征点法

视觉SLAM主要分为视觉前端和优化后端。前端也称为视觉里程计（VO），根据相邻图像的信息估计处粗略的相机运动，给够后端提供较好的初始值。 VO的实现方法，按是否需要提取特征，可分为特征点的前端和不提特征点的直接法前端。

VO的主要问题是如何根据图像估计相机的运动：从图像选取有代表性的点（路标），在相机发生微笑变化时会保持不变，接着从各个图片找到相同的点，进而讨论相机位姿估计问题。其中，一种直观的提取特征方式就是在不同图像间辨认角点，确定其关系。

特征点由关键点和描述子俩部分组成。“提取关键点，计算描述子。关键点是指该特征点在图像里的位置，有些具有朝向、大小等。描述子为向量，描述了该关键点周围像素的信息，“外观相似的特征应有相似的描述子”。

特征提取：SIFT，考虑了图像变换过程中出现的光照等变化，但是计算量大，需要经过GPU加速后才能满足实时要求。FAST关键点，没有描述子。ORB特征提取，改进了FAST不具方向的问题，运用了BRIEF描述子，较好的选择。

1.1 ORB特征

俩个步骤：
1.FAST角点提取：找出角点，计算特征点主方向，为后面的描述子增加了旋转不变性。
2.对前一步提出特征点的周围图像区域进行描述。

A. 原始FAST关键点：

主要检测局部像素灰度变化明显的地方，速度快。主要思想是：如果一个像素与领域像素差别较大，则很有可能是角点。

1.在图像中选取像素p，假设它的亮度为 $I_p$ 。
2.设置一个阈值$T$（比如，$I_p$ 的$30\%$）
3.以像素为中心，选取半径为3的圆上的16个像素点。
4.假如圆上有连续的N个点的亮度大于$I_p+T$ 或小于 $I_p-T$，那么像素$p$可以被认为是特征点（$N$通常取12，FAST-12，其他取值为9和11）
5.对每个像素执行此操作。

虽然速度快，但是经常会出现角点“扎堆”，FAST特征点数量很大但是不确定，而我们往往希望提取固定数量的特征点。

B. ORB改进后的FAST关键点（Oriented FAST）:

在满足上述方法的前提下，对角点进行Harris响应值计算，然后选取前N个具有最大响应值的角点作为最终角点集合。同时添加了尺度和旋转的描述，尺度不变性：由构建图像金字塔，并在金字塔的每一层上检测角点来实现。特征的旋转：由灰度质心法实现。

灰度质心法：质心是指以图像块灰度值作为权重的中心。
1.在图像块B中，定义图像块的矩为

$m_{pq}={\sum }_{x,y\in B}{x}^p{y}^{q}I(x,y)$，$ p,q={0,1}$，$I(x,y) $ 为灰度值，即所谓的亮度

2.通过矩可以找到图像块的质心：$C=(\frac{m_{10}}{m_{00}},\frac{m_{01}}{m_{00}})$

3.连接图像块的几何中心O(类似于原点那种)与质心，得到方向向量$\overrightarrow{OC}$，于是特征点的方向可以定义为：

$\theta =arctan(m_{01}/m_{10})$

C. BRIEF描述子（Steer BRIEF）

BRIEF是一种二进制描述子，有许多0,1组成，这里的0,1编码了关键点附近俩个像素(比如p和q的大小)关系：如果p比q大，那么取1，反之取0。$p$和$q$是按照某种概率分布随机选点的比较。如果个我们选择了128个这样的$p,q$，最 后就得到128维由0,1组成的向量。

俩个特征点的描述子在向量空间上的距离相近，就可以认为他们是同样的特征点。

1.2 特征匹配

特征匹配是视觉SLAM中的数据关联问题，即确定当前看到路标和之前看到的路标之间的对应关系。通过图像与图像或图像与地图之间的描述子进行准确匹配，可以为后续的姿态估计减轻负担，但是误匹配广泛存在，是限制SLAM的一大瓶颈。

图像 $I_t$ 特征点有 $x_t^m$，$m=1,2,...,M$
图像$I_{t+1}$ 有$x_{t+1}^n$，$n=1,2,...,N$ 如何匹配呢？

暴力匹配： 对图像上的所有特征点历遍求与跟描述子的距离，进行比较，求出最小值那个。

汉明距离：俩个二进制串之间的汉明距离，指的是其不同位数的个数。

2.2D-2D：对极约束

假如从俩张图片中得到了一对配对好的特征点，如果有若干对这样的匹配点，那么可以通过这些二维图像点的对应关系，恢复俩帧之间摄像机的运动。

以上示意图中，$O_1,O_2,P$ 三个点可以确定一个平面,称为极平面。$O_1{O}_2$连线与像平面的交点分别为 $e_1,e_2$,称为极点, $O_1{O}_2$ 被称为基线。称极平面与两个像平面 $I_1,I_2$ 之间的相交线$l_1{l}_2$ 为极线。

通过推导，我们可以定义本质矩阵$E=t^{\wedge }R$，定义基础矩阵$F=K^{-T}E{K}^{-1}$,且有 $x_2^{T}E{x}_1=p_2^{T}F{p}_1=0$,其中 $p$ 点为像素点，$x$ 点为像素点 $p$ 转换到归一化平面上的点。

于是，相机估计问题变为了以下俩步：
1.根据配对点的像素位置求出本质矩阵E和基础矩阵F
2.根据E和F求出R和t

2.1 本质矩阵

本质矩阵的特点：
1.本质矩阵是对极约束的，由于对极约束是等式为0的约束，所以对E乘以任何非零常数后，对极约束依然满足，极E在不同尺度下依然是满足的。
2.奇异值为 $[\gamma ,\gamma ,0{]}^T$ 形式的。
3.由于平移和旋转各有3个自由度，$t^{\wedge }R$ 有6个自由度，由于尺度不变性原因，E有5个自由度。

至于本质矩阵的求解，可以利用8点法，$x_2^{T}E{x}_1=$