视觉SLAM十四讲学习笔记——第七章 视觉里程计

7.1 特征点法

VO 的实现方法，按是否需要提取特征，分为特征点法的前端以及不提特征的直
接法前端，基于特征点法的前端，长久以来（直到现在）被认为是视觉里程计的主流方法。

7.1.1 特征点

1. 从图像中选取比较有代表性的点，这些点在相机视角发生少量变化后会保持不变，所以会在各个图像中找到相同的点，这些点称为特征点，也称为图像特征。
2. 特征点是图像里一些特别的地方，可以把图像中的角点、边缘和区块都当成图像中有代表性的地方。角点是最具有特征的属性。
3. 朴素的角点不易满足要求，而人工设计的特征点具有很好的性质：
   1. 可重复性：相同的“区域”可以在不同的图像中被找到
   2. 可区别性：不同的“区域”有不同的表达
   3. 高效率：同一图像中，特征点的数量应远小于像素的数量
   4. 本地性：特征仅与一小片图像区域相关。
4. 特征点由关键点和描述子两部分组成
   1. 关键点是指该特征点在图像里的位置，有些特征点还具有朝向、大小等信息。
   2. 描述子通常是一个向量，按照某种人为设计的方式，描述了该关键点周围像素的信息，描述子是按照“外观相似的特征应该有相似的描述子”的原则来设计
   3. 只要两个特征点的描述子在向量空间上的距离相近，就可以认为它们是同样的特征点
5. SIFT质量高但速度慢，成本高；FAST速度快但质量低，ORB是两者很好的折中

7.1.2 ORB 特征

1. ORB 特征亦由关键点和描述子两部分组成。关键点称为“Oriented FAST”，是
   一种改进的 FAST 角点，描述子称为 BRIEF。
2. 提取 ORB 特征分为两个步骤：
   1. FAST 角点提取：找出图像中的” 角点”。ORB 中计算了特征点的主方向，为后续的 BRIEF 描述子增加了旋转不变特性
   2. BRIEF 描述子：对前一步提取出特征点的周围图像区域进行描述。
3. FAST 是一种角点，主要检测局部像素灰度变化明显的地方，速度快，它的思
   想是：如果一个像素与它邻域的像素差别较大（过亮或过暗） , 那它更可能是角点。
4. FAST的检测过程：
   1. 在图像中选取像素 $p$ ，假设它的亮度为 $I_p$ 。
   2. 设置一个阈值 $T$ (比如 $I_p$ 的 $20\%$) 。
   3. 以像素 $p$ 为中心, 选取半径为 3 的圆上的 16 个像素点。
   4. 假如选取的圆上，有连续的 $N$ 个点的亮度大于 $I_p+T$ 或小于 $I_p-T$ ，那么像素 $p$ 可以被认为是特征点 ( $N$ 通常取 12，即为 FAST-12。其它常用的 $N$ 取值为 9 和 11，他们分别被称为 FAST-9， FAST-11)。
   5. 循环以上四步，对每一个像素执行相同的操作。
5. 在 FAST-12 算法中，直接检测邻域圆上的第 1， 5， 9， 13 个像素的亮度。只有当这四个像素中有三个同时大于 $I_p+T$ 或小于 $I_p-T$ 时，当前像素才有可能是一个角点，否则应该直接排除。这样效率更高。同时，在一定区域内仅保留响应极大值的角点，避免角点集中的问题。
6. FAST算法的问题：FAST 特征点数量很大且不确定，角点不具有方向信息，尺度弱
7. 在ORB中，指定最终要提取的角点数量 $N$，对原始 FAST 角点分别计算 Harris 响应值，然后选取前 $N$ 个具有最大响应值的角点，作为最终的角点集合。ORB 添加了尺度和旋转的描述。
8. 质心是指以图像块灰度值作为权重的中心。其具体操作步骤如下：
   1. 在一个小的图像块 $B$ 中，定义图像块的矩为：
      
   2. 通过矩可以找到图像块的质心：
      
   3. 连接图像块的几何中心 $O$ 与质心 $C$ ，得到一个方向向量 $\mathbf{O}\mathbf{C}$，于是特征点的方向可以定义为：
      
