【视觉SLAM入门】5.1. 特征提取和匹配--FAST,ORB(关键点描述子)，2D-2D对极几何，本质矩阵，单应矩阵，三角测量，三角化矛盾

为什么重要？我们是在做什么事？

• 特征提取和匹配： 首先是两幅图像的特征提取，然后是对应特征点的匹配。接下来的工作是根据得到的匹配点对，估计相机的运动，具体根据相机分为三种方法：
• • 单目相机：2D-2D： 对极几何 方法
• • 双目或者RGBD相机： 3D-3D： ICP 方法
• • 一个3D点和它相机中的投影位置： 3D-2D ： PnP 方法

0. 基础知识

视觉SLAM两阶段：

• 前端(VO) —> 粗略相机运动 ------> 提供给后端初始值
• 后端 —> 优化

VO的实现方法两派：

• 不提取特征点 ----> 直接法
• 提取特征点 ------> 特征点法 ----> 成熟

1. 特征提取和匹配

注意：有些东西的作用你要明白：

• 关键点： 是在一幅图像中找到的点，作用是在一幅图中找到路标点(有代表性的点)。
• 描述子： 在两个图像的关键点找到的情况下，匹配两个图像中的对应关键点。 通常是向量
• 特征点： 由关键点和描述子两部分组成，任务是(提取XXX关键点，计算XXX描述子)
• 尺度不变性： 为了确保从远到近都能检测出来关键点
• 旋转不变性： 为了确保图像旋转后还能检测出来关键点
• 特征提取的是关键点和描述子，特征匹配是根据描述子匹配的

---

几种图像特征：

• SIFT特征：计算量太大，有些精确
• FAST关键点：没有描述子，最快，不准。
• ORB特征：改进FAST关键点，采用BRIEF描述子

1.1 FAST关键点

• 1. 比较周围半径圆范围内的灰度情况，差别大就是角点。
• 2. 设定一个数量，比如9，范围内至少有连续9个点和选定点的亮度色差大于阈值T的时候，该点就称为特征点。这种方法叫FAST-9。
• 3. 检测完角点扎堆,非极大值抑制

1.2 ORB的关键点–改进FAST

改进了FAST关键点法，克服了缺点：

• 1. 可以指定提取数量： 对点分别计算Harris相应，取前N个响应最大的角点；
• 2. 尺度不变性： 用图像金字塔提取每一层的角点，均为角点才是角点；
• 3. 旋转不变性：灰度质心法，保证图像旋转后还能检测到。最后得到的是角度，从图像光度明指向光度暗的一侧，具体实现如下：

1.3 ORB的描述子–BRIEF

作用：为了保证两个图像中提取出的关键点能对应上各自匹配的点对。

• BRIEF是二进制描述子，描述向量由0和1组成
• 做法：选取关键点周围的图像块，随机选取像素点对(有很多选点方法)，如128就是取128个点对，设两个点像素分别为 $p,q$ , 然后计算 $p,q$ 的大小关系，按结果分别记为0，1， 最后得到128位的二进制数。匹配的时候在第二幅图像中也用相同的选点方法,最后比较两幅图像中关键点描述子距离(二进制的字串衡量就是汉明距离)。

1.4 总结

通过图像特征点的对应关系，解决了SLAM最重要的一步：同一个点在不同图像中如何检测出来。
特征匹配的方法有：

• 暴力匹配(Brute-Force Matcher): 第二幅图像中每个点都计算其在第一幅图对应的特征点，运算量大；
• 快速最近邻(FLANN)： 适用于匹配点数量多
• 。。。

---

2. 对极几何，对极约束

目的是求相机运动 $R,t$，内参一般知道

这是2D-2D的单目情形，假设相机经过一次运动 $R,t$ 后得到的两帧图如下：

其中点和线定义如下：

• $p_1,p_2$ ： 分别同一个点在两帧下的投影点
• $O_1,O_2$ ： 相机光心
• $P$ ： 真实世界中的点
• $I_1,I_2$ : 两帧图像
• $O_1{O}_2连线$ ： 基线
• $e_1,e_2$ : $O_1{O}_2$ 和 $I_1,I_2$ 的交点，也叫极点
• 极平面 ： $O_1,O_2,P$ 所在平面
• 极线 ： $l_1,l_2$ 。

如果没有深度信息，则$O_{1}P$ 直线上任一点投影都在 $p_1$，且他在第二帧图像上的轨迹在 极线 $p_2{e}_2$ 上，所以有真确的匹配，就可以推断 $P$ 的位置，然后得到相机的运动。

2.1 本质矩阵(对极约束)

推理部分略，详见《视觉SLAM十四讲》第七章7.3节，这里给出结果。
仍参考上图，取两个像素点归一化平面上的点 $x_1,x_2$：
$$x_1=K^{-1}{p}_1,x_2=K^{-1}{p}_2$$
则 最终的对极约束 为：
$p_2^T{K}^{-T}t$^$R{K}^{-1}{p}_1=0$

它的含义是$O_1，O_2,P$ 三点共面。从式中心部分，记本质矩阵 $\mathbf{E}$ 和基础矩阵 $\mathbf{F}$ 如下:

$E=t$^$RF=K^{-T}E{K}^{-1}{x}_2^{T}E{x}_1=p_2^{T}F{p}_1=0$

可以看出 $\mathbf{E}$ 和 $\mathbf{F}$ 只差内参 $K$ (已知)，所以二者 求一即可。
不妨以$E=t$^$R$来求解。则后续工作如下：

