拆分基础Slam（1）--静态相机前端

前言

我在学习slam的过程中遇见了很多问题，边学习边记录，希望本系列能帮助更多同学学习理解slam基本理论。参考了高翔slam十四讲，但我读的时候很多地方三四遍都看不懂。

单目相机及标定

单目相机模型

Slam中前端的作用是粗略获取图像间的变换规则R（旋转矩阵），t（平移矩阵）。Slam中前端可以采用的传感器包括视觉、激光、超声等。其中视觉传感器就有单目、双目、RGB-D三种为主。本文主要介绍如何使用单目相机完成传统Orb-Slam。
由于单目相机存在深度丢失等问题，需要在代码中进行额外的处理。单目相机测距的核心原理就一点：对极约束。配合视差计算就可以得到深度距离。

两种主要方法

特征点匹配指的是使用多种特征点计算的方法比如：Orb、Sift、Surf等。在前后两张图中得到匹配点后可以采用基本的八点法或者五点法求解出本质矩阵，SVD分解后可以得到旋转矩阵R和平移矩阵t。

特征点匹配方法

ORB原理

本文主要讲解特征点匹配中的Orb方法。Orb首先采用Fast角点得到特征点，之后在各个特征点周围提取得到Brief描述子。Fast角点首先从图中遍历得到一个像素点P，给定一个阈值t，当两个点的灰度值之差绝对值大于t时，我们认为这两个点不相同。之后判断这个点周围16个点与阈值的关系，如果这16个点连续有n个点都和其他地方不相同，则判定为角点。如下图：

得到了角点信息后，需要对每个角点计算Brief描述子用以之后对特征匹配。Brief描述子是一种简单且速度快的描述子计算方法。其原理是首先以一定模式在特征点周围选择N个点对，将这些点对进行N（A,B）处理。如下图：

在这个图中N=3，但在实际计算中N可以取512。N（）的计算方法如下：

