ORBSLAM2源码阅读（2）——算法部分
算法
- 通用数据结构
- Tracking
- Orb特征提取
- oFAST
- rBRIEF
- ORBSLAM中的实际应用
- 从之前的关键帧中初始化位姿估计
- 从全局重定位中初始化位姿估计
- 跟踪本地地图
- 决定新的关键帧
- LOCAL MAPPING
- covisibility graph
- 系统组成
- 动态窗口（活动窗口）
- 相对表示
- 减少姿态姿态影响（Pose-pose reduction）
- 系统概述
- 总览
- SLAM图结构
- 优化与边缘化
- 参考地图点的选择与增加新的关键帧
- 回环关闭（回环检测?）
- 米制回环关闭（Metric Loop Closure）
- 大尺度回环关闭（Largescale Loop Closure）
- LOOP CLOSING
- 词袋模型
- 特征
- 图片数据库
- 回环检测算法
- 数据库查询
- 分组匹配
- 暂时一致性（Temporal consistency）
- 有效几何一致性（Efficient geometrical consistency）
- 基础图（Essential Graph）
- 回环纠正
本来的打算是直接进三个线程源码的，结果整理了一下发现如果直接进源码根本不知道他在干什么，而且虽然名字叫ORB-SLAM，但是内容却不仅仅是ORB，牵扯到论文很多也很细，内容也非常有意思。所以想着就先单独把算法部分提出来，统一梳理一边，也方便之后与源代码对比查看，还是准备分成三个线程来说，会附上对应论文的链接。
总体流程来自于ORB-SLAM: a Versatile and Accurate Monocular SLAM System以及ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras，上面的两个论文会分别用ORB与ORB2做简称。ORB以及ORB2中对原本算法改进的地方也会专门写出来~
那么让我们开始吧！！
通用数据结构
地图点存储：
- 世界坐标系下的3D坐标。
- 观察方向（viewing direction），所有能观测到该地图点的观测向量平均数的单位向量。
- 具有代表性的ORB描述子，与能观测到该地图点的所有关键帧中所有的相关描述字汉明距离最短的描述子。
- 能够观测到该点的最大以及最小距离。
关键帧储存：
- rigid body transformation（不知道怎么翻译出内味）将世界坐标系转移到相机坐标系。
- 相机内参。
- 所有该帧内提取的所有ORB特征。
Tracking
这部分在ORB中，，分为以下五步：
- ORB特征提取；
- 从之前的关键帧中初始化位姿估计；
- （如果2失败）从全局重定位中初始化位姿估计；
- 跟踪本地地图；
- 确定新的关键帧；
Orb特征提取
论文为ORB: An efficient alternative to SIFT or SURF.。ORB特征主要由带方向的关键点oFAST以及考虑到旋转的rBRIEF构成。
oFAST
FAST因为其易于计算而大受欢迎，但是它不包含方向信息。针对这个，论文作者进行了优化。
FAST关键点检测，我直接转载这个博客如下：
图中是计算半径为3的FAST关键点，计算方式是拿像素点与待选点（中心点）做亮度差：
1、计算1和9是否会大于阈值，如果大于阈值则可以进入下一个，否则就不是关键点。
2、计算1,9,5,13是不是大于阈值的个数超过3个，不是的话，就不是关键点。是的话进入下一步。
3、计算一个像素点，取周围距离为3的一个圆，然后这个像素点与周围的【1-16】的差值大于阈值的个数count，如果会大于阈值，则计算的count+1。如果count大于等于9，那么就作为候选项。
4、使用NMS，如果在3x3或5x5的邻域内，如果没有有比当前像素更大的score的点，那么就抑制这个点。
FAST对边线很敏感，所以采用Harris角点测量对所有的备选点经行排序，最后选取评分较高的N个。Harris角点本身的内容请参考这个博客。
FAST本身不具备尺寸不变性，所以需要在不同的尺度上分级寻找FAST关键点。
给FAST增加方向信息，定义，这里提出了一个叫做intensity centroid的东西。
具体计算方法就是：
首先是计算moment，就是在某个领域内计算m，计算公式如下图：
这里计算的就是强度中心，注意这里p, q = {0, 1}：
然后我们从中点O（也就是FAST的关键点）到C点就有了一个向量： 计算FAST的方向就可以使用下面的公式计算咯。在ORB特征里面，它使用的领域就是一个圆。
作者在这里提到为了找到方向，他特地使用MAX和BIN的方法计算方向，但是他发现还是center centroid的方法最好。
rBRIEF
BRIEF算法的流程如下：
- 为减少噪声干扰，先对图像进行高斯滤波（方差为2，高斯窗口为9x9）。
- 以特征点为中心，取SxS的邻域大窗口。在大窗口中随机选取一对（两个）5x5的子窗口，比较子窗口内的像素和（可用积分图像完成），进行二进制赋值。（一般S=31）
- 在大窗口中随机选取N对子窗口，重复步骤2的二进制赋值，形成一个二进制编码，这个编码就是对特征点的描述，即特征描述子。（一般N=256）
为了让BRIEF具有旋转不变性，这里使用了得当初寻找FAST的关键点的方向的作为BRIEF的方向，步骤如下： - 把关键点周围的要用来计算的坐标列成2xn的矩阵：
- 计算通过旋转矩阵之后这些点的坐标。旋转矩阵就是[cos a sin a； sin a -cos a]这个矩阵:
- 这里就是计算旋转之后的坐标中取出点来计算BRIEF描述子:
此时BRIEF描述子也被称为steered BRIEF。
ORBSLAM中的实际应用
以1.2为缩放因子分8层提取FAST关键点。为了保证分布较为平均，对每一个缩放层次，我们将图片分为一个个小格子，每个格子内希望至少找到5个关键点。在每个格子中寻找的同时调整阈值。在某些格子不包含角点时也要调整角点的总数。之后计算这些关键点的rBRIEF描述子。
从之前的关键帧中初始化位姿估计
这个在原文中并没有对应的论文。这里只是假定仍按照上一帧的运动。如果没找到足够的匹配，则在上一帧中位置附近寻找地图点尝试匹配，看看是否能匹配上。如果可以匹配成功则以这些点为基础初始化相机姿态。
从全局重定位中初始化位姿估计
这个在原文中并没有对应的论文。如果无法从之前的关键帧中初始化位姿，那么就得从之前的关键帧候选中查询，看看是否能做到全局重定位。对于所有的候选关键帧我们采用RANSAC迭代并采用pnp算法寻找合适的相机位姿。如果我们可以找到一个拥有足够多内点的姿态，则在对应的关键帧中去寻找更多可能的匹配。最后，相机的姿态再次被优化，如果有足够的内点，那么Tracking线程继续。
跟踪本地地图
一单我们有一个估计的相机姿态并且有一组初始的匹配，我们在地图中寻找更多的地图点，期望找到更多的相关性。为了避免在大地图下过于复杂，我们只投影本地地图。本地地图包含从covisibility graph（这个概念在LOCAL MAPPING会详细说）提取的关键帧集合k1，其中的所有的关键帧与当前帧共享地图点。关键帧集合k2，为在covisibility graph中与k1中关键帧相近。本地图中包含参考帧kref，其与当前帧共享最多的地图点。现在对k1、k2中所有的地图点，进行以下的步骤：
- 将该地图点投影至当前帧，如果投影到图片外头就拒绝这个地图点；
- 计算当前帧看到地图点的方向向量v与地图点存储的平均方向向量n，计算v * n，如果v * n < cos(60°)则拒绝这个点；
- 计算地图点与当前相机中心的距离，如果d不在[最大距离， 最小距离]内，则拒绝该点；
- 计算当前帧的尺度信息d/dmin；
- 将这个地图点的描述子与当前帧中仍未匹配的特征点进行比较，找到x（关键帧上地图点投影的坐标）附近，在当前尺度下，可能的匹配到的点。将x与该地图点相关联。
最后通过所有匹配上的地图点，优化相机姿态。
决定新的关键帧
最后一步用以决定当前帧是否为新的关键帧。在local mapping线程中有机制专门用来删除冗余的关键帧，所以我们要尽可能多的插入新的关键帧，这会让相机运动更加鲁棒，尤其是当相机旋转的时候。为了插入新的关键帧，必须满足如下条件：
- 从上次全局重定位中至少已经经过了20帧；
- local mapping无效，或者上一次插入关键帧已经经过了20帧；
- 当前帧至少跟踪50个点；
- 当前帧与参考帧kref跟踪重合率小于90%。
条件1保证好的重定位，条件3保证这次跟踪不错，条件4保证至少产生了一些变化。
LOCAL MAPPING
这一部分会用到很多关于covisibility graph的概念，这个概念会有点重要必须展开说下~
covisibility graph
论文地址。这里也可以参考《视觉slam十四讲》中的第11章来看。
这几天把covisibility graph相关的部分重新整理了下，毕竟这个名字实在是查不到是什么意思，这么理解就好了co-visibility，姑且可以翻译成共视图，即能看到同一地图点的图。理解了这个下面也就好说一些了~
当机器人行动的时候，伴随着变量数量的增加，全尺寸的光束平差（Full bundle adjustment）的计算时间急剧上升，使得在难以在大尺度下进行SLAM。例如PTAM（论文地址，顺带一提ORB本身受这个影响也很大）在后台经行Full BA，所以只能在较小的空间内使用。目的是在小尺寸时与PTAM有相同的精度，同时在尺寸变大时候可以表现的好得多。使用3个技术去达到这个目标：
- 动态窗口（活动窗口）
- 相对表示
- 减少姿态姿态影响（Pose-pose reduction）
系统组成
动态窗口（活动窗口）
为了在V-SLAM系统中一直有常数运行时间，选择所有关键帧中的子集作为活动窗口是一个常用的方法。通常将最新得到的关键帧们当作活动窗口。通常活动窗口内关键帧的个数是确定的，并且可以观察到相同关键点的关键帧会被特别包含进来。
当固定的关键帧移动距离较大且不成环的时候结果较好。反之，这将会固定过多边界关键帧，影响收敛性。
相对表示
相对BA（Relative Bundle Adjustment）在关键帧与地图点之间使用相对表示。因为使用相对表示，所以全局地图点坐标与相机位姿仍然需要计算，但是通常来说，全局信息并不是时刻都需要的。所以为了可以在常数时间内做RBA，我们做了动态窗口假设。如果在小范围内没有回环，那么RBA表现不错，反之如果有回环的话，表现就会下降。
减少姿态姿态影响（Pose-pose reduction）
论文中这里对应的地方为Pose-graph Optimisation，姿态-图 优化。只优化姿态，（共视）点的信息被边缘化作为关键帧之间的约束。这样做是对原问题的一种近似，因为地图点-位姿之间两两之间的联系并不能完全代表它们之间的关系。同时也不会减少时间复杂度，仍然是线性至二次复杂度。
系统概述
总览
为了可以在常数时间内经行计算，我们使用活动窗口法。创新的地方在于我们使用双窗口法。内窗口为姿态-点约束，使用光束平差法优化。外窗口为姿态-姿态约束。姿态-姿态约束由共视性来定义（covisiblity），如果两个姿态可以观测到足够多数量的地图点，则给它们之间加上约束。不同于别人将两个窗口优化交替进行，我们在同一个过程中优化两个窗口。内窗口优化时会尽量精确，外窗口会稳定边缘化（应该指的是稳定姿态与姿态间的约束）。通过RBA，我们优化姿态与姿态之间的软约束，这个优化过程较难收敛。
SLAM图结构
SLAM中包含三种结构：关键帧集合，3D点集合与相对边集合。每个关键帧包括其绝对位姿，能够观测到的地图点以及所有相关的观测点（也就是图上的点）。所有的相对边包括两个姿态点中的共视性权重，等于两个姿态可以共同看到的所有地图点的数量。同时也会记录这个边有没有边缘化，如果边已经边缘化了，那么边也会记录两个节点之间的相对运动。
优化与边缘化
为了经行双窗口优化，我们从参考帧开始。在参考帧附近进行搜索，按照共视性进行排序。前M1个关键帧被认为在内窗口，后M2个关键帧被认为在外窗口（通常M1 << M2）。因此所有的M1，以及部分的M2，可以就像普通的BA一样，优化反投影误差即可。同时所有在外窗口的关键帧通过姿态-姿态约束相连，其总cost function如下：
第二项代表相对姿态误差。
优化过程中会优化所有能观测到的点xk，以及两个窗口内的所有姿态。
参考地图点的选择与增加新的关键帧
选择参考帧附近的帧与参考帧本体都可以观测到的点作为地图点备选。
一般来说，当前的视频帧会被当作关键帧直接加入。在所有的关键帧集合中与当前帧共视超过15-30个点时，我们给两个关键帧之间添加一条约束，这个约束非边缘化。
回环关闭（回环检测?）
米制回环关闭（Metric Loop Closure）
在选择参考地图点的时候，选择靠近参考帧的一部分关键帧集合N1。然后以参考帧为基础，按照cost function去搜索较大范围内的相邻帧N2。将N2中可以观测到N1中观测不到的地图点集合记为A。如果A数量足够大，就最小化其重投影误差。以参考帧作为基础，可以得到姿态Tloop。之后将A中所有重投影误差超过某个阈值的所有点全部移除。
对所有N1、N2中的关键帧，检查其能观测到多少A中的点。如果被选中的关键帧与参考帧有足够多的共同观测的点，那么就检测到一个米制回环关闭（Metric Loop Closure）。它们之间的约束就是边缘化的，记为：
大尺度回环关闭（Largescale Loop Closure）
使用3点RANSAC快速找到参考帧与以前的关键帧。如果超过阈值个内点被找到，就认为找到一个大尺度回环关闭（Largescale Loop Closure）。这将给两个关键帧之间添加一条约束，相关联的3D点将会合并为一个。
LOOP CLOSING
词袋模型
论文地址
特征
使用的特征就是上文中的FAST关键点和BRIEF描述子。定制了一部分参数但是总体没什么大差别，这里还是请看论文吧，都是作者试出来的。
图片数据库
使用的是二分离散数据。建立流程如下：1. 对训练数据集用k-means算法构成kw个群集（clusters）。这些群集组成词典树的第一层，即根节点Lw；2. 以每一个群集的描述子为基础，重复Lw次，构成一个有w个叶子的词典树。每一个叶子均有一个权重，出现频率越高的单词，其权重就越低（即tf-idf法）。那么图片It，就可以转化为词袋（ag-of-words）向量vt。图像的描述子均遍历词典书，在每一层选择汉明距离最小的节点。
计算两个向量v1与v2，使用下式经行评价：
同时也维护反向目录（inverse index）。每一个单词保存一个相关图片的列表以加速图片查询速度。每当一个新的图片加入时都会更新这个列表。
对每一张图片，增加直接索引以快速记录每个图片的特征。每一层都记录图片包含叶节点祖先的节点ID还有每个节点对应的特征。
回环检测算法
数据库查询
对所有的候选图片需要进行配对尝试。计算出来的分数分布差别很大，所以需要归一化处理，其计算方式为：
分母的意思即计算当前帧与上一帧的s。当分母很小（如机器人旋转）时计算结果可能会很大。所以给分母设定一个最小阈值，或者要求其至少有一定的数量相同的匹配特征点。
分组匹配
为了缩短查询时间，将一定范围内的关键帧当作一个岛屿（island）。对岛屿内的分数求和即该岛屿的分数。同时这个方法也可以帮助搜索回环。
暂时一致性（Temporal consistency）
找到匹配最合适的岛屿后，需要保证当前帧前k个帧与当前岛的(overlap)也有一定的相关性。如果通过这项测试，选取当前岛中分数最高的一帧作为候补，进入最后一步。
有效几何一致性（Efficient geometrical consistency）
当至少有12个匹配点时，通过RANSAC计算fundamental matrix检查回环候补。之前在构建词典树时我们记录了直接索引。这样对每张图，在字典树上的某一层上只需要比较相同节点的结果。至于在那一层上进行搜索，则是对准确性和计算速度的一种衡量。
基础图（Essential Graph）
为了快速检测回环，引入基础图，其保存所有的关键帧，但是边却比较少。基础图的生成树以第一个关键帧为树根建立一个生成树。当插入新的帧时，其被包括在共享最多的节点。删除一个节点时候使用删除策略（culling policy）。
基础图包括生成树，共识点超过100个点的共视图子集，以及回环之间的边。
回环纠正
回到orbslam中。通过回环检测，有一部分地图点会被再次确认。当前关键帧附近所有观测到确认过的地图点的关键帧，会与上一个回环中的位姿增加边的约束。之后更新基础图即可。