《视觉SLAM十四讲 第二版》笔记及课后习题（第七讲）
读书笔记：视觉里程计1
之前的内容，介绍了运动方程和观测方程的具体形式，并讲解了以非线性优化为主的求解方法。从本讲开始，我们结束了基础知识的铺垫，开始步入正题：按照第二讲的内容，分别介绍视觉里程计、优化后端、回环检测和地图构建四个模块。本讲和下一讲主要介绍作为视觉里程计的主要理论，然后在第九章中进行一次实践。本讲关注基于特征点方式的视觉里程计算法。我们将介绍什么是特征点，如何提取和匹配特征点，以及如何根据配对的特征点估计相机运动。
特征点法
- 经典SLAM模型中以位姿——路标（Landmark）来描述SLAM过程
- 路标是三维空间中固定不变的点，能够在特定位姿下观测到
- 数量充足，以实现良好的定位
- 较好的区分性，以实现数据关联
- 在视觉SLAM中，可利用图像特征点作为SLAM中的路标
2D-2D: 对极几何
特征匹配之后，得到了特征点之间的对应关系：
- 如果只有两个单目图像，得到2D-2D间的关系 ——对极几何
- 如果匹配的是帧和地图，得到3D-2D间的关系 ——PnP
- 如果匹配的是RGB-D图，得到3D-3D间的关系 ——ICP
八点法的讨论： - 尺度不确定性：归一化 t 或特征点的平均深度
- 纯旋转问题：t=0 时无法求解
- 多于八对点时：最小二乘
- 有外点时：RANSAC
小结：
- 2D-2D情况下，只知道图像坐标之间的对应关系
- 当特征点在平面上时（例如俯视或仰视），使用H恢复R,t
- 否则，使用E或F恢复R,t
- t 没有尺度
- 求得R,t后：
- 利用三角化计算特征点的3D位置（即深度）
- 实际中用于单目SLAM的初始化部分
3D-2D PnP
3D-2D ICP
三角化与深度估计
小结：
本节介绍了基于特征点的视觉里程计中的几个重要的问题。包括：
- 特征点是如何提取并匹配的；
- 如何通过2D-2D 的特征点估计相机运动；
- 如何从2D-2D 的匹配估计一个点的空间位置；
- 3D-2D 的PnP 问题，它的线性解法和Bundle Adjustment 解法；
- 3D-3D 的ICP 问题，其线性解法和Bundle Adjustment 解法。
课后习题
1. 除了本书介绍的ORB 特征点外，你还能找到哪些其他的特征点？请说说SIFT 或SURF 的原理，对比它们与ORB 之间的优劣。
除了ORB之外，一般还有还有SIFT、SURF、BRISK、AKAZE，这些都是在OpenCV中已经实现了的。
SIFT算法，又称为尺度不变特征转换(Scale-invariant feature transform），大致方法是首先搜索所有尺度下图像的位置，通过高斯微分函数来识别潜在的对于尺度和旋转不变的兴趣点，然后在候选位置通过拟合精细的模型来确定位置和尺度，再基于图像梯度，分配给关键点一个或多个方向，最后在每个关键点的周围邻域，在选定的尺度下测量图像局部梯度来描述关键点。使用斑点检测方法和浮点型特征描述子，在建立高斯差分空间金字塔的基础上提取初具有尺度不变性的特征点，然后对特征点邻域内的点的梯度方向进行直方图统计。特征点的主方向就是直方图中比重最大的方向，必要时可选一个辅方向。SIFT特征集旋转不变性，尺度不变性，对图像变形和光照鲁棒鲁等优点于一身，不足之处是计算量大，计算速度慢，需要GPU加速才可满足实时性需求。
SURF算法，又称为加速稳健特征（Speeded Up Robust Features)，其方法和构建图像金字塔的方法相反，其主要方法是通过修改滤波器的尺寸和模糊度从而得到不同层级的影像，但是寻找特征点的方法和SIFT类似。使用基于DoH的斑点特征检测方法，在特征点的描述上，SURF算法通过积分图，利用两个方向上的Harr小波模型进行梯度计算，然后对邻域内点的梯度方向以扇形的方式进行统计，得到特征点的主方向。其算法速度快且稳定性好。
SIFT和SURF能够比较稳定地提供较高的准确率，其中SIFT比SURF跟准确一点，但是二者都特别慢。相较而言ORB速度更快，但是更容易出现问题，而且错误率远比其他二者大。
计算速度： ORB>>SURF>>SIFT（各差一个量级）
旋转鲁棒性： SURF>ORB~SIFT（表示差不多）
模糊鲁棒性： SURF>ORB~SIFT
尺度变换鲁棒性： SURF>SIFT>ORB（ORB并不具备尺度变换性）
综上所述，如果对计算实时性要求非常高，可选用ORB算法，但基本要保证正对拍摄；如果对实行性要求稍高，可以选择SURF；基本不用SIFT
2. 设计程序，调用OpenCV 中的其他种类特征点。统计在提取1000 个特征点时，在你的机器上所用的时间。
参考自己做的待填坑
3. * 我们发现OpenCV 提供的ORB 特征点，在图像当中分布不够均匀。你是否能够找到或提出让特征点分布更加均匀的方法？
用熵来度量局部信息量大小，将图像分为若干网格，计算每个网格图像的熵，在熵较大的区域内提取特征点。若局部依旧集中可再次划分网格，在没有特征的网格中，强行提取一个兴趣值最大的特征点。
2016年出的ORB_SLAM2中已经用四叉树实现的特征点均匀分布算法。四叉树均匀划分的方法跟经典ORB算法变化不大，一般还是：1.构建图像金字塔；2.将每层图像划分网格，FAST算法提取特征点；3.对网格内的特征点进行评估，选取质量最好的留下作为代表关键点；4.用强度质心法计算每个关键点的方向；5对金字塔图层进行高斯滤波；6.使用BRIEF计算各个关键点的描述子（包含论文中的启发式搜索算法得到的256对匹配对坐标进行优化）；7.保留关键点和描述子。
4. 研究FLANN 为何能够快速处理匹配问题。除了FLANN 之外，还能哪些可以加速匹配的手段？
FLANN的全称为Fast Library for Approximate Nearest Neighbors。它是一个对大数据集和高维特征进行最近邻近的算法的集合。在面对大数据集时它的性能要好于BFMatcher。匹配问题实际上就是一个特征向量求相似度问题。对于最简单的办法，就是逐个匹配对计算距离。明显这种遍历的方式是效率极低的，对于大数据情况下不适用。因此经典kd-tree的搜索回溯的搜索办法在这里派上了用场，减少了不必要的计算过程，提高了效率，但是对于多维数据而言，其效果往往不升反降。在经典kd-tree算法上提出了随机kd-tree算法，随即选出方差较大的维度，建立kd-tree，再进行搜索回溯。还有一种分级k-means tree，与之前不同的是先通过k-means算法（之后回环检测时会用到）来进行先数据聚类，然后再进行kd-tree的建立。这些方法相交于传统的直接匹配法优势都比较大。有关k-d tree
加速匹配的方法：预排序图像检索；GPU加速，可以使得匹配速度提高十多倍；再后来就是用FPGA加速，其匹配速度能提升10倍；再后来的VF-SIFT（very fast SIFT）算法，其核心思想是从SIFT特征中提取4个特征角，根据特征角区间的不同，避免了大量不必要的搜索，这样据说是普通搜索的1250倍。
5. 把演示程序使用的EPnP 改成其他PnP 方法，并研究它们的工作原理。
目前除了EPnP外，OpenCV还提供了另外两种方法：迭代法和P3P法，其中，在OpenCV3中还另外提供了DLS法和UPnP法。
EPnP(Efficient PnP)的思路是将空间中的任意3D点可以用4个不共面的3D点加权表示，然后通过n个3D点在相机平面的投影关系以及四个控制点的权重关系构建一个的矩阵，求这个矩阵的特征向量，就可以得到这个相机平面的坐标，再用POSIT正交投影变换算法得到相机位姿。迭代法实质是迭代求出重投影误差的最小解先用DLT直接线性变换求解，然后用LM算法进行优化，这个解显然不是正解，而且这个方法只能使用4个共面特征点才能求得。
P3P法，它需要3对3D-2D的匹配点，然后通过三角形投影关系得到的方程组，然后将方程组进行变换得到两个二元二次方程进行求解。
DLS（Direct Least-Squares）算法整体思路是首先对PnP非线性最小贰乘建模，重新定义LS模型以便于参数降维然后构造Maculy矩阵进行求解，将PnP问题重构为无约束LSM问题然后优化求解。UPnP(Uncalibrated PnP)算法跟EPnP差不了太多，仅仅是多估计了焦距。因此，比较适合未标定场合。
6. 在PnP 优化中，将第一个相机的观测也考虑进来，程序应如何书写？最后结果会有何变化？
7. 在ICP 程序中，将空间点也作为优化变量考虑进来，程序应如何书写？最后结果会有何变化？
8. *在特征点匹配过程中，不可避免地会遇到误匹配的情况。如果我们把错误匹配输入到PnP 或ICP 中，会发生怎样的情况？你能想到哪些避免误匹配的方法？
目前书中用的是根据汉明距离的暴力匹配方法，然后根据经验参数（30或者是最小距离的两倍）对匹配子根据其距离进行筛选。如果误匹配情况输入到PnP或是ICP中，再加上迭代算法选择不正确，初值估计不准确，就很容易导致计算结果产生误差，更有甚者会让迭代过程不稳定，甚至报错。目前比较流行的避免误匹配方法有：交叉匹配（在暴力匹配的基础上再匹配一次，如果两次结果一致，则认为是个特征点，如果不一致则滤掉，BFMatcher XX (NORM_HAMMING, true) ）、KNN匹配（K邻近匹配，匹配时候选择K个与特征点相似的点，一般K是2，如果区别足够大，则选择最相似的点作为匹配点，bfMatcher->knnMatch(descriptors1, descriptors2, knnMatches, 2) ）、RANSAC（随机采样一致性，利用两个图像之间的单应矩阵，根据重投影误差判定某个匹配是不是正确匹配，findHomography）等等，一般可以跟觉已有的成熟框架如ORB_SLAM2等等观察其对于不同场景所采取的避免误匹配的方法。同样，对于后端，在优化时可以用Huber损失函数等等增强优化算法的鲁棒性。
9. *使用Sophus 的SE3 类，自己设计g2o 的节点与边，实现PnP 和ICP 的优化。
也就是将g2o优化转换成Ceres优化。由于《视觉SLAM十四讲》一书中主要运用的是重投影误差优化法，主要针对的是BA优化问题，而g2o工具对这一类问题求解比较准确方便，以至于对Ceres这工具不太了解。相交于g2o仅适用于BA问题的求解，Ceres更加通用对于最小贰乘问题的计算。通过本题我们可以增加对Ceres优化方法的认识，当然更多的大家还是去看Ceres官方教材。