以下皆以单目为主

（一）输入

• 图像+时间戳（用于传感器融合）
• 相机内参：zoom不可改变，否则影响焦距和成像：内参跟图像大小有关（图像缩放，内参也要相应功缩放）
• 特征检测描述子类型，及对应描述子DBOW词袋（这里是ORB特征）

（二）数据结构

1、Fmera：

将获取的图像数据连同当前相机参数封装成

帧

，之后主要对帧进行操作：

2、其他：

3、关系图：

（三）Tracking线程

功能：

确定每帧位姿；确定关键帧给Local Mapping

分模块：

初始化、跟踪、重定位、确定关键帧

1、初始化：

目的：每两帧2D-2D计算的位姿尺度不是统一的，需要确定较为准确的3D点，通过3D-2D获得统一尺度的位姿。

自动初始化方法：分出两个线程同时计算单应矩阵约束与基础矩阵约束，根据两种约束关系计算累计误差，由累计误差比值确定特定环境下最终选择哪一约束。

2、跟踪：

获得当前帧与MapPoint的匹配关系，一般跟踪方法：帧与帧；帧与地图。

• 帧与帧（以下二选一）：

跟最近关键帧匹配（利用词袋来辅助匹配）

根据假设运动模型估计当前帧位姿，将上一帧map point投影到当前帧来匹配

• 帧与局部地图（必须进行）：

（1）ORB-SLAM局部地图组成：

（2）MapPoint投影到当前帧经过以下过滤后，再寻找匹配：

MapPoint必须能投到当前帧上（超出边界就排除）；

MapPoint在相机坐标系下深度得是正；

MapPoint在深度范围内（每个MapPoint都会计算最大最小深度）

当前视图与MapPoint平均视图方向夹角小于60（防止视角变化导致的误匹配）。

3、重定位：

• 对关键帧序列查找找到共视帧作为后选帧；
• 对每一关键帧DBow匹配，当匹配数超过15，EPnp计算当前帧位姿；
• 5次RANSAC迭代，优化位姿；
• 优化后匹配特征数若小于50，则通过候选帧MapPoint投影到当前帧寻找匹配，然后再次优化帧，直到匹配大于50则重定位成功。

4、确定新关键帧：

当两帧间隔较大，匹配到的MapPoint较少，局部优化空闲时需要添加关键帧。

（四）Local　Mapping线程

功能：

局部地图维护与优化

分模块：

关键帧添加、新Map Point创建与融合、局部优化、差Map point与重叠关键帧剔除

１、关键帧添加：

确定新关键帧后，需维护Ｃｏｖｉｓｉｂｉｌｉｔｙ　Ｇｒａｐｈ，ｓｐａｎｎｉｎｇ　ｔｒｅｅ，Ｍａｐ并计算该帧词袋匹配，为三角化作准备；

２、新Ｍａｐ　Ｐｏｉｎｔ创建：

• 找到共视邻接关键帧（共视点数设为２０）；
• 对邻接关键帧遍历，通过与当前关键帧极线上搜索并匹配；
• 匹配后三角化计算３Ｄ点；
• 配置MapPoint相关属性（平均视向，观测距离，最佳描述子等）；
• 检查新建MapPoint与之前MapPoint是否有交集：找到当前帧的邻接帧的邻接帧（二级邻接帧），将当前关键帧MapPoint投影到两级邻接帧上，判读同一特征下是否有多个MapPoint值，若是就选观测最多的那个；否则将新MapPoint加入每一帧上。
• 更新了MapPoint后对Ｃｏｖｉｓｉｂｉｌｉｔｙ　Ｇｒａｐｈ和Spanning　Tree更新。

３、局部ＢＡ优化：

根据Civisibility　Graph确定局部关键帧并设为图节点，并将局部关键帧上的MapPoint设为图节点；图边观测值为MapPoint对应特征坐标，考虑一个像素误差＊尺度作为信息矩阵；去除不好的边多次优化。

４、MapPoint和关键帧剔除：

MapPoint被观测次数少；关键帧上的MapPoint在局部关键帧中被至少３个其他关键帧观测９０％以上都剔除。

（五）Loop　Closing线程

功能：

克服位姿计算误差引起的尺度漂移，进而导致的轨迹误差进一步扩大

分模块：

回环检测、相似变换、闭环融合、优化Essential　Graph

１、回环候选帧检测

：根据Covisibility　Graph确定局部关键帧；在关键帧序列找相似度与局部关键帧相似度一样的作为候选帧；对每个候选帧检测时间一致性（当前帧前后几帧都与候选帧相似）

２、计算ｓｉｍ３：

当前帧与候选帧匹配后再Map point匹配，然后RANSAC计算ｓｉｍ３，然后互投影各自Map point寻找匹配，优化ｓｉｍ３　，然后将闭环帧和其相邻帧的Map point用ｓｉｍ３投影到当前帧，寻找更多匹配，若匹配数大于４０则计算成功。

３、闭环融合

：将当前帧对应Mappoint换成对应闭环上及其相邻帧的；重建ｃｏvisibility　graph；

４、优化Essential　Graph：

所有关键帧作为顶点，估计量为其ｓｉｍ３，闭环帧不做优化；对于当前帧集合与闭环帧集合添加闭合边（观测值为两帧间的相似变化）；对每个关键帧找到spanning　tree中的父节点帧，添加边；边添加完后进行位姿图优化。

５、全局BA：

所有关键帧与mappoint一起作为顶点进行全局优化。