1. 根据已匹配点对，求出$\mathbf{E}$或$\mathbf{F}$
2. 根据$\mathbf{E}$或$\mathbf{F}$,求出相机运动 $\mathbf{R},\mathbf{t}$

2.1.1 求解本质矩阵

探究本质矩阵的特点：

• 1. 由对极约束 $x_2^{T}E{x}_1=0$ , 所以它在不同尺度下等价，左右乘依旧满足约束。又因为$E=t$^$R$, 原本有6个自由度，故去掉尺度，还有5个自由度
• 2. $E的内在性质$ ： 它的奇异值必定是 $[\delta ，\delta ，0{]}^T$ 的形式，非线性的性质。

求解依据：
$$x_2^{T}E{x}_1=0(1)$$
理论上可以用5对点来求解，但是很麻烦。故用 八点法 求解(由于尺度不变性)。

1.首先考虑一对点(归一化坐标 $x_1,x_2$ )： 将(1)式展开:
$$\begin{gathered} (u_1,v_1,1)\left(\begin{array}{c}{e}_1{e}_2{e}_3\\ e_4{e}_5{e}_6\\ e_7{e}_8{e}_9\end{array}\right)\left(\begin{array}{c}{u}_2\\ v_2\\ 1\end{array}\right)=0 \\ \Downarrow 将e展开 \\ e=[e_1,e_2,e_3,e_4,e_5,e_6,e_7,e_8,e_9] \\ \Downarrow 展开并重写 \\ [u_1{u}_2,u_1{v}_2,u_1,v_1{u}_2,v_1{v}_2,v_1,u_2,v_2,1]\cdot e=0 \\ \Downarrow 考虑8对点的方程组 \end{gathered}$$

至此，本质矩阵的求解结束

2.1.2 恢复相机运动$R，t$

1. 对 $\mathbf{E}$ 做SVD分解
   $$E=U\sum V_T(U,V正交阵，\sum 为奇异矩阵且=diag(\delta ,\delta ,0))$$
2. 求解较为复杂，这里给出结果
   一共存在4组解。如下：
   
   蓝色横线就是相机平面，红色点为投影点。

有(1)满足要求，因为只有这样才符合投影模型，深度才为正。将解出来的解带入验算即可。

---

2.1.3 本质矩阵调整

5个自由度，用了8个点，上边的方程求解出的 $E$ 可能不满足 $E$ 的内在性质( $\sum =\mathbf{diag}(\mathbf{\delta },\mathbf{\delta },0)$ )，因此要调整。做法如下：
在做SVD分解时，得到
$$\begin{gathered} \sum =diag({\delta }_1,{\delta }_2,{\delta }_3) \\ \Downarrow 设{\delta }_1\ge {\delta }_2\ge {\delta }_3，则新的\sum 如下 \\ \sum ^{\prime }=diag(\frac{{\delta }_1+{\delta }_2}{2},\frac{{\delta }_1+{\delta }_2}{2},0) \\ \Downarrow 带入SVD分解式 \\ E=Udiag(\frac{{\delta }_1+{\delta }_2}{2},\frac{{\delta }_1+{\delta }_2}{2},0)V^T \end{gathered}$$

相当于把求出来的矩阵投影到了 $\mathbf{E}$ 的流形上，也可以直接取$\sum =(1,1,0)$ (尺度不变性)

2.1.3 遗留问题

1. $\mathbf{E}$ 的尺度不确定性导致了 $\mathbf{t}$ 的尺度不确定性。(由于 $\mathbf{R}$) 自身带有约束。因此单目SLAM存在初始化： 以 $\mathbf{t}$ 的单位为固定尺度1的计算相机运动和特征点；
2. 单目初始化不能只有纯旋转，必须要有一定的平移： 因为 $\mathbf{t}$ 为0，所以 $\mathbf{E}$ 最终也为0;
3. 当点多于8对，此时构成超定方程，我们有两种做法：

• 3.1. 最小化一个二次型(最小二乘意义下的)
• 3.2. 随机采样一致性(RANSAC)，可以处理有错误匹配的情况，一般用这个。

2.2 单应矩阵（特别提一下）

为什么需要单应矩阵 $H$ (Homography)？

• 当特征点共面，相机纯旋转，$\mathbf{F}$ 的自由度少了 $t$ ，下降。这就是退化现象。
• 如果这时仍用八点法求解，多出来的自由度是噪声带来的。
• 为了避免退化，同时估计基础矩阵 $\mathbf{F}$ 和 单应矩阵 $\mathbf{H}$，选择重投影误差小的矩阵作为最终运动估计矩阵。

故 $H$ 假设的所有特征点位于平面上。

详细推导内容见SLAM十四讲7.3.3 。根据法平面做的，求解与 $E和F$ 相似。只需要4对匹配点就可以算出。

2.3 三角测量(Triangulation)—深度信息

• 计算深度：回想相机模型那一节，这里的深度就是之前被我们固定为1的 $\mathbf{s}$ 。以第一帧图像为坐标原点，由以上对极约束内容有：
  $$\begin{gathered} s_1{x}_1=s_{2}R{x}_2+t \\ \Downarrow 分别单独计算，左乘x_1的反对称矩阵 \end{gathered}$$
  $s_1(x_1$^$)x_1=0=s_2(x_1$ ^$)R{x}_2+(x_1$ ^$)t$

• 可以直接求得深度 ${\mathbf{s}}_1,{\mathbf{s}}_2$ 。但是由于噪声的存在，我们一般是求最小二乘解，而不是零解。同样由于尺度不确定性，我们只知道深度对于t的数量，而不知道具体究竟是多少米。

• 三角化矛盾： 平移越大三角化越精确，但是视野越短，反之亦然。