9. BRIEF 是一种二进制描述子，它的描述向量由许多个 0 和 1 组成， 0 和 1 编
   码了关键点附近两个像素（$p$ 和 $q$）的大小关系：如果 $p$ 比 $q$ 大，则取 1，反之就取 0。若取128个点，则构成128维的0-1向量

7.1.3

1. 特征匹配解决了 SLAM 中的数据关联问题，即确定当前看到的路标与之前看到的路标之间的对应关系。
2. 由于图像特征的局部特性，误匹配的情况广泛存在，主要原因是因为场景中存在大量重复的纹理。
3. 暴力匹配：在图像 $I_t$ 中提取到特征点 $x_t^m，m=1,2,...M$，在图像 $I_{t+1}$ 中提取到特征点 $x_{t+1}^n，n=1,2,...N$ ，对每一个特征点 $x_t^m$ ，与所有的 $x_{t+1}^n$ 测量描述子的距离，然后排序，取最近的一个作为匹配点。
4. 描述子距离表示了两个特征之间的相似程度，对于浮点类型的描述子，使用欧氏距离进行度量，对于二进制的描述子，则采用汉明距离
5. 快速近似最近邻（FLANN） 算法更加适合于匹配点数量极多的情况

7.2 实践：特征提取和匹配

 

7.3 2D-2D: 对极几何

7.3.1 对极约束

1. 求取两帧图像 $I_1,I_2$ 之间的运动，设第一帧到第二帧的运动为 $\mathbf{R},\mathbf{t}$ 。两个相机中心分别为 $O_1,O_2$ 。考虑 $I_1$ 中有一个特征点 $p_1$ ，它在 $I_2$ 中对应着特征点 $p_2$ ，这两个点是同一个空间点在两个成像平面上的投影。连线 $O_1{p}_1$ 和连线 $O_2{p}_2$ 在三维空间中相交于点 $P$ 。点 $O_1,O_2,P$ 三个点确定一个平面，称为极平面，$O_1{O}_2$ 连线与像平面 $I_1,I_2$ 的交点分别为 $e_1,e_2$，称为极点。$O_1,O_2$ 称为基线 。称极平面与两个像平面 $I_1,I_2$ 之间的相交线 $l_1,l_2$ 为极线。
   
2. 对极约束
   
   
   ${\mathbf{x}}_1,{\mathbf{x}}_2$ 是两个像素点的归一化平面上的坐标，$\mathbf{K}$ 为相机内参矩阵
3. 把中间部分记作两个矩阵：基础矩阵 $\mathbf{F}$ 和本质矩阵 $\mathbf{E}$，可以进一步简化对极约束,实践当中往往使用形式更简单的 E：
    
   
   对极约束简洁地给出了两个匹配点的空间位置关系
4. 由上式，相机位姿估计问题变为以下两步：
   1. 根据配对点的像素位置，求出 $\mathbf{E}$ 或者 $\mathbf{F}$
   2. 根据 $\mathbf{E}$ 或者 $\mathbf{F}$，求出 $\mathbf{R},\mathbf{t}$ 。

7.3.2 本质矩阵

1. $\mathbf{E}={\mathbf{t}}^{\wedge }\mathbf{R}$ 。它是一个 $3\times 3$ 的矩阵，有 9 个未知数。从 $\mathbf{E}$ 的构造方式上看，有以下值得注意的地方：
   1. 对极约束是等式为零的约束，所以对 $\mathbf{E}$ 乘以任意非零常数后， 对极约束依然满足，称为 $\mathbf{E}$ 在不同尺度下是等价的
   2. 本质矩阵 $\mathbf{E}$ 的奇异值必定是 $[\sigma ,\sigma ,0{]}^T$ 的形式，称为本质矩阵的内在性质。
   3. 由于尺度等价性，故 $\mathbf{E}$ 实际上有五个自由度
2. 考虑它的尺度等价性，使用八对点来估计 $\mathbf{E}$ ——经典的八点法
   
   其中，$u^i,v^i$ 表示第 $i$ 对特征点，每对特征点的形式是 $(u_1{v}_1)，(u_2,v_2)$, $\mathbf{e}$向量是本质矩阵 $\mathbf{E}$ 的展开形式。。如果八对匹配点组成的矩阵满足秩为 8 的条件，那么 $\mathbf{E}$ 的各元素就可由上述方程解得。
