【总结】视觉SLAM笔记整理

说明

这篇博客是我对《视觉SLAM十四讲》相关基础知识点的一个整理，没有详细的推导过程，仅仅相当于一个思维导图，同时在网上搜罗了一些相关的问题进行的补充总结，本人水平有限，如果文中有误，还请大家指出。

基础知识点

1. 特征提取、特征匹配

这是整个SLAM系统最开始的部分，先进行特征提取，然后进行特征匹配，通过匹配的特征点才求取的相关变换矩阵，这里容易搞混的概念是特征提取，特征提取是包括特征点和特征描述子，以ORB为例，ORB是由FAST特征点和BRIEF特征描述子构成。而我们通常所说的Harris角点通常仅仅指特征点，仅仅拥有Harris角点是无法进行特征匹配的，还需要通过向量对Harris角点进行特征描述（特征描述子），两帧之间才能进行特征的匹配。

（1）Harris

Harris角点：如下图所示，通过一个小的滑动窗口在邻域检测角点在任意方向上移动窗口，若窗口内的灰度值都有剧烈的变化，则窗口的中心就是角点。转化为数学描述就是自相关矩阵两个特征值大小。

Harris特征描述子：Harris 角点的描述子通常是由周围图像像素块的灰度值，以及用于比较的归一化互相关矩阵构成的。图像的像素块由以该像素点为中心的周围矩形部分图像构成
优点：计算简单；提取的点特征均匀且合理；稳定，稳定Harris算子对图像旋转、亮度变化、噪声影响和视点变换不敏感。
缺点：对尺度很敏感，不具有尺度不变性；提取的角点精度是像素级的；需要设计角点匹配算法

（2）SIFT

SIFT特征点：利用高斯金字塔和DOG函数进行特征点提取。高斯金字塔的当前层图像是对其前一层图像先进行高斯低通滤波，然后做隔行和隔列的降采样(去除偶数行与偶数列)而生成的。DoG (Difference of Gaussian)是高斯函数的差分，具体到图像处理来讲，就是将同一幅图像经过两个不同高斯滤波得到两幅滤波图像，将这两幅图像相减，得到DoG图。DOG图上的邻域梯度方向直方图峰值即特征点的主方向。

SIFT特征描述子：以特征点为中心取窗口，通过高斯加权增强特征点附近像素梯度方向信息的贡献，即在4 × 4的小块上计算梯度方向直方图（ 取8个方向），计算梯度方向累加值，形成种子点，构成4× 4 × 8= 128维特征向量。然后进行统计。

优点：SIFT特征对旋转、尺度缩放、亮度变化保持不变性，对视角变化、仿射变换、噪声也保持一定程度的稳定性。

（3）SUFT

SUFT是对SIFT的改进，他们在思路上是一致的，只是采用的方法不同而已
SUFT特征点：基于Hessian矩阵构造金字塔尺度空间，利用箱式滤波器（box filter）简化二维高斯滤波
SUFT特征描述子：通过Haar小波特征设定特征点主方向，这样构建的特征描述子就是64维的

缺点：在求主方向阶段太过于依赖局部区域像素的梯度方向；图像金字塔的层取得不足够紧密也会使得尺度有误差

（4）ORB

Fast角点：如果某个像素与周围领域内足够多的像素相差较大，则该像素有可能为角点。直接的阈值判断来加速角点提取简单高效。

BRIEF特征描述子：BRIEF算法计算出来的是一个二进制串的特征描述符。它是在一个特征点的邻域内，选择n对像素点pi、qi（i=1,2,…,n）。然后比较每个点对的灰度值的大小。如果I(pi)> I(qi)，则生成二进制串中的1，否则为0。所有的点对都进行比较，则生成长度为n的二进制串。
优点：ORB算法的速度大约是SIFT的100倍，是SURF的10倍。
以上参考 https://zhuanlan.zhihu.com/p/36382429

（5）特征匹配

特征匹配的方法有很多，就不在这里一一赘述，包括暴力匹配（Brute-Force Macher）、近似最近邻（FLANN）等，ORB SLAM2中采用词典加速匹配过程。

