ORB随便记一记

论文摘取

（这部分看的是泡泡机器人的翻译）

基于特征点、单目、完全自动初始化，基于PTAM框架。

系统框架

A.特征选择

对于构图、跟踪、重定位以及闭环检测都采用相同的特征每张图像特征提取<33毫秒，且具有旋转不变性

B.三线程

追踪、局部地图构建、闭环检测

跟踪

对每帧图像的相机位置进行定位，插入关键帧，与前一帧匹配得到初始特征点，运动BA优化摄像头位姿，跟丢则全局重定位。获取初始位姿后，则使用系统维护的关键帧的covisibility graph提取一个局部可视化地图。然后通过重投影方法搜索当前帧与局部地图对应的匹配点，并用所有匹配点优化当前位姿。

局部地图

负责处理新的关键帧，对周围相机位姿进行局部BA优化重构。三角化新的地图点，删除冗余的点和关键帧。

闭环检测

对每个新的关键帧进行闭环检测，存在闭环则计算相似变换查看闭累计误差。

最后利用相似性约束对位姿图进行优化，优化的是essensiol graph。

优化算法全部采用g2to中的Levenverg-Marquardt

C.地图点云、关键帧及其选择标准

对每个地图点云保存以下信息：

世界坐标系中的3D坐标
视图方向n_i，连接点云和对应关键帧光心
ORB特征描述子
根据ORB尺度不变性约束，可观测点云的最大距离d_max和最小距离d_min

关键帧K_i保存以下信息：

相机位姿
相机内参
图像帧提取的ORB特征，经过畸变矫正

D.convisibilty graph 和 essential graph

convisibilty graph：两个关键帧之间同时观测到15个点，则关键帧之间连一条边，权重为公共点数

essential graph：节点数和convisibilty graph一样，但是点更少（类似于最小生成树），插入新的关键帧时，将公共点最多的帧相连。

E.词袋模型的位置识别

基于DBOW2算法执行闭环检测和重定位

视觉词典

关键帧重叠问题

字典树强制匹配加快特征点匹配，用于三角化新的点云，闭环检测和重定位

引入方向一致性改进匹配点

地图自动初始化

分为平面视图（单映矩阵）和非平面视图（基础矩阵），启发式方法选择模型。算法步骤：

查找初始点对（ORB）匹配
并行计算两个模型：单映矩阵和基础矩阵，前者为四对特征点，后者为8对特征点，每次迭代时给每个模型计算一个分值SM，从单映矩阵和基础矩阵中选择分值高的。分值都不高则重启
模型选择：启发式计算RH
运动和从运动到结构的重构：单映矩阵是否能执行，基础矩阵计算本证矩阵后奇异值分解，见slam14讲7
BA：全局BA优化，见附录

跟踪

A.ORB特征提取

在8层图像金字塔上提取FAST角点，金字塔尺度因子为1.2。如果图像分辨率较高，则适当增加角点数。根据角点计算ORB特征描述子。用于后续特征匹配。

B.通过前一图像帧估计相机的初始位姿

如果上一帧图像跟踪成功，则认为摄像头处于匀速运动。利用前后帧的匹配点对当前相机进行进一步优化。未找到足够匹配点则扩大搜索范围。

C.通过全局重定位来初始化位姿

若扩大搜索范围依然找不到足够匹配点，则计算当前帧的词袋向量选取若干帧备选，对于每个备选帧执行pnp算法计算当前帧的位姿（Ransac迭代求解）。如果找到一个姿态能够涵盖足够多的有效点，则搜索该关键帧对应的更多电云。最后基于找到的所有匹配点对相机位姿进行优化。

D.跟踪局部地图

当获得了相机位姿和一组初始的匹配点，则可以将更多的点云投影到图像上以找到更多的匹配点。为了降低复杂度，只映射局部地图。该局部地图包含一组关键帧K1，它们和当前关键帧有共同的地图点，还包括与关键帧K1在covisibility graph中相邻的一组关键帧K2，局部地图中有一参考关键帧Kref∈K1，它与当前帧有最多的共同地图点。对K1和K2的每个地图点，在当前帧下做如下搜索：

计算地图点云在当前帧图像中的投影点x，超出图像边缘则删除。
计算当前视图射线v和地图点云平均视图方向n的夹角，如果n<cos(60)，删除对应点云。
计算地图点云到相机中心的距离d，若其不在地图点云的尺度不变区间内，即d∉[d_min,d_max]，删除该点。
计算每帧图像的尺度比d/d_min
对比地图中点的特征描述子D和当前帧中还未匹配的ORB特征，在预测的尺度层和靠近x的点做最优匹配。

相机位姿最后通过当前帧获得所有的地图点进行优化。（目的是在当前点帧和局部地图之间找到更多的匹配点对优化位姿）。

E.新关键帧的判断标准