$$N(A,B)=\{\begin{array}{rlr}0& & A的灰度大于B的灰度\\ 1& & A的灰度小于B的灰度\end{array}$$
由此则可以生成一串二进制向量来描述这个特征点。
但是这么做仍然存在一个问题。因为Brief描述子在取点时用的是固定规则，如果此时图像的尺度和方位发生了变化，那么相同的角点得到的描述子就会不同。所以需要具备尺度不变性和旋转不变性。
对于尺度问题，图像处理中广泛使用的是图像金字塔，基本思路是用多个比例缩放一张原图然后在上面做处理。Opencv中也集成了相应的方法，对于旋转不变性可以参考Hu不变矩使用的方法，可以计算出这块圆形区域的质心来判断旋转关系。不变矩是衡量空间特性非常常用的方法，还可以计算出图像的倾斜角等信息。

特征点匹配

得到了Brief特征描述子后就是需要匹配不同图像中的像素点。对于两个不同的Brief描述子，通常使用的是汉明距离。汉明距离指的是值不同的位数。比如：10010和01010的汉明距离是2。
对于在原图中的一个特征点，我们需要在下一张图中找到与之匹配的特征点，最简单的算法是采用暴力匹配法，即分别跟下张图的所有特征点计算汉明距离，距离最短的就是匹配点，但是这样算法耗时。还有种方法采用的是快速近似最邻近，

光流法

虽然特征点法是目前的主流，但是仍然存在一些问题，比如特征点的计算和描述子的提取非常耗时，高级的特征点仍然无法做到实时性，还比如在图像纹理特征较低的情况下，难以提取跟踪特征点，比如白墙和走廊。
先介绍光流法。光流法常用在图像处理的跟踪问题上，其作用是检测一个点的移动。通俗来讲，光流就是像水流一样，我要看某些点怎么在图中流动的。光流法的数学原理比直观感受要抽象许多，我争取通俗地讲解一下LK光流法，首先，光流法的基础是灰度不变假设，就是在两帧图像中，同一个点的灰度值是不变的，不会因为曝光程度的变化而变化，也就是说t时刻位于*（x,y）*处的像素点在t+1时刻位于（x+dx,y+dy），用表示灰度则有下列公式：
$$I(x+dx,y+dy,t+dt)=I(x,y,t)$$
我们对上式进一步处理，用泰勒公式展开微分项，可得：
$$I(x+dx,y+dy,t+dt)=I(x,y,t)+\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt$$
基于灰度不变假设，那么就是说：
$$\frac{\partial I}{\partial x}dx+\frac{\partial I}{\partial y}dy+\frac{\partial I}{\partial t}dt=0$$
对上式两边除以dt：
$$\frac{\partial I}{\partial x}\frac{dx}{dt}+\frac{\partial I}{\partial y}\frac{dy}{dt}+\frac{\partial I}{\partial t}=0$$
上式经过整理后可以变为我们最终需要求解的方程形式：
$$\begin{gathered} [I_x{I}_y]\ast {\left[u \\ v\right]}^T=-I_t \end{gathered}$$

其中u,v就是我们要求解的特征点的位移，u为像素在x方向的移动，v为像素在y方向的移动，Ix指的是该点x方向的灰度梯度，Iy为y方向的灰度梯度，It为灰度对时间的变化量。t是离散单位，如果是相邻两帧的话我们认为t=1，则It就是（x,y），(x+dx,y+dy)两个像素点之间的灰度差。Ix和Iy就是两个像素点在x和y方向上的梯度。
我们要求解u和v两个变量，光有一组点还不够，我们还需要更多组数据。此时我们采用一个正方形块，由w*w个点构成，构成一个超定方程，用最小二乘法求解，这个方法之后在后端优化的时候也会用到。则光流法最后求解的方程是：
$$\begin{gathered} A\ast {\left[u \\ v\right]}^T=-b \end{gathered}$$
$$A=\left(\begin{array}{c}{\left[I_x,I_y\right]}_1\\ {\left[I_x,I_y\right]}_2\\ {\left[I_x,I_y\right]}_3\\ ...\end{array}\right)b=\left(\begin{array}{c}{I}_{t1}\\ I_{t2}\\ I_{t3}\\ ...\end{array}\right)$$

直接法

直接法也是一种非特征点方法，与光流法类似的地方是同样会用到最小二乘解最优解。光流法在进行位姿估计的时候还会通过获得匹配的特征点来进行位姿估计，但是与特征点匹配法不同的是没有通过提取点的特征来进行匹配（理解可以这么理解但事实上操作的时候不会用所有点来计算，还是会提取出部分特别的点来做计算）。直接法获得位姿估计点方法就不匹配特征点。而是直接用位姿估计来计算投影，使投影过去的点与原点最相似来优化姿态。
首先，在世界坐标中有一个点P，将这个点投影到第一帧图像上P₁，投影到第二帧图像上是 P₂，但是将P₁通过不准确的R、t计算后得到的点由于误差是P₁₁，这样就存在了差异。我们就需要得到更准确的位姿估计也就是R、t。

直接法的基本准则是灰度不变假设，目标是最小化光度误差，我们认为的p11就是真实的p2，需要建立关系优化p11和p2之间的误差，则对投影关系建立表达式：
$$e=I_1(p_1)-I_2(p_{11})$$
使用最小二乘优化使e减小，此时的优化结果就是R，t。

特征点法的位姿计算

上文已经介绍了相机前端三种基础的办法，其中需要提取特征点的是以ORB为例的特征点和描述子提取法和光流法。在这两种方法里都能得到在两张图中目标点对应关系。现在需要利用对应关系完成位姿的计算。
对极约束的表达式如下：
$$x_2^T\hat{t}R{x}_1=0$$
令基础矩阵E=tR，依据上述对角约束原理，用常见的八点法求解可以写出E的方程组如下：
$$（u_1,v_1,1）\left(\begin{array}{ccc}{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$$
有九个未知量，但由于尺度不确定性所以具有一个自由度，只剩下八个未知量，所以需要八对点建立方程。求解出E=tR之后就将E分解出t和R就得到了姿态的变化量具体使用SVD分解。SVD分解E矩阵的时候可以得到四种可能的解，我们只需要验证哪种情况下目标点的深度为正数即可。

总结

相机前端计算的目的是得到机器人的移动，所以前端也叫视觉里程计，对于提取特征点，具有更好的准确度，但是在低纹理的情况下难以提取，同时具有较大的运算量，更高级的Sift等特征点难以做到实时行。如果相机的环境下存在火焰烟雾等动态干扰，还需要引入动态Slam进行去噪。光流法和直接法可以应用在纹理内容较弱的环境下。
单目直接法代码已上传到GitHub单目直接法