1. 理解图像特征点的意义, 并掌握在单幅图像中提取出特征点，及多幅图像中匹配
特征点的方法。
2. 理解对极几何的原理，利用对极几何的约束，恢复出图像之间的摄像机的三维运
动。
3. 理解 PNP 问题，及利用已知三维结构与图像的对应关系，求解摄像机的三维运
动。
4. 理解 ICP 问题，及利用点云的匹配关系，求解摄像机的三维运动。
5. 理解如何通过三角化，获得二维图像上对应点的三维结构。
7.1 特征点法
前端也称为视觉里程计（VO） ，根据相邻图像的信息，估计出粗略的相机运动，给后端提供较好的初始值。
VO分为两种：特征点法的前端以及不提特征的直接法前端 。
基于特征点法的前端运行稳定，对光照、动态物体不敏感，目前比较成熟 。
7.1.1 特征点
长年的研究中，设计了许多更加稳定的局部图像特征，如著名的 SIFT（计算量大）, SURF,ORB 等等。相比于朴素的角点，这些人工设计的特征点能够拥有如下的性质：
1. 可重复性（Repeatability）：相同的“区域”可以在不同的图像中被找到。
2. 可区别性（Distinctiveness）：不同的“区域”有不同的表达。
3. 高效率（Efciency）：同一图像中，特征点的数量应远小于像素的数量。
4. 本地性（Locality）：特征仅与一小片图像区域相关。
特征点由关键点（Key-point）和描述子（Descriptor）两部分组成
7.1.2 ORB特征
ORB 特征亦由关键点和描述子两部分组成。它的关键点称为“Oriented FAST”，是一种改进的 FAST 角点，什么是 FAST 角点我们将在下文介绍。
’它的描述子称为 BRIEF（Binary Robust Independent Elementary Features）。因此，提取 ORB 特征分为两个步骤：
1. FAST 角点提取：找出图像中的” 角点”。相较于原版的 FAST, ORB 中计算了特征点的主方向，为后续的 BRIEF 描述子增加了旋转不变特性。
2. BRIEF 描述子：对前一步提取出特征点的周围图像区域进行描述
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. 循环以上四步，对每一个像素执行相同的操作。
预测试操作 ：每个像素，直接检测邻域圆上的第 1， 5， 9， 13 个像素的亮度 。
原始的 FAST 角点经常出现“扎堆”的现象。所以在第一遍检测之后，还需要用非极大值抑制（Non-maximal suppression），在一定区域内仅保留响应极大值的角点，避免角点集中的问题。
FAST 特征点数量很大且不确定，而我们往往希望对图像提取固定数量的特征。
因此，在 ORB 中，对原始的 FAST 算法进行了改进。我们可以指定最终要提取的角点数量N，对原始 FAST 角点分别计算 Harris 响应值，然后选取前 N 个具有最大响应值的角点，作为最终的角点集合。
FAST 角点不具有方向信息。而且，由于它固定取半径为 3 的圆，存在尺度问题：远处看着像是角点的地方，接近后看可能就不是角点了。针对 FAST 角点不具有方向性和尺度的弱点， ORB 添加了尺度和旋转的描述。
尺度不变性由构建图像金字塔‹，并在金字塔的每一层上检测角点来实现。而特征的旋转是由灰度质心法（Intensity Centroid）实现的。
BRIEF 描述子
BRIEF 是一种二进制描述子，它的描述向量由许多个 0 和 1 组成，这里的 0 和 1 编码了关键点附近两个像素（比如说 p 和 q）的大小关系：如果 p 比 q 大，则取 1，反之就取 0。
ORB 在 FAST 特征点提取阶段计算了关键点的方向，所以可以利用方向信息，计算了旋转之后的“Steer BRIEF”特征，使 ORB 的描述子具有较好的旋转不变性。
7.1.3特征匹配
快速近似最近邻（FLANN） 集成在opencv
7.2 实践 特征提取和匹配
7.3 2D-2D 对极几何
求取两帧图像 I1; I2 之间的运动，设第一帧到第二帧的运动为R; t。两个相机中心分别为 O1; O2。现在，考虑 I1 中有一个特征点 p1，
它在 I2 中对应着 特征点 p2 。这里我们需要一些术语来描述它们之间的几何关系。首先，连线 O1p1 和连线 O2p2 在三维空间中会相交于点 P。这时候点 O1; O2; P 三个点可以
确定一个平面，称为极平面（Epipolar plane） 。 O1O2 连线与像平面 I1; I2 的交点分别为 e1; e2。 e1; e2，称为极点（Epipoles） ， O1O2 被称为基线（Baseline） 。称极平面与
两个像平面 I1; I2 之间的相交线 l1; l2 为极线（Epipolar line） 。
通过特征匹配的p1,p2就可以确定p的值。
以上两个式子就是对极约束。对极约束中同时包含了平移和旋转
基础矩阵（Fundamental Matrix） F 和本质矩阵（Essential Matrix） E，
相机位姿估计问题变为以下两步
1. 根据配对点的像素位置，求出 E 或者 F；
2. 根据 E 或者 F，求出 R; t。
7.3.2本质矩阵
E 在不同尺度下是等价的
本质矩阵 E 的奇异值必定是 [σ; σ; 0]T
只考虑它的尺度等价性，使用八对点来估计 E——这就是经典的八点法
7.3.3单应矩阵
单应矩阵通常描述处于共同平面上的一些点，在两张图像之间的变换关系。
无人机俯视相机、扫地机顶视相机
考虑在图像 I1 和 I2 有一对匹配好的特征点 p1 和 p2。这些特征点落在某平面上。设这个平面满足方程： 
这种做法把 H 矩阵看成了向量，通过解该向量的线性方程来恢复 H，又称直接线性变换法（Direct Linear Transform）。
单应性在 SLAM 中具重要意义。当特征点共面，或者相机发生纯旋转的时候，基础矩阵的自由度下降，这就出现了所谓的退化（degenerate）。
能够避免退化现象造成的影响，通常我们会同时估计基础矩阵 F 和单应矩阵H，选择重投影误差比较小的那个作为最终的运动估计矩阵
7.5 三角测量
三角测量是指，通过在两处观察同一个点的夹角，确定该点的距离。
然而由于噪声的影响，O1P1 O2P2直线往往无法相交。 通过最二小乘去求解。
已经知道了 R; t，想要求解的是两个特征点的深度 s1; s2。
理想条件下求零解，常见做法最小二乘
7.7 3D-2D ： PnP
PnP（Perspective-n-Point）是求解 3D 到 2D 点对运动的方法。它描述了当我们知道n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿。
如果两张图像中，其中一张特征点的 3D 位置已知，那么最少只需三个点对（需要至少一个额外点验证结果）就可以估计相机运动。
特征点的 3D 位置可以由三角化，或者由 RGB-D 相机的深度图确定。因此，在双目或 RGB-D 的视觉里程计中，我们可以直接使用 PnP 估计相机运动。
在单目视觉里程计中，必须先进行初始化，然后才能使用 PnP。 3D-2D 方法不需要使用对极约束，又可以在很少的匹配点中获得较好的运
动估计，是最重要的一种姿态估计方法。
PnP 问题有很多种求解方法，例如用三对点估计位姿的 P3P[45]，直接线性变换（DLT），EPnP（Efcient PnP） [46]， UPnP[47] 等等）。此外，还能用非线性优化的方式，构建最
小二乘问题并迭代求解，也就是万金油式的 Bundle Adjustment。我们先来看 DLT，然后再讲 Bundle Adjustment。 
P3P 需要利用给定的三个点的几何关系。它的输入数据为三对 3D-2D 匹配点
该方程组是关于 x; y 的一个二元二
次方程（多项式方程）。
该方程组是关于 x; y 的一个二元二次方程（多项式方程）。 该方程最多可能得到四个解，但我们可以用验证点来计算最可能的解，得到 A; B; C 在相机坐标系下 的 3D 坐标。然后，根据 3D-3D 的点对，计算相机的运动 R; t。
为了求解 PnP，我们利用了三角形相似性质，求解投影点 a; b; c 在相机坐标系下的 3D 坐标，最后把问题转换成一个 3D 到 3D 的位姿估计问题。
1. P3P 只利用三个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息。
2. 如果 3D 点或 2D 点受噪声影响，或者存在误匹配，则算法失效。
所以后续人们还提出了许多别的方法，如 EPnP、 UPnP 等。它们利用更多的信息，而且用迭代的方式对相机位姿进行优化，以尽可能地消除噪声的影响。
7.7.3 Bundle Adjustment
除了使用线性方法之外，我们可以把 PnP 问题构建成一个定义于李代数上的非线性最小二乘问题。这将用到本书第四章和第五章的知识。
构建最小二乘问题，然后寻找最好的相机位姿，使它最小化
该问题的误差项，是将像素坐标（观测到的投影位置）与 3D 点按照当前估计的位姿进行投影得到的位置相比较得到的误差，所以称之为重投影误差。
最小二乘优化问题已经在第六讲介绍过了。使用李代数，可以构建无约束的优化问题 在使用 G-N 和 L-M 之前，我们需要知道每个误差项关于优化变量的导数，也就是线性化
这里的 J 的形式是值得讨论的，甚至可以说是关键所在。 后面看不懂了....先把公式贴上来。
记变换到相机坐标系下的空间点坐标为 P ′，并且把它前三维取出来：
相机投影模型相对于 P ′ 则为： 
⊕ 指李代数上的左乘扰动。 。。。。。
结果2 × 6 的雅可比矩阵： 
一方面，除了优化位姿，我们还希望优化特征点的空间位置。因此，需要讨论 e 关于空间点 P 的导数。所幸这个导数矩阵相对来说容易一些。仍利用链式法则，有：
7.9 ICP
3D-3D 的位姿估计问题。
有一组配对好的 3D 点
想要找一个欧氏变换 R; t，使得：
可以用迭代最近点（Iterative Closest Point, ICP）求解。 ICP 的求解也分为两种方式：利用线性代数的求解（主要是 SVD），以及利用非线性优化方式的求解（类似于 Bundle Adjustment）。
各环节