3. 根据已经估得的本质矩阵 $\mathbf{E}$，恢复出相机的运动 $\mathbf{R},\mathbf{t}$ 。这个过程是由奇异值分解（SVD）得到。
   
   其中 $\mathbf{U},\mathbf{V}$ 为正交阵， $\mathbf{\Sigma }$ 为奇异值矩阵，根据 $\mathbf{E}$ 的内在性质，我们知道 $\mathbf{\Sigma }=diag(\sigma ,\sigma ,0)$
4. 从 $\mathbf{E}$ 分解到 $\mathbf{t},\mathbf{R}$ 时，一共存在四个可能的解：
   
   只有第一种解中， $P$ 在两个相机中都具有正的深度。把任意一点代入四种解中，检测该点在两个相机下的深度，就可以确定哪个解是正确的。
5. 根据线性方程解出的 $\mathbf{E}$，它的奇异值不一定为 $\sigma ,\sigma ,0$ 的形式。刻意地把 $\mathbf{\Sigma }$ 矩阵调整成上面的样子。做法是，对八点法求得的 $\mathbf{E}$ 进行 SVD 分解后，会得到奇异值矩阵 $\mathbf{\Sigma }=diag(\sigma 1,\sigma 2,\sigma 3)$ ，不妨设 ${\sigma }_1\ge {\sigma }_2\ge {\sigma }_3$ 。取：
    
   
   更简单的做法是将奇异值矩阵取成 $diag(1,1,0)$

7.3.3 单应矩阵

1. 单应描述了两个平面之间的映射关系。若场景中的特征点都落在同一平面上（比如墙，地面等），则可以通过单应性来进行运动估计。
2. 考虑在图像 $I_1$ 和 $I_2$ 有一对匹配好的特征点 $p_1$ 和 $p_2$ ，这些特征点落在某平面上，则有：
    
   
   得到了一个直接描述图像坐标 $p_1$ 和 $p_2$ 之间的变换，把中间这部分记为 $\mathbf{H}$ ，于是
    
   
3. 自由度为 8 的单应矩阵可以通过 4 对匹配特征点算出，注意，这些特征点不能有三点共线的情况：
   
   其中，未显示的 $h_9=1$。这种做法把 $\mathbf{H}$ 矩阵看成了向量，通过解该向量的线性方程来恢复 $\mathbf{H}$，又称直接线性变换法。
4. 与本质矩阵相似，求出单应矩阵以后需要对其进行分解，才可以得到相应的旋转矩阵 $\mathbf{R}$ 和平移向量 $\mathbf{t}$ 。分解的方法包括数值法与解析法。
5. 退化：当特征点共面，或者相机发生纯旋转的时候，基础矩阵的自由度下降。为了能够避免退化现象造成的影响，通常会同时估计基础矩阵 $\mathbf{F}$ 和单应矩阵 $\mathbf{H}$ ，选择重投影误差比较小的那个作为最终的运动估计矩阵。

7.4 实践：对极约束求解相机运动

7.4.1 讨论

1. 用 $\mathbf{E}$ 来分解运动时，通常把 $\mathbf{t}$ 进行归一化，让它的长度等于 1。
2. 对 $\mathbf{t}$ 长度的归一化，直接导致了单目视觉的尺度不确定性。对两张图像的 $\mathbf{t}$ 归一化，相当于固定了尺度。虽然不知道它的实际长度为多少，但以这时的 $\mathbf{t}$ 为单位 1，计算相机运动和特征点的 3D 位置，这称为单目 SLAM 的初始化。初始化之后的轨迹和地图的单位，就是初始化时固定的尺度，初始化的两张图像必须有一定程度的平移，而后的轨迹和地图都将以此步的
   平移为单位。
3. 另一种方法是令初始化时所有的特征点平均深度为 1，也可以固定一个尺度。把特征点深度归一化可以控制场景的规模大小，使计算在数值上更稳定
4. 从 $\mathbf{E}$ 分解到 $\mathbf{R},\mathbf{t}$ 的过程中，如果相机发生的是纯旋转，导致 $\mathbf{t}$ 为零，则得到的 $\mathbf{E}$ 也将为零，导致无从求解 $\mathbf{R}$，此时可以依靠 $\mathbf{H}$ 求取旋转，但仅有旋转时，无法用三角测量估计特征点的空间位置。所以单目初始化不能只有纯旋转，必须要有一定程度的平移
5. 当给定的点数多于八对时，可以计算最小二乘解：
    
   
   当可能存在误匹配的情况时，更倾向于使用随机采样一致性。