2. 2D-2D：对极约束、基础矩阵、本质矩阵、单应矩阵

对极约束 $\bm{x_2^T t^\wedge R x_1 = 0( p_2^T K^{-T} t^\wedge R K^{-1} p_1 = 0)}$
本质矩阵E $\bm{E=t^\wedge R}$
基本矩阵F $\bm{F=K^{-T}EK^{-1}}$
其中， $p_1,p_2$是像素坐标，对极约束描述的是空间中两个匹配点的空间位置关系，本质矩阵的奇异值必定是 $[\delta,\delta,0]$,由于平移和旋转各三个自由度，因此本质矩阵有六个自由度。但通常采用八点法进行求解。由本质矩阵恢复 $R,t$的过程通过SVD分解完成。
当场景中所有特征点都落到一个平面上时就可以通过单应性来进行运动估计。通过这个关系可以推导得 $\bm{p_2=Hp_1}$,其中 $H$就是单应矩阵。可以通过四对点求解。单应性的重要性在于，当相机发生纯旋转或者特征点共面时，基础矩阵的自由度会下降，就出现退化，这时候如果我们继续用八点法求基础矩阵，基础矩阵多余出来的自由度将主要由噪声决定。

3. 3D-2D:PnP

PnP是求解3D到2D点对运动的方法，即问题描述为，我们的已知条件是n个3D空间点以及它们作为特征点的位置（以归一化平面齐次坐标表示），我们求解的是相机的位姿 $R,t$，如果3D空间点的位置是世界坐标系的位置，那么这个 $R,t$也是世界坐标系下的。特征点的3D位置可以通过三角化或者RGBD相机的深度图确定。

（1）直接线性变换方法

根据推导，一对特征点（一个3D点加一个2D点）可以提供两个线性约束，因此12维的齐次变换矩阵需要6对特征点。

（2）P3P方法

P3P的作用是将利用三对特征点，讲空间点在世界坐标系下的坐标，转换到像极坐标系中的坐标，将PnP问题转化为ICP问题，推导过程是利用三角形特征完成的。

（3）Bundle Adjustment方法

其本质是一个最小化重投影误差的问题，公式如下：
即理解为调整相机的位姿使得重投影误差变小，而最小的重投影误差就对应着实际的位姿。需要求这里要注意的是这里仅仅是采用了BA的方法，但是实际做BA优化的时候，是同时优化位姿和路点位置，因此有两个相关的雅克比矩阵，但这里仅仅优化位姿，因此所求的雅克比矩阵仅仅和位姿有关，可以直接求取 $J$如下，然后就可以进行求解 $\delta \xi$进行迭代

4. 3D-3D:ICP

其本质就是确定两个点集之间的匹配关系。

（1）SVD方法

其问题构建为：

求解过程是先对每个点进行去质心坐标，然后根据优化问题计算旋转矩阵 $R$（这里会用到SVD），最后求 $t$

（2）非线性优化方法

其问题构建为：

这里的推导和PnP的类似，即求位姿导数。

5. 直接法和光流法

（1）光流法

光流法的基本假设是灰度不变假设，即同一个空间点的像素灰度值，在各个图像中是固定不变的

在LK光流中假设某一窗口内的像素具有相同的运动，因此 $w×w$大小的窗口内有 $w^2$个像素，即构成 $w^2$个方程，然后构成关于 $\frac{dx}{dt}$, $\frac{dy}{dt}$的超定线性方程，求其最小二乘解。LK光流是得到特征点之间的对应关系，如同描述子的匹配，之后还是需要通过对极几何、PnP等求解相机位姿。

（2）直接法

直接法之所以称为直接法是因为它是直接获得相机位姿，而不需要通过匹配、求解矩阵等过程，直接法的思路是根据当前相机的位姿估计值,来寻找 $p_2$ 的位置。但若相机位姿不够好, $p_2$的外观和 $p_1$会有明显差别, 然后通过优化光度误差来优化相机位姿。

非常快的框架SVO就是结合了直接法和特征点法，SVO采用的是提取稀疏特征点（类似特征点法），帧间VO用图像对齐（类似于直接法）。
直接法的缺点：1. 非凸性；2. 单个像素没有区分度；3. 灰度值不变是很强的假设