距离上一次全局重定位后需要超过20帧图像
局部地图构建处于空闲状态，或距上一个关键帧插入后，已有超过20帧图像
当前帧跟踪少于50个地图点
当前帧跟踪少于参考关键帧K_ref点的90%

局部地图构建

A.关键帧插入

首先更新covisibility graph，包括：添加一个关键帧节点Ki，检查与Ki有共同点的其他关键帧，用边线连接。计算该关键帧的词袋，并利用三角法生成新的地图点。

B.地图点云筛选

三角化的点为了保留在地图中，必须在其创建后的头三个关键帧中通过一个严格的测试，该测试确保留下的云点都是能被跟踪的，不是由于错误的数据而被三角化的。该点必须满足以下条件：

跟踪线程必须在超过25%的图像中找到该特征点
如果创建地图点经过了多个关键帧，那么它必须至少是能够被其他3个关键帧观测到。

一旦一个地图点通过测试，它只能在被少于3个关键帧观测到的情况下移除。这样的情况在关键帧被删除以及局部BA排除异值点的情况下发生。这个策略使得我们的地图包含很少的无效数据。

C.新地图点创建

新的地图点的创建是通过对covisibility graph中连接的关键帧Kc中的ORB特征点进行三角化实现的。对Ki中每个未匹配的ORB特征，我们在其他关键帧的未匹配云点中进行查找，看是否有匹配上的特征点。这个匹配过程在第三部分第E节中有详细阐述，然后将那些不满足对级约束的匹配点删除。ORB特征点对三角化后，需要对其在摄像头坐标系中的深度信息，视差，重投影误差和尺度一致性进行审查，通过后则将其作为新点插入地图。起初，一个地图点通过2个关键帧观测，但它在其他关键帧中也有对应匹配点，所以它可以映射到其他相连的关键帧中，搜索算法的细则在本文第5部分D节中有讲述。

D.局部BA

局部BA主要对当前处理的关键帧Ki,以及在covisibility graph中与Ki连接的其他关键帧Kc，以及这些关键帧观测到的地图点进行优化。所有其他能够观测到这些点的关键帧但没有连接Ki的会被保留在优化线程中，但保持不变。优化期间以及优化后，所有被标记为无效的观测数据都会被丢弃，附录有详细的优化细节。

E、局部关键帧筛选

为了使重构保持简洁，局部地图构建尽量检测冗余的关键帧，删除它们。这样对BA过程会有很大帮助，因为随着关键帧数量的增加，BA优化的复杂度也随之增加。当算法在同一场景下运行时，关键帧的数量则会控制在一个有限的情况下，只有当场景内容改变了，关键帧的数量才会增加，这样一来，就增加了系统的可持续操作性。如果关键帧Kc中90%的点都可以被其他至少三个关键帧同时观测到，那认为Kc的存在是冗余的，我们则将其删除。尺度条件保证了地图点以最准确的方式保持它们对应的关键帧。

闭环检测

线程抽取Ki——最后一帧局部地图关键帧，用于检测和闭合回环。具体步骤如下：

A、候选关键帧

我们先计算Ki的词袋向量和它在covisibility graph中相邻图像（θmin=30）的相似度，保留最低分值Smin。然后，我们检索图像识别数据库，丢掉那些分值低于Smin的关键帧。这和DBoW2中均值化分值的操作类似，可以获得好的鲁棒性，DBoW2中计算的是前一帧图像，而我们是使用的covisibility信息。另外，所有连接到Ki的关键帧都会从结果中删除。为了获得候选回环，我们必须检测3个一致的候选回环（covisibility graph中相连的关键帧）。如果对Ki来说环境样子都差不多，就可能有几个候选回环。

B、计算相似变换

单目SLAM系统有7个自由度，3个平移，3个旋转，1个尺度因子 [6]。因此，闭合回环，我们需要计算从当前关键帧Ki到回环关键帧Kl的相似变换，以获得回环的累积误差。计算相似变换也可以作为回环的几何验证。

我们先计算ORB特征关联的当前关键帧的地图云点和回环候选关键帧的对应关系，具体步骤如第3部分E节所示。此时，对每个候选回环，我们有了一个3D到3D的对应关系。我们对每个候选回环执行RANSAC迭代，通过Horn方法（如论文[42]）找到相似变换。如果我们用足够的有效数据找到相似变换Sil，我们就可以优化它，并搜索更多的对应关系。如果Sil有足够的有效数据，我们再优化它，直到Kl回环被接受。

C、回环融合

回环矫正的第一步是融合重复的地图点，在covisibility graph中插入与回环相关的的新边缘。先通过相似变换Sil矫正当前关键帧位姿Tiw，这种矫正方法应用于所有与Ki相邻的关键帧，这样回环两端就可以对齐。然后，回环关键帧及其近邻能观测到的所有地图云点都映射到Ki及其近邻中，并在映射的区域附近小范围内搜索它的对应匹配点，如第5部分D节所述。所有匹配的地图云点和计算Sil过程中的有效数据进行融合。融合过程中所有的关键帧将会更新它们在covisibility graph中的边缘，创建的新边缘将用于回环检测。

