[论文笔记]ORB-SLAM: a Versatile and Accurate Monocular SLAM System
ORB-SLAM
一、introduction
PTAM
- 缺少闭环检测
- 缺少对于闭塞（occlusion）的处理
- 重定位时对视角的不变性低
- 自启动(bootstrapping)时需要人工干涉
ORB SLAM
以PTAM为基础
contributions：
- 使用ORB features在tracking、mapping、relocalization、loop closing任务中，保证在视点和光照变化时的不变性。
- 使用covisibility graph，使得能在大规模环境中实时运行。
- 基于essential graph的实时闭环检测。
- 实时相机重定位
- 提出基于模型选择的自动地图初始化方法，能够在平面和非平面环境下创建初始地图。
- 使用
宽进严出，”宽进严留“ (generous in the spawning but very restrictive in the culling)的survival of the fittest方法，选取关键帧和地图点。
二、system overview
总结一下就是，tracking、local mapping、loop closing三个线程并行，这三个部分都使用ORB特征。mapping部分的关键帧和地图点存储宽进快出，数据结构比较常规。loop closing部分使用DBoW2进行匹配以检测闭环。构造了两图一树，表示关键帧之间的临近关系，这个两图一树暂时不知道是用在哪儿的。
1. feature choice
特征的选择要求：
- 每张图片的提取时间小于33ms
- 具有旋转不变性
选取ORB特征。 并在tracking、mapping、place recognition(用于重定位和闭环检测)中都使用ORB特征。
2. 线程
分为三个并行线程：tracking、local mapping、loop closing
2.1. tracking
功能：每一帧相机位姿的估计以及关键帧的检测。
步骤：
- 当前帧特征抽取，与上一帧特征匹配，得到当前位姿的初始估计。
- 若匹配失败（由于遮挡或两帧差距过大），则使用place recognition模块进行全局的重定位，得到初始位姿。
- 使用motion-only BA进行位姿估计的优化。
- 从局部地图模块得到当前视角的局部地图（由全局关键字的convisibility graph维护），根据重投影，搜索当前帧特征点和局部地图点的匹配。
- 根据得到的匹配结果，再次优化位姿估计。
- 决定当前帧是否作为关键帧插入。
2.2. local mapping
功能：处理新进入的关键帧，执行局部BA，以达到当前相机位姿附近的最优的局部重建。维护地图，加入新的点，去除低质量的点和关键帧。
步骤：
- 插入新的关键帧
- 使用exigent point culling policy去除低质量的点。
- 若当前帧的特征点和局部地图中的点不匹配，则covisibility graph的连接关键帧中搜索对应关系，三角化出新的点，插入。
- 执行局部BA。
- 去除低质量的局部关键帧。
2.3. loop closing
功能：在每个新关键帧执行闭环检测，若检测到闭环，则做调整。
步骤：
- 对新进入的关键帧执行闭环检测。
- 若检测到闭环，计算相似性转换，得到累积误差。
- 将闭环的两侧对齐，融合重复点。
- 在essential graph上，根据相似性限制，执行位姿图优化，以达到全局一致性。
3. 地图点、关键帧的描述及选取
3.1. 地图点 p i p_i pi
包含：
- 世界坐标系中的三维坐标 X w , i X_{w,i} Xw,i
- 观测方向 n i n_i ni。是所有观测到这个点的关键帧的中心视角的平均单位向量。(是只有一个吗？？)
- ORB描述子 D i D_i Di。 是所有观测到它的关键帧的描述子中，汉明距离最小的一个。
- d m a x , d m i n d_{max},d_{min} dmax,dmin。（不知道是啥的距离？？）
3.2. 关键帧 K i K_i Ki
包含：
- 相机位姿 T i w T_{iw} Tiw，从世界坐标系到相机坐标系。
- 相机内参，包含焦距和焦点。
- 此帧的所有ORB特征，无论是否与地图点关联。未扰动的。
3.3. 选取方式
宽容选取，紧急去除。去除冗余关键帧、误匹配地图点、漏匹配地图点。能够提高地图探索时的鲁棒性，外点少。
4. covisibility graph and essential graph
essential graph的权重阈值是多少？比spaaning tree小？
好像这不是三个图，而是两个图。
spanning tree指的就是essential graph，essential graph是covisibility graph的最小生成树（MST）。
4.1. covisibility graph
无向有权图。表示关键帧之间的相邻关系。
- 点：关键帧
- 边：若两个关键帧之间共同观测到的地图点大于15，则连一条边
- 权重：两个关键帧之间共同观测到的地图点数量
4.2. essential graph
保留covisibility graph中的全节点，保留很少量的边。
4.3. spanning tree
从最初关键帧开始持续维护的树，是covisibility graph的子图。
当新关键帧插入：此节点连接到与他有最多共同观测的节点上
当关键帧删除：更新受影响的其他节点的连接关系
essential graph包含spanning tree，这个树的权重阈值很高(100)。
5. bags of words place recognition
基于DBoW2的词袋模型，实现回环检测和重定位。词典由数据集抽取的ORB描述子离线制作。
维护一个数据集：包含单词到包含它的关键帧的索引。对于overlap的情况，将这些重叠的关键帧建成covisibility graph。（这儿不太懂，这个图是干嘛的，之前的方法是怎么做的）
trick：在对比两组ORB特征的相似性时，强制只匹配单词树特定等级的属于相同节点的特征，以加速。（？？？这节点是啥，单词树的等级是啥？是K叉树吗）
三、 地图自动初始化
目标：计算相邻两帧之间的位姿，用于三角化匹配点，得到其深度，以初始化地图点。
方法：同时并行两个几何模型。
- 对于平面：使用单应矩阵
- 对于非平面：使用基础矩阵
使用启发式方法选择一个模型，并使用该模型的特定方法恢复其位姿。
算法流程：
总结一下是，先提取特征点并匹配，然后同时计算单应和基础两个模型，根据得分选则其中的一个，计算出来矩阵后，根据矩阵算出相机位姿，再三角化算出匹配点深度。最后用BA优化位姿和深度。
1. 匹配特征点
对当前帧 F c F_c Fc抽取ORB特征，与参考帧 F r F_r Fr进行匹配。若匹配到的数量不足，则重置参考帧(这个重置是什么意思？)
2. 并行计算两个模型
并行的计算单应矩阵 H c r H_{cr} Hcr和基础矩阵 F c r F_{cr} Fcr。
X c T F c r X r = 0 X_c^{T} F_{cr}X_r=0 XcTFcrXr=0
X C = H c r X r X_C=H_{cr}X_r XC=HcrXr
在RANSAC迭代中，使用DLT和8点法分别计算单应矩阵和基础矩阵，二者的迭代次数固定且相同，每次迭代用的点分别是4和8。每次迭代，使用对称转换误差（symmetric transfer errors）计算模型分数 S M S_M SM，并维护最高得分。
3. 模型选择
选取标准：
R H = S H S H + S F R_H=\frac {S_H}{S_H+S_F} RH=SH+SFSH
平面、低视差，选择单应模型， R H > 0.45 R_H>0.45 RH>0.45。
非平面、高视差，选择基础矩阵模型， R H < = 0.45 R_H<=0.45 RH<=0.45。
4. 相机运动与结构恢复
选择相应模型并计算，可以得到矩阵。根据这个矩阵求出相机运动(R,t)，三角化得到点的深度。
对于单应矩阵模型：使用参考文献[23]的方法得到8种可能的假设，对其分别三角化，选取低视差、景深为正（点在相机前面）、低重投影误差的假设。
对于基础矩阵模型：使用内参矩阵 K K K将其转为本质矩阵。
E r c = K T F r c K E_{rc}=K^{T}F_{rc}K Erc=KTFrcK
使用奇异值分解得到4种可能的假设，对其分别三角化，选取景深为正的假设。
5. BA
使用BA优化初始位姿和深度。
四、tracking
此部分的整个系统始终运行的平行三线程之一，其作用是：对每一帧，估计其相机位姿，选择新的关键帧。
（这部分和初始化的关系是啥？初始化只用了前面一部分的帧，初始化了一部分的地图点吗？初始化部分是用单目2D-2D的方式，然后再 通过tracking，使用3D-2D的方式，估计随后的每一帧位姿？）
（这里的提取ORB特征，和初始化部分的应该是不一样的，这里在八个尺度上进行，初始化部分只在一个尺度上，这里得到的ORB描述子，应该也没用于初始化？但是初始化用的是啥呢，初始化的“当前帧”和这里的“新帧”不是一个吗？）
基于前一帧的位姿初始化，是建立当前帧与上一帧之间的联系；基于全局重定位的位姿初始化，是建立当前帧与某一关键帧的联系；基于局部地图的位姿估计，是建立当前帧与局部关键帧地图的联系，也就是通过搜索更多的3D-2D的匹配点来对更精确的估计当前帧的位姿。
1. 提取ORB特征
先对图像提取固定量的FAST角点，尽量保证角点分布均匀。然后计算焦点的朝向和ORB描述子。
最终得到ORB描述子，用于所有模块的特征匹配。
2. 根据前一帧初始化相机位姿
如果前一帧的估计成功，则使用匀速运动模型（constant velocity motion model）估计当前帧的位姿，并使用guided search在当前帧中搜索上一帧的地图点。这个模型是认为相机处于匀速运动，根据匀速运动来估计当前帧位姿的初始值。
之后根据搜索到的点匹配关系（3D-2D），优化当前帧的相机位姿。若没有搜索到足够的匹配点，则在地图点的周围搜索，看有没有匹配。
3. 根据全局重定位初始化相机位姿
若前一帧丢失，则把当前帧转化为词袋向量，查询关键帧数据库实现全局重定位。
对于数据库中的每一个关键帧，使用RANSAC和PnP算法，估计相机初始位姿，若由足够多的内点， 就优化这个位姿（咋优化的？？？），并根据这个关键帧，使用guided search，找到匹配的3D-2D点，再次优化相机位姿。若内点足够多，当前帧则被tracking到。
4. 基于局部地图的追踪
得到初始化的相机位姿和3D-2D匹配点集后，根据局部地图丰富匹配点集，对相机位姿再次优化。
局部地图的选取：与当前帧具有相同观测地图点的关键帧、以及这些关键帧在covisibility graph上的邻居关键帧。其中，与当前帧有最多共同观测地图点的关键帧，就是参考帧。
对这些关键帧观测到的每一个地图点：
- 计算地图点到当前帧上的投影，若超出边界，不要。
- 计算当前帧视角 v v v与地图点的平均视角方向 n n n（是地图点数据结构中的一个成员）的夹角，若 v ⋅ n < c o s 6 0 ° v·n<cos60^{°} v⋅n<cos60°，不要。
- 计算地图点到当前帧相机中心的距离 d d d，若不在地图点的区间 [ d m i n , d m a x ] [d_{min},d_{max}] [dmin,dmax]内（也是地图点数据结构的一个成员），不要。
- 计算尺度， d / d m i n d/d_{min} d/dmin
- 笔记地图点的描述子 D D D(还是它数据结构的一个成员)，和当前帧未匹配的ORB特征描述子，选择尺度因子和距离最近的作为最优匹配对(什么意思？？？)，把这组匹配加入当前帧的3D-2D匹配集。
遍历完成后，用丰富的3D-2D匹配集再次优化当前帧的位姿。
5. 选取新关键帧
用于决策当前帧是否作为关键帧加入，其条件是：
- 与上次全局重定位间隔20帧以上。目的是确保良好的重定位。
- 局部地图线程空闲，或与上一次关键帧插入间隔20帧以上。目的是不打扰局部地图线程，不太频繁的更新地图。
- 当前帧追踪了至少50个地图点。目的是确保良好的追踪，当前帧的质量要好。
- 当前帧追踪的地图点在其参考帧的90%以下。目的是确保最小的视角变化。（这不太懂，这样岂不是每一个关键帧的点都越来越少?）
五、local mapping
这部分的平行三线程之一，它的任务是：处理每一个新关键帧，在其插入的时候，管理地图点和关键帧的集合，进行局部BA优化。
1. 关键帧插入
关键帧插入的时候，要更新以下内容：
- covisibility graph。新关键帧作为新节点插入，并与所有与其有共同观测的节点连边。
- spanning tree。连接到与与其有最多共同观测的节点上。
- 词袋向量。计算其词袋向量。
2. 地图点剔除
地图点剔除策略是：在其创建后的前三个关键帧之内，对其稳定性测试。过了这段视时间后，进入稳定状态，不容易被剔除。
创建后前三帧内的稳定性条件：
- 追踪系统预测，在至少25%的帧中，这个点都能被观测到。（这个帧比关键帧要密集的多！）
- 该地图点创建后，至少有连续三个关键帧观测到它。
稳定状态后的剔除条件：
- 任意时间，观测到它的关键帧少于3。
- 关键帧被剔除。
- 局部BA把它当作外点而丢弃。
3. 地图点创建
K i K_i Ki：当前关键帧
K c K_c Kc：covisibility graph中与 K i K_i Ki相连的关键帧
对于每一个 K i K_i Ki中未匹配的ORB特征，在 K c K_c Kc中寻找它的匹配点，并丢弃不满足堆积约束的ORB特征。找到匹配对后，对其三角化。
若其满足：
- 景深为正
- 视差条件
- 重投影误差条件
- 尺度一致性
则作为新地图点加入。
如果这个新创建的地图点还能被其他帧匹配到，就把它当作3D-2D对，用于优化那一帧的相机位姿[四-4]。
4. 局部BA
优化的目标有：
- 当前关键帧 K i K_i Ki
- 与 K i K_i Ki相连的关键帧 K c K_c Kc
- K i K_i Ki和 K c K_c Kc能观测到的所有地图点
同时，能观测到这些被优化的地图点的关键帧，也参与优化，但其值固定不变。
过程中会抛弃被判定为外点的地图点。
5. 关键帧剔除
若一个关键帧观测到的90%的地图点，能被其他三个关键帧观测到，则剔除这个关键帧。
六、loop closing
闭环检测部分对于当前最后一个被local mapping线程处理完的关键帧， 进行回环的检测和优化。
这部分看不太懂啊。
大概是，先进行闭环检测，先检测候选帧，从候选帧中检测是不是闭环帧。
检测到闭环帧之后，消除累积误差。通过计算得到的相似性矩阵，调整当前帧局部的关键帧位姿、调整它们观测到的地图点、调整他们相关的covisibility graph、最后通过位姿图优化调整全局的关键帧位姿和地图点。
1. 闭环检测
首先检测闭环候选帧：
- 根据词袋向量，计算当前帧与其所有covisibility graph上相邻帧的相似性，得到最小值 s m i n s_{min} smin
- 查询关键帧数据库，所有所有相似性得分小于 s m i n s_{min} smin的关键帧都排除
- 排除与当前帧直接相连的关键帧
- 其他关键帧作为闭环候选帧
确定闭环帧：
- 若连续三个候选闭环帧是一致的（？？？），则作为闭环帧。
2. 计算相似变换
单目slam中有七个自由度可以累积漂移：三个平移、三个旋转、一个缩放。
计算当前帧和闭环帧的相似变换，可以得知闭环中的累积误差，也能用来验证闭环的有效性（确定闭环候选帧是不是闭环帧吗？）。
首先计算当前帧和闭环帧分别对应的地图点，其ORB特征的相似性。（闭环帧和当前帧虽然是同一个地方，但是却生成了不同的地图点，要找出来这些相同真实点的不同地图点的匹配对?）得到3D-3D点对，对于每一个闭环候选帧（为啥这里还是候选帧？？）使用RANSAC算法迭代，计算相似性矩阵。
若计算的相似性矩阵有足够的内点，就优化这个候选帧，然后使用guided search，得到更多的匹配点对，再次优化，若计算得到的相似性矩阵仍然有足够的内点，则选为闭环帧。
3. 闭环融合
检测到闭环，要用闭环帧纠正当前帧的错误。
调整关键帧位姿：
当前帧的位姿 T i w T_{iw} Tiw，被相似性矩阵作用，调整其位姿估计。covisibility graph中与当前帧相连的关键帧，其位姿也会被调整。
调整地图点：
闭环帧和其邻居观测到的地图点，都被投影到当前帧和其邻居中，并寻找3D-3D匹配对。这些被匹配到的地图点，和相似性矩阵中的内点进行融合。
更新covisibility graph：
所有参与融合的关键帧，都更新其边的信息（？），并创建与回环帧相连的边（地图点融合了，这个帧就能看到地图点，就与回环帧有边了）。
4. essential graph优化
调整全局：
为了有效的闭合回环，在essential graph上进行位姿图优化。优化后，对每个地图点，根据其中一个观测到它的关键帧的校长，对其进行变换。
七、 实验
进行了三组实验：
- NewCollege Dataset，考察系统整体表现
- TUM RGB-D，考察定位精度、重定位、生存期
- KITTI，考察实时大尺度场景操作、定位精度、图优化效率
评测标准：
- 定位精度：ATE(absolute trajectory error)
- 轨迹精度：RMSE
1. NewCollege Dataset
20fps, 512*382, 存在闭环和视角的快速旋转。
2. TUM RGB-D Benchmark
有轨迹标签，适于评测定位精度。测试时，去除了弱纹理、纯旋转、无运动的部分。
3. KITTI
有高精度的轨迹标签，含有回环。