6. Bundle Adjustment

首先注意目标函数如下：

其中 $F_{ij}$为整个代价函数在当前状态下对相机姿态的偏导数， $E_{ij}$表示该函数对路标点位置的偏导数。因此在非线性优化过程中获得的 $H$矩阵为

H矩阵最重要的一个特性就是它的稀疏性，其中左上角和右下角为对角阵且一般左上角较小右下角较大。左下角和右上角可能稀疏也可能稠密，矩阵的非对角线上的非零矩阵块,表示了该处对应的两个相机变量之间存在着共同观测的路标点,有时候称为共视(Co-visibility)。其对应关系如下：

其求解 $\bm{H\Delta x=g}$的过程中消元的过程即Marginalization（边缘化），首先消元结果如下：

先求解

将解得的 $\Delta \bm x_c$带入原方程，再求解 $\Delta \bm x_p$，其优势在于：

1. 在消元过程中,由于 $\bm C$为对角块,所以 $\bm {C^{-1}}$容易解得。
2. 求解了 $\bm{\Delta x_c}$ 之后,路标部分的增量方程由 $\bm{\Delta x_p = C^{-1}(w-E^T\Delta x_c)}$给出。这依然用到了 $\bm{C^{-1}}$易于求解的特性。

从概率角度来看,我们称这一步为边缘化，是因为我们实际上把求 ( $\Delta \bm x_c$ , $\Delta \bm x_p$) 的问题,转化成先求 $\Delta \bm x_c$ ,再求 $\Delta \bm x_p$的过程。这一步相当于做了条件概率展开:
所谓鲁棒核函数就是减小误匹配带来的误差，如Huber核，其实就是改变一下目标函数的定义:

当误差 $e$大于某个阈值 $\delta$后,函数增长由二次形式变成了一次形式,相当于限制了梯度的最大值.

回环检测

这一部分之前并没有花很多时间去研究，主要是知道目前SLAM中用的比较多的方法是词袋模型，词袋模型中涉及到字典的生成和使用的问题，这一部分和机器学习的只是挂钩比较深。
字典的生成问题就是非监督聚类问题，可以采用K-means对特征点进行聚类，然后通过K叉树进行表达，相似度判断采用是TD-IDF的方法。

相关问题

问题及部分回答来源：
https://www.cnblogs.com/xtl9/p/8053331.html
https://zhuanlan.zhihu.com/p/46694678
http://www.voidcn.com/article/p-ngqfdzqe-ot.html
https://zhuanlan.zhihu.com/p/28565563

1. SIFT和SUFT的区别

1. 构建图像金字塔，SIFT特征利用不同尺寸的图像与高斯差分滤波器卷积；SURF特征利用原图片与不同尺寸的方框滤波器卷积。
2. 特征描述子，SIFT特征有4×4×8=128维描述子，SURF特征有4×4×4=64维描述子
3. 特征点检测方法，SIFT特征先进行非极大抑制，再去除低对比度的点，再通过Hessian矩阵去除边缘响应过大的点；SURF特征先利用Hessian矩阵确定候选点，然后进行非极大抑制
4. 特征点主方向，SIFT特征在正方形区域内统计梯度幅值的直方图，直方图最大值对应主方向，可以有多个主方向；SURF特征在圆形区域内计算各个扇形范围内x、y方向的haar小波响应，模最大的扇形方向作为主方向

2. 相似变换、仿射变换、射影变换的区别

等距变换：相当于是平移变换（t）和旋转变换（R）的复合，等距变换前后长度，面积，线线之间的角度都不变。自由度为6（3+3）
相似变换：等距变换和均匀缩放（S）的一个复合，类似相似三角形，体积比不变。自由度为7（6+1）
仿射变换：一个平移变换（t）和一个非均匀变换（A）的复合，A是可逆矩阵，并不要求是正交矩阵，仿射变换的不变量是:平行线，平行线的长度的比例，面积的比例。自由度为12（9+3）
隐射变换：当图像中的点的齐次坐标的一般非奇异线性变换，射影变换就是把理想点（平行直线在无穷远处相交）变换到图像上，射影变换的不变量是:重合关系、长度的交比。自由度为15（16-1）
参考：https://blog.csdn.net/try_again_later/article/details/81281688