D、Essential Graph优化

为了有效地闭合回环，我们通过Essential Graph优化位姿图，如第三部分D节所示，这样可以将回环闭合的误差分散到图像中去。优化程序通过相似变换校正尺度偏移，如论文[6]。误差和成本计算如附录所示。优化过后，每一个地图云点都根据关键帧的校正进行变换。

自己的一些问题：

１．
frame.cpp中在调用的thread在哪定义的
thread threadLeft(&Frame::ExtractORB,this,0,imLeft);
thread threadRight(&Frame::ExtractORB,this,1,imRight);
2.关键帧cpp中使用了大量锁，？

3.keyframe.cpp 中GetConnectedKeyFrames为什么要用set

4.mappoint.cpp 中PC是什么向量

5.mappoint.h 中mnvisble和mnfound有啥区别 GetFoundRatio函数干嘛的 PredictScale 尺度方面还是不太懂

6.localMapping.cpp中 createnewmapPoints视差原理

7.尺度连续性是什么局部地图cpp中有提到
8.SearchInNeighbors 局部地图cpp 什么是一级相邻关键帧和二级相邻关键帧明白了，相邻和相邻的相邻

代码：

单目启动方式(以TUM数据集为例)：

编译后执行：

./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUMX.yaml PATH_TO_SEQUENCE_FOLDER

TUMX.yaml 为下载的数据集文件，X为1,2,3，由数据集名决定，PATH_TO_SEQUENCE_FOLDER为数据集路径

主函数在mono_tum.cpp中

mono_tum.cpp：

加载数据集函数 LoadImages()。由于只由于设定为单目SLAM，因此只加载TUM数据集中的rgb文件。
初始化System类对象：SLAM。System构造函数：加载字典和参数文件，创建关键帧数据库，创建并初始化地图(map)、创建画图(FrameDrawer、Mapdrawer)，初始化追踪线程tracking()。初始化局部地图线程并启动run()，初始化回环检测线程并启动run()，如果使用画图则初始化画图线程并启动run()。初始化完之后在进程之间设置指针。
按时间戳顺序遍历每一张（帧）图片，启动追踪TrackMonocular()函数。计时。停止所有线程shutdown()。
计算时间保存相机轨迹。

先看追踪TrackMonocular()：

加锁，判断模式，是不是只需要定位
判断系统是否重置
GrabImageMonocular()函数（该函数包含追踪），加锁，更新追踪后的状态。

转到GrabImageMonocular()函数：

将图像转化为灰度图（注意通道数和RGB顺序 )
创建第一帧Frame()，第一张图像的特征参数不一样
追踪Track()

tracking()构造函数：

加载并设定相机内参、图像矫正系数 baseline * fx、每一帧特征点数、图像金字塔变化尺度、层数、fast特征点阈值（用于特征点提取）
创建ORBextractor() 对象，双目则多一个右特征提取器。单目初始化时特征点数×2
双目和RGBD设置一个判断3D点远近的阈值： mbf * 35 / fx ？？？
对于深度相机，获取参数DepthMapFactor

track()函数：

判断是否已经初始化，否则根据相机类型初始化MonocularInitialization()、StereoInitialization()
判断模式（是仅仅追踪）
- 是否需要重定位
- 动力学位姿队列为空则追踪参考帧
- 动力学位姿队列非空则根据恒速模型设定当前帧的初始位姿
追踪定位模式，且匹配点足够多
- 是否需要重定位
- 追踪参考帧：TrackReferenceKeyFrame()
该帧匹配到点太少
- 动力学位姿队列非空则根据恒速模型设定当前帧的初始位姿
- 重定位
- 只要重定位成功整个跟踪过程正常进行
将最新的关键帧作为参考帧
在帧间匹配得到初始的姿态后，对local map进行跟踪得到更多的匹配：TrackLocalMap()，并优化当前位姿
更新绘图器
更新恒速模型的位姿
地图绘制对象设定当前相机位姿
排除UpdateLastFrame函数中为了跟踪增加的MapPoints
清除临时的MapPoints，这些MapPoints在TrackWithMotionModel()的UpdateLastFrame()函数里生成（仅双目和rgbd），这里生成的仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了
判断是否需要添加关键帧：NeedNewKeyFrame()，需要则：CreateNewKeyFrame();
删除那些在bundle adjustment中检测为outlier的3D map点
跟踪以及重定位都失败，则reset()
记录位姿信息，用于轨迹复现,如果跟踪失败，则相对位姿使用上一次值

TrackLocalMap()函数：