7.5 三角测量

1. 在单目 SLAM 中，仅通过单张图像无法获得像素的深度信息，需要通过三角测量（或三角化）的方法来估计地图点的深度。
2. 三角测量是指，通过在两处观察同一个点的夹角，确定该点的距离
   
   如上图，理论上直线 $O_1{p}_1$ 与 $O_2{p}_2$ 在场景中会相交于一点 $P$，该点即是两个特征点所对应的地图点在三维场景中的位置。然而由于噪声的影响，这两条直线往往无法相交。可以通过最二小乘去求解。
3. 按照对极几何中的定义，设 ${\mathbf{x}}_1,{\mathbf{x}}_2$ 为两个特征点的归一化坐标，它们满足：
    
   
   对上式两侧左乘一个 ${\mathbf{x}}_1^{\wedge }$ ，得：
    
   
   可以根据它直接求得 $s_2$ ，继而求得 $s_1$。由于噪声的存在，我们估得的 $\mathbf{R},\mathbf{t}$，不一定精确使上式为零，所以更常见的做法求最小二乘解而不是零解。

7.6 实践：三角测量

1. 三角测量是由平移得到的，有平移才会有对极几何中的三角形。
2. 当平移很小时，像素上的不确定性将导致较大的深度不确定性，平移较大时，在同样的相机分辨率下，三角化测量将更精确。
3. 要增加三角化的精度，其一是提高特征点的提取精度，但会增加成本。另一方式是使平移量增大。但是，平移量增大，会导致图像的外观发生明显的变化，导致匹配失效，这称为三角测量的矛盾。

7.7 3D-2D: PnP

PnP（Perspective-n-Point）是求解 3D 到 2D 点对运动的方法。它描述了当知道 n 个 3D 空间点以及它们的投影位置时，如何估计相机所在的位姿，3D-2D 方法不需要使用对极约束，又可以在很少的匹配点中获得较好的运动估计

7.7.1 直接线性变换

1. 考虑某个空间点 $P$，它的齐次坐标为 $\mathbf{P}=(X,Y,Z,1{)}^T$。在图像 $I_1$ 中，投影到特征
   点 ${\mathbf{x}}_1=(u_1,v_1,1{)}^T$（以归一化平面齐次坐标表示）。此时相机的位姿 $\mathbf{R},\mathbf{t}$ 是未知的。与单应矩阵的求解类似，我们定义增广矩阵 $[\mathbf{R}|\mathbf{t}]$ 为一个 $3\times 4$ 的矩阵，包含了旋转与平移信息
    
   
2. 定义 $\mathbf{T}$ 的行向量：
   
   假设一共有 $N$ 个特征点，可以列出线性方程组：
    
   
   由于 $\mathbf{t}$ 一共有 12 维，因此最少通过六对匹配点，即可实现矩阵 $\mathbf{T}$ 的线性求解，这种方法（也）称为直接线性变换。当匹配点大于六对时，可以使用 SVD 等方法对超定方程求最小二乘解。

7.7.2 P3P

1. P3P仅使用三对匹配点，对数据要求较少，它的输入数据为三对 3D-2D 匹配点，此外， P3P 还需要使用一对验证点，以从可能的解出选出正确的那一个（类似于对极几何情形）。记验证点对为 $D-d$ ，相机光心为 $O$ 。
   
2. 根据余弦定理：
   
   对上面三式全体除以 $O{C}^2$，并且记 $x=OA/OC,y=OB/OC$ ，得
   
   记 $v=A{B}^2/O{C}^2,uv=B{C}^2/O{C}^2;wv=A{C}^2/O{C}^2$ ，有：
   
   该方程组是关于 $x,y$ 的一个二元二次方程
3. P3P最后把问题转化为一个3D-3D的问题，然而， P3P 也存在着一些问题：
   1. P3P 只利用三个点的信息。当给定的配对点多于 3 组时，难以利用更多的信息
   2. 如果 3D 点或 2D 点受噪声影响，或者存在误匹配，则算法失效

7.7.3 Bundle Adjustment