3. Homography、Essential和Fundamental Matrix的区别

Homography Matrix可以将一个二维射影空间的点变换该另一个二维射影空间的点，如下图所示，在不加任何限制的情况下，仅仅考虑二维射影空间中的变换，一个单应矩阵 $H$可由9个参数确定，减去scale的一个自由度，自由度为8。
Fundamental Matrix对两幅图像中任何一对对应点 $\bm x$和 $\bm x'$基础矩阵 $\bm F$都满足条件： $\bm{x^T F x' = 0}$，基础矩阵是反对称矩阵，秩只有2，因此F的自由度为7。它自由度比本质矩阵多的原因是多了两个内参矩阵。
Essential matrix：本质矩是归一化图像坐标下的基本矩阵的特殊形式，其参数由运动的位姿决定，与相机内参无关，其自由度为6，考虑scale的话自由度为5。

4. 视差与深度的关系

在相机完成校正后，则有 $d/b=f/z$,其中 $d$ 表示视差， $b$ 表示基线， $f$ 是焦距， $z$ 是深度

5. 描述PnP算法

已知空间点世界坐标系坐标和其像素投影，公式如下

目前一共有两种解法，直接线性变换方法（一对点能够构造两个线性约束，因此12个自由度一共需要6对匹配点），另外一种就是非线性优化的方法，假设空间坐标点准确，根据最小重投影误差优化相机位姿。
目前有两个主要场景场景，其一是求解相机相对于某2维图像/3维物体的位姿；其二就是SLAM算法中估计相机位姿时通常需要PnP给出相机初始位姿。
在场景1中，我们通常输入的是物体在世界坐标系下的3D点以及这些3D点在图像上投影的2D点，因此求得的是相机坐标系相对于世界坐标系(Twc)的位姿
在场景2中，通常输入的是上一帧中的3D点（在上一帧的相机坐标系下表示的点）和这些3D点在当前帧中的投影得到的2D点，所以它求得的是当前帧相对于上一帧的位姿变换

6. 闭环检测常用方法

本人知道的现在常用的就是利用词袋模型进行闭环检测，也有利用深度学习进行闭环检测的方法，暂时没有去了解过

7. 给一个二值图，求最大连通域

这个之后单独写一篇博客来研究这个好了，二值图的连通域应该是用基于图论的深度优先或者广度优先的方法，后来还接触过基于图的分割方法，采用的是并查集的数据结构，之后再作细致对比研究。

8. 梯度下降法、牛顿法、高斯-牛顿法的区别

在BA优化、PnP、直接法里面都有接触到非线性优化问题，上面几种方法都是针对对非线性优化问题提出的方法，将非线性最优化问题作如下展开，就可以获得梯度下降法和牛顿法

梯度下降法是一个一阶最优化算法，通常也称为最速下降法。 要使用梯度下降法找到一个函数的局部极小值，必须向函数上当前点对应梯度（或者是近似梯度）的反方向的规定步长距离点进行迭代搜索。因此指保留一阶梯度信息。缺点是过于贪心，容易走出锯齿路线。
牛顿法是一个二阶最优化算法，基本思想是利用迭代点处的一阶导数(梯度)和二阶导数(Hessen矩阵)对目标函数进行二次函数近似。因此保留二阶梯度信息。缺点是需要计算 $\bm H$矩阵，计算量太大。
$\Delta \bm x^* = -\bm {J^T}(\bm x)/\bm H$
而把非线性问题，先进行一阶展开，然后再作平方处理就可以得到高斯-牛顿法和列文博格方法

高斯-牛顿法对上式展开并对 $\Delta \bm x$进行求导即可得高斯牛顿方程，其实其就是使用 $\bm {JJ^T}$对牛顿法的 $\bm H$矩阵进行替换，但是 $\bm {JJ^T}$有可能为奇异矩阵或变态， $\Delta \bm x$也会造成结果不稳定，因此稳定性差