MonocularInitialization()函数：

单目初始帧的特征点数必须大于100
在初始帧和当前帧之间寻找匹配的特征点SearchForInitialization()
如果初始化的两帧之间的匹配点太少（100以内），重新初始化
通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints，删除那些无法进行三角化的匹配点
将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵,由Rcw和tcw构造Tcw,并赋值给mTcw，mTcw为世界坐标系到该帧的变换矩阵
将三角化得到的3D点封装成MapPoints并更新map，并进行BA优化: CreateInitialMapMonocular()

initialize

局部地图 LocalMapping:: run():

只要进入LocalMaping队列就不再接收关键帧SetAcceptKeyFrames(false);循环执行知道队列里的帧都被处理完。
队列中待处理关键帧非空则构建局部地图：CheckNewKeyFrames()
计算关键帧特征点的BoW映射，将关键帧插入地图：ProcessNewKeyFrame()
剔除ProcessNewKeyFrame函数中引入的不合格MapPoints：MapPointCulling()
三角化新的点：CreateNewMapPoints()
如果处理完队列里的所有关键帧，则检查并融合当前关键帧与相邻帧重复的mapPoints，且进行局部BA优化：LocalBundleAdjustment()后剔除冗余关键帧
将当前帧加入到闭环检测队列
继续添加关键帧SetAcceptKeyFrames(true);

回环检测LoopClosing::run():

循环处理
闭环检测队列mlpLoopKeyFrameQueue中的关键帧不为空：CheckNewKeyFrames()（与LocalMap不一样）
闭环检测：DetectLoop()
计算当前帧与闭环帧的sim3变换：ComputeSim3()
修正位姿，更新map：CorrectLoop()

DetectLoop()：

从队列取出一帧作为当前关键帧，如果距离上次闭环少于十帧，则不闭环检测
得到所有共视关键帧，计算当前关键帧与共视帧的bow相似度最低分minScore
在所有关键帧中找出闭环候选帧集合：DetectLoopCandidates()
在闭环候选帧中检测具有连续性的候选帧（具体这怎么做呢？）子候选组、子连续组、当前候选组

ComputeSim3()：

初始化一个Sim3Solver列表
对于每一闭环候选帧，将当前帧mpCurrentKF与闭环候选关键帧pKF匹配
通过Bow加速获取mpCurrentKF与pKF之间的匹配特征点，如果匹配点过少，则将该候选帧剔除
否则构造Sim3求解器设定Ransac迭代参数，每个候选帧最多迭代五次，总迭代次数达到最大限制还没有求出合格的Sim3变换，该候选帧剔除
上一步通过Sim3求解器求出当前候选闭环帧pKF与当前帧的R、t、s（变换尺度）。之后通过描述子进行匹配，匹配更多的特征点，更新mvpMapPointMatches。
更新vpMapPointMatches后再根据Sim3优化。如果求出的内点超过20，则当前候选帧即为选出来的闭环帧。
得到从世界坐标系到该候选帧的Sim3变换，Scale=1。跳出遍历
取出闭环帧的相连关键帧，得到它们的MapPoints放入mvpLoopMapPoints。
将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配SearchByProjection()：根据Sim3变换，将每个mvpLoopMapPoints投影到mpCurrentKF上，并根据尺度确定一个搜索区域，根据该MapPoint的描述子与该区域内的特征点进行匹配，如果匹配误差小于TH_LOW即匹配成功，更新mvpCurrentMatchedPoints。
判断当前帧与检测出的所有闭环关键帧是否有足够多的MapPoints匹配
清空mvpEnoughConsistentCandidates

CorrectLoop()：

停止局部地图，防止插入新关键帧
根据共视关系更新当前帧与其它关键帧之间的连接UpdateConnections()
通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的MapPoints
- 先将mpCurrentKF的Sim3变换存入，固定不动，其它的关键帧根据相对关系得到Sim3调整的位姿，得到闭环g2o优化后各个关键帧的位姿
- 得到调整相连帧位姿后，修正这些关键帧的MapPoints
- 将Sim3转换为SE3，根据更新的Sim3，更新关键帧的位姿
- 根据共视关系更新当前帧与其它关键帧之间的连接
检查当前帧的MapPoints与闭环匹配帧的MapPoints是否存在冲突，对冲突的MapPoints进行替换或填补,如果有重复的MapPoint（当前帧和匹配帧各有一个），则用匹配帧的代替现有的
通过将闭环时相连关键帧的mvpLoopMapPoints投影到这些关键帧中，进行MapPoints检查与替换SearchAndFuse()
更新当前关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系
进行EssentialGraph优化，LoopConnections是形成闭环后新生成的连接关系，不包括步骤7中当前帧与闭环匹配帧之间的连接关系
添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）
新建一个线程用于全局BA优化RunGlobalBundleAdjustment()