列文博格法就是在高斯-牛顿法的基础上对 $\Delta \bm x$添加一个信赖区域，保证其只在展开点附近有效，即其优化问题变为带有不等式约束的优化问题，利用Lagrange乘子求解

9. 推导一下卡尔曼滤波、描述下例子滤波

这方面很难简单回答、可以参考一下博客
http://blog.csdn.net/heyijia0327

10. 如何求解 $Ax=b$的问题

11. 什么是极限约束

所谓极线约束就是说同一个点在两幅图像上的映射，已知左图映射点p1，那么右图映射点p2一定在相对于p1的极线上，这样可以减少待匹配的点数量。如下图：

12. 单目视觉SLAM中尺寸漂移是怎么产生的

用单目估计出来的位移，与真实世界相差一个比例，叫做尺度。这个比例在单目初始化时通过三角化确定，但单纯靠视觉无法确定这个比例到底有多大。由于SLAM过程中噪声的影响，这个比例还不是固定不变的。修正方式是通过回环检测。

10. 解释SLAM中的绑架问题

绑架问题就是重定位，是指机器人在缺少之前位置信息的情况下，如何去确定当前位姿。例如当机器人被安置在一个已经构建好地图的环境中，但是并不知道它在地图中的相对位置，或者在移动过程中，由于传感器的暂时性功能故障或相机的快速移动，都导致机器人先前的位置信息的丢失，在这种情况下如何重新确定自己的位置。
初始化绑架可以阐述为一种通常状况初始化问题，可使用蒙特卡洛估计器，即粒子滤波方法，重新分散粒子到三维位形空间里面，被里程信息和随机扰动不断更新，初始化粒子聚集到/收敛到可解释观察结果的区域。追踪丢失状态绑架，即在绑架发生之前，系统已经保存当前状态，则可以使用除视觉传感器之外的其他的传感器作为候补测量设备。

11. 描述特征点法和直接法的优缺点

特征点法
优点：1. 没有直接法的强假设，更加精确；2. 相较与直接法，可以在更快的运动下工作，鲁棒性好
缺点：1. 特征提取和特征匹配过程耗时长；2. 特征点少的场景中无法使用；3.只能构建稀疏地图
直接法：
优点：1.省去了特征提取和特征匹配的时间，速度较快；2. 可以用在特征缺失的场合；3. 可以构建半稠密/稠密地图
缺点：1. 易受光照和模糊影响；2.运动必须慢；3.非凸性，易陷入局部极小解

12. EKF和BA的区别

（1） EKF假设了马尔科夫性，认为k时刻的状态只与k-1时刻有关。BA使用所有的历史数据，做全体的SLAM
（2） EKF做了线性化处理，在工作点处用一阶泰勒展开式近似整个函数，但在工作点较远处不一定成立。BA每迭代一次，状态估计发生改变，我们会重新对新的估计点做泰勒展开，可以把EKF看做只有一次迭代的BA

13. 边缘检测算子有哪些？

边缘检测一般分为三步，分别是滤波、增强、检测。基本原理都是用高斯滤波器进行去噪，之后在用卷积内核寻找像素梯度。常用有三种算法：canny算子，sobel算子，laplacian算子
canny算子：一种完善的边缘检测算法，抗噪能力强，用高斯滤波平滑图像，用一阶偏导的有限差分计算梯度的幅值和方向，对梯度幅值进行非极大值抑制，采用双阈值检测和连接边缘。
sobel算子：一阶导数算子，引入局部平均运算，对噪声具有平滑作用，抗噪声能力强，计算量较大，但定位精度不高，得到的边缘比较粗，适用于精度要求不高的场合。
laplacian算子：二阶微分算子，具有旋转不变性，容易受噪声影响，不能检测边缘的方向，一般不直接用于检测边缘，而是判断明暗变化。

14. 简单实现cv::Mat()

15. 10个相机同时看到100个路标点，问BA优化的雅克比矩阵多少维

因为误差对相机姿态的偏导数的维度是2×6,对路标点的偏导数是2×3，又10个相机可以同时看到100个路标点，所以一共有10×100×2行，100×3+10×6个块。