文章目录

ORB-Mono原理梳理
- 0. ORB主要特点
- 1. 补充内容
- - 1）ORB特征点介绍
  - 2）地图要素
  - 3）共视图&本质图&最小生成树
  - 4）单应矩阵，本质矩阵和基础矩阵
- 2. ORB实现原理
- - 2.1 地图初始化
  - 2.2 跟踪定位
  - 2.3 局部建图
  - 2.3 LOOP CLOSING
- 3. ORB实现过程：
- - 3.1 System SLAM构造
  - 3.2 Tracking线程
  - Local mapping线程
  - 3.4 loop closing线程
- 参考链接

ORB-Mono原理梳理

0. ORB主要特点

1）ORB特征点；2）共视图；3）本质图的回环处理

1）ORB特征点描述子相对SIFT轻量，节省计算时间。
利用角点(Fast关键点)的尺度方向恢复角点因为相机旋转带来的方向变化，恢复角点方向，再计算描述子，才能得到更加准确的匹配关系。

2）共视图用于局部BA优化，并辅助回环检测。
共视图根据观测权重可以被删减为本质图(Essential graph)，其包含单链的生成树(Spanning tree)。

3）本质图用于回环处理时进行的全局BA优化。缩小计算量。

1. 补充内容

1）ORB特征点介绍

ORB特征点的表述有3个关键内容：
1、Fast特征点判定；2、特征点描述子(Brief)；3、特征点主方向
1、Fast特征点：图像灰度值较大变动的点，即角点是指与该点临近处的像素灰度值波动较大。

2、ORB(Brief)描述子：用于对特征点进行描述，在特征点周围指定的区域内，提取特定的模式(pattern)下确定的N对固定坐标对应的像素，比较每对像素的大小(1/0)，得到N个对应(1/0)值，则作为特征点描述子，ORB描述子的pattern定义如下：
src/ORBextractor.cc

207 static int bit_pattern_31_[256*4] =
208 {209     8,-3, 9,5/*mean (0), correlation (0)*/,
210     4,2, 7,-12/*mean (1.12461e-05), correlation (0.0437584)*/,
……
463     7,0, 12,-2/*mean (0.127002), correlation (0.537452)*/,
464     -1,-6, 0,-11/*mean (0.127148), correlation (0.547401)*/
465 };

3、特征点主方向：描述特征点的灰度方向，计算以特征点为中心，半径为R的区域内的图像灰度质心，特征点指向质心的方向，设为特征点主方向。用于去除因相机旋转带来的影响。计算步骤：
计算沿两个坐标轴x,y方向的矩：

计算像素灰度值总和：

图像质心：

主方向：中心点O到灰度质心C的方向OC⃗\vec {OC}OC：

如下图所示：P为特征点，Q为计算得到的灰度质心。在进行ORB描述子匹配时，首先要使被匹配的特征点的主方向旋转一致，再进行描述子判断。

2）地图要素

地图要素：地图点&关键帧、轨迹。
地图点包含信息：
1、世界坐标系下的三维点坐标；
2、观测方向：所有能观测到该点的关键帧观测方向的单位平均方向，也即所有能观测到该点的关键帧对应的相机中心到该点的方向的平均值单位化；
3、最具代表性的ORB描述子。一个地图点可被多个关键帧观测到，计算描述子进行匹配，即一个特征点具有多个描述子。最具代表性的描述子表示该描述子与其他描述子的汉明 (hamming)距离最小。
4、地图点的最小、最远的观测距离。即该地图点能够被观测到的最大距离和最小距离，代码中使用金字塔的放缩因子计算得出。

关键帧包含信息：
1、对应的相机的位姿；
2、相机的内参；
3、所有提取的ORB特征点。

3）共视图&本质图&最小生成树

共视图表示所有观测到相同地图点的关键帧的连接关系(最小同时观测到15个以上的地图点)。上图（b）所示，共视图以关键帧为节点，讲具有共视关系的关键帧进行两两连线自称的图为共视图。

本质图（上图（d）所示）包含：
生成树；
共视图中具有较强共视关系(至少同时观测到100个共视地图点)的连接；
回环关系连线；

生成树：单链父子连接关系，新增的关键帧以其共视关系最大的已有关键帧作为父节点。

4）单应矩阵，本质矩阵和基础矩阵

https://blog.csdn.net/weixin_41469272/article/details/122906167?spm=1001.2014.3001.5502

2. ORB实现原理

ORB SLAM由三个独立线程：tracking线程、local mapping线程、loop closing线程。Tracking线程为主线程。初始化过程被集成到tracking线程中。

2.1 地图初始化

地图初始化以相机初始化帧的所在位置为原点，计算出第二帧的相机位姿，并通过三角化得到特征点深度(地图点三维坐标)。建立初始的局部地图(前两个关键帧及其地图点)。
相机位姿估计使用两种方法计算：H(单应矩阵homography)和F(基础矩阵fundamental)来恢复。根据比分自动选取合适的模型来得到位姿估计。

初始化步骤：
1）寻找匹配点，
接收到图像后，提取特征点，当特征点足够时，将当前帧作为初始化帧，等待下一帧进行匹配，当下一帧与初始化帧有足够多的匹配点，则将初始位姿作为世界坐标系，计算第二帧的位姿。当初始帧与第二帧没有足够多的匹配点，则清楚初始帧，等待下一帧再重复上述操作，直到有足够多的匹配点。

2）同时使用H/F来计算
由于单应矩阵和基础矩阵各自的特征，初始化过程中，使用随机策略选取匹配点对同时计算单应矩阵和基础矩阵。过程中使用随机策略选择8对匹配点，基础矩阵使用其中4对点，多次迭代随机选择匹配点，得到迭代中使外点数最少的单应矩阵及基础矩阵，以及各自的得分。矩阵的的得分计算公式如下：

其中xcix_c^ixci表示当前帧的第iii个特征点，xrix_r^ixri表示参考帧(初始帧作为参考帧)的第iii个特征点，MMM对应模型矩阵，即表示HHH或FFF。
dcr2d_{cr}^2dcr2和drc2d_{rc}^2drc2分别是参考帧与当前帧的双向投影误差。
ρM\rho_MρM对投影误差进行评估，得到分数。
TMT_MTM为模型误差阈值，根据卡方分布表确定，TH=5.99,TF=3.84T_H=5.99,\ \ T_F=3.84TH=5.99, TF=3.84
Γ=TH\Gamma=T_HΓ=TH保证不同模型相等的d2d^2d2得分相同，取较大的THT_HTH作为Γ\GammaΓ值。

3）H/F选择
H矩阵可以应用在平面和低视差两种情况下，而F矩阵可以用于非平面场景，但是在低视差场景下，解算位姿会失效，因此可以使用H阵来检测是否是低视差场景，从而取消初始化，因为准确三角化解算出地图点深度。ORB使用以下比值来自行抉择选择F或H恢复相机位姿。

当RH>0.45R_H>0.45RH>0.45时，使用HHH，反之，使用FFF。

4）位姿解算及3D点坐标恢复
当使用H阵时，对解算得到的8种位姿可能性都进行评估，直接通过8种可能性，直接三角化得到地图点3D坐标，检测点外点个数最小，且在两个相机的前方，并且统计重投影误差最小的解作为最优解。若没有最优解，则放弃初始化。
当使用F时，通过计算本质矩阵E来恢复4种位姿可能性，同H解算位姿一样，同时对4种可能位姿进行评估，选择最佳的解。

5）BA优化
最后，通过全局BA对初始化的关键帧以及地图点进行进一步优化。

2.2 跟踪定位

主要内容：位姿估计：提取特征点，初始化地图，粗略估计相机位姿，并生成关键帧。

定位方法有三种：恒速模式、帧间匹配，重定位匹配。

简要流程：提取特征点，匹配相邻两帧的特征点，估计两帧之间的相机位姿，得到特征点三维坐标，利用共视图观测优化相机位姿。并生成关键帧。

概述：
提取角点作为特征点，计算特征点的灰度质心及方向，以及特征点描述子，之后对两个相邻普通帧进行特征点匹配。地图初始化过程被包含在Tracking过程中，当没有初始化，则执行初始化。初始化成功以后的普通帧对应的相机位姿采用三种模式(恒速模式、帧间匹配，重定位匹配)估计得到，使用三角化得到特征点对应的环境3维坐标，然后找到其一二级共视帧，优化相机位姿估计。最后当当前帧满足一定的条件，则将该帧设为关键帧。

A. 特征点提取
在8层图像金字塔上提取Fast角点，每层金字塔的放缩比例系数为1.2。对于分辨率为m 512 × 384 和 752 × 480的图像，公提取1000个特征点，对于高分辨率 512 × 384 to 752 × 480的图像，提取2000个角点。每层图像期望提取的特征点数阈值固定，为确保特征点在图像上的均匀分布，ORBSLAM将每层图像分格，当栅格中纹理比较低或者低分辨率时，每格的期望角点数也相应进行动态调整，保证最后提取的特征点数大于期望阈值，且特征点分布均匀离散化。同时对每个角点的灰度方向以及ORB描述子进行计算。下图为特征点提取示意图。

B. 基于上一帧对当前帧进行位姿初始
当前一帧位姿计算成功时，首先使用速度模型度当前帧的位姿进行预测，即将速度乘以两帧间时间差，在累加到上一帧的位姿上，从而得到当前的位姿预测，并根据此预测值，将地图点映射到当前帧，在当前帧图像的映射点的范围内进行搜索匹配(根据ORB描述子及方向验证)，当找到的匹配点少于20，则降低匹配条件再次搜索，若仍旧找不到足够数量的内点则跟踪失败。当使用速度模型跟踪失败，则使用参考帧进行跟踪，即普通的特征点匹配(3D地图点-2D特征点-帧间匹配)方式求解位姿。

C. 重定位位姿
上一帧参考位姿计算失败后，需要开启重定位来进行位姿解算。将当前帧转换为Bow值，并在识别数据库中查询，得到候选关键帧。对于每个候选关键帧，均通过与地图点的ORB描述子进行匹配得到当前帧与候选关键帧的匹配点，而后通过PnP算法估计姿态。

D. 局部地图优化
使用地图对当前位姿估计进行优化。当得到相机的位姿估计以及特征点匹配关系后，将局部地图中点映射到当前帧，得到更多的地图点匹配关系。

局部地图包括：
1）一级共视关键帧K1\mathcal{K}_1K1，即与当前地图有共视地图点的关键帧的集合，其中包括了当前帧的参考帧Kref∈K1K_{ref}\in\mathcal{K}_1Kref∈K1，参考帧是与当前帧具有最多共视点的关键帧。
2）二级共视关键帧K2\mathcal{K}_2K2，即关键帧K1\mathcal{K}_1K1在共视图中的邻居关键帧，也即与关键帧K1\mathcal{K}_1K1有相同共视地图点的关键帧的集合；
3）K1\mathcal{K}_1K1与K2\mathcal{K}_2K2中的所有地图点。
注：K\mathcal{K}K表示关键帧集合，KKK表示单个关键帧

对局部地图中地图点进行筛选：
1）将地图点投影到当前帧，若投影坐标超出图像的边界，则丢弃。
2）计算地图点在当前帧相机视角下的观测方向vvv，以及地图点的平均观测方向nnn，当vvv与nnn偏离很大时v⋅n<cos(60°)v\cdot n<cos(60°)v⋅n<cos(60°)，该地图点被丢弃。
3）计算地图点距离当前帧相机中心的距离ddd，当ddd不在可允许的观测深度内(d∉[dmin,dmax]d\notin[d_{min},d_{max}]d∈/[dmin,dmax])，则丢弃。
4）计算地图点在当前帧的放缩尺度：d/dmind/d_{min}d/dmin，用于表征该地图点可在金字塔第几层上被观测到。
5）比较地图点最具代表性的描述子DDD与当前帧预测的图像层上的特征点进行匹配。
得到局部地图(关键帧、地图点及当前帧与地图点的匹配关系)后，进行局部位姿优化。

E. 是否插入新的关键帧：
为保证鲁棒性，关键帧的插入采取比较宽松的政策，在Local Mapping线程中会对冗余的关键帧进行筛除。当当前帧满足以下条件时，当前帧会被作为关键帧：

距离上一次重定位的帧，已经经历了20以上的普通帧。保证重定位的有效性
Local Mapping线程空闲，或者距离上一次的关键帧已经经历了20帧以上的普通帧。
当前帧可以匹配50个以上的地图点。
当前帧匹配的地图点比参考关键帧匹配的关键帧要少90%，即表示相机已经有了足够多的运动。

2.3 局部建图

主要内容：创建地图，处理新到的关键帧及其特征点，并将关键帧及地图点插入地图。

简要流程：当新的关键帧被建立后，添加关键帧到地图，将新检测到的地图点添加到候选队列，剔除劣质的地图点，然后进行局部BA优化，剔除冗余的关键帧之后，最后将关键帧加入闭环检测队列中。主要作用是对关键帧和地图点进行维护和筛选，即维护地图。

概述：

当新的关键帧被建立后，根据当前关键帧的描述子计算BoW向量，并将当前帧中的地图点与地图中的地图点进行匹配，如果当前关键帧的地图点是已经在地图中的，在该地图点的观测帧中加入当前帧；如果该点新提取得到的，将该点加入待筛选的队列当中，供后续筛选。
更新关键帧之间的连接关系，并将该关键帧插入地图。
随后筛选地图点，根据地图点被观测的召回率等来判断地图点的可靠性，剔除不可靠地图点。
将当前关键帧分别与共视程度最高的前10(单目相机取20)个共视关键帧两两进行特征匹配，并进行角度及卡方检验等，最后将合格点添加到地图。
将当前帧与一二级共视关键帧的地图点进行融合，更新共视图。
共视图得到了更新，利用共视图进行局部BA优化，一级共视图及局部地图点都会得到优化。二级参与约束，不被优化。
最后将关键帧送到回环检测队列中。

A. 关键帧插入
在得到新的关键帧后，首先需要根据该关键帧观测的地图点，更新共视图，为该关键帧建立新的节点，根据该帧观测到的地图点，建立与已在地图中的关键帧的共视关系。同时更新生成树，将与当前关键帧共视程度最高(观测到相同地图点个数最多)的关键帧作为父节点。之后，计算关键帧的BOW，为之后的匹配及三角化做准备。

B. 地图点筛选
满足以下条件的3D点才会被保存在地图：
1）实际观测到的点的帧数要大于理论上能观测到该点帧数的25%
2）该点从创建到当前观测帧已经过了2帧以上，此时该点被至少3个关键帧观测到。
当该点经过测试后，只有在被少于3个关键帧观测到时，会被删除。
当关键帧被删除以及局部BA去除外点时，地图点也会随着被筛除掉。

C. 地图点创建
将当前关键帧分别与共视程度最高的前10(单目相机取20)个共视关键帧两两进行特征匹配，丢弃掉不满足极线约束的匹配。三角化得到当前关键帧特征点的3D坐标。当3D点会被检查：深度在两帧中是否为正；视差，重投影误差，尺度一致性。最后3D点基于共视图进行融合。

D. 局部BA
当前关键帧及与其有共视关系的关键帧集合记为Kc\mathcal{K}_cKc。即当前关键帧及与当前关键帧的一级共视关键帧。
局部BA优化Kc\mathcal{K}_cKc中所有关键帧对应的相机位姿以及Kc\mathcal{K}_cKc中关键帧观测到的3D点。二级关键帧参与约束，但对应的位姿固定，不参与优化。外点会在优化中及结束时删除。

E. 局部关键帧筛除
局部建图会将冗余的关键帧筛除掉，从而降低BA优化时的计算复杂度，同时避免相同环境下由于长时间运行导致的关键帧大量冗余情况。
如果Kc\mathcal{K}_cKc中的地图点中90%被超过3个其他关键帧在相同或者更优层观测到，则将Kc\mathcal{K}_cKc中所有关键帧都删除。

2.3 LOOP CLOSING

对新到的关键帧进行回环检测，并利用相似矩阵(sim3)进行本质图优化及全局优化。
闭环检测原理: 若连续4个关键帧都能在数据库中找到对应的闭环匹配关键帧组,且这些闭环匹配关键帧组间是连续的,则认为实现闭环，并更新观测关系。

以上图片来自https://blog.csdn.net/ncepu_chen/category_9874746.html

A. 回环候选帧检测
从Local Mapping获得新的关键帧KiK_iKi，判断其距离上次闭环的时间，太短，不进行回环检测。计算与关键帧KiK_iKi共视关系(θmin=30\theta_{min}=30θmin=30有30个共视点)大于30的关键帧的BOW的相似度得分，并记下最低得分smins_{min}smin。丢弃掉得分小于smins_{min}smin的其他关键帧，此外去掉与关键帧KiK_iKi直接相连的关键帧，从而形成多个闭环候选帧Kl\mathcal{K}_lKl。对于每个候选关键帧，都将其跟其共视关键帧形成候选关键帧组。
若连续4个关键帧都能在数据库中找到对应的闭环匹配关键帧组,且这些闭环匹配关键帧组间是连续的,则认为实现闭环。

B. 相似变换计算
由于单目的尺度会随着时间累积漂移，因此需要对尺度也进行优化。相似变换矩阵7个自由度：3个旋转，3个平移，一个尺度因子。
在进行闭环时，需要对KiK_iKi及其找到的回环关键帧KlK_lKl进行相似变换进行解算。
遍历所有回环候选关键帧Kl∈KlK_l\in\mathcal{K}_lKl∈Kl，通过ORB描述子寻找KiK_iKi和KlK_lKl中的匹配点。从而可得到3D-3D的位姿求解问题，当解算得到的相似变换矩阵SilS_{il}Sil能够获得足够的内点时，进行局部优化，当内点足够，则认为找到回环KlK_lKl。

C.回环融合
将SilS_{il}Sil传播到局部关键帧组上及对应的地图点上，将KlK_lKl及其共视关键帧的地图点都映射到KiK_iKi，在映射点坐标的附近(小范围)寻找对应的匹配点，寻找到匹配点的即为内点，进行正方性坐标融合Ki⇋KlK_i\leftrightharpoons K_lKi⇋Kl。最后更新共视图，并连接KiK_iKi和KlK_lKl。

D. 本质图优化
最后在本质图的基础上进行优化。

3. ORB实现过程：

代码中由System构造SLAM系统，包括各个线程及其他要素，如配置文件，数据库及Viewer等。

3.1 System SLAM构造

src/System.cc System::System

// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

1设置配置文件：fsSettings()
2创建字典：ORBVocabulary()；加载字典loadFromTextFile(); 根据字典建立数据库KeyFrameDatabase()；
3创建地图：Map()
4创建并设置各种Viewer
5创建Tracker，LocalMapper和LoopCloser；创建开启LocalMapping与LoopClosing线程；
mpTracker = new Tracking…
mpLocalMapper = new LocalMapping…
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper);
mpLoopCloser = new LoopClosing…
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);6设置三个主要线程的指针
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);

3.2 Tracking线程

小模式：这里指只开启定位不进行建图。标志为mbActivateLocalizationMode

文件Examples/Monocular/mono_tum.cc

主线程，主要函数：SLAM.TrackMonocular(): >接收到新的图像后，
1 首先检查是否为小模式，是则关掉LocalMapping，开启OnlyTracking；否则释放地图关键帧，并关闭OnlyTracking。
2 调用GrabImageMonocular解析得到当前相机位姿估计Tcw。>GrabImageMonocular()子函数：
1 转化彩色图为灰度图；
2 创建Frame：设置金字塔，提取特征点，初始化地图点，分散特征点到网格等
3 跟踪Track()得到相机位姿估计Tcw;

Track() in GrabImageMonocular():
三种定位模型：TrackReferenceKeyFrame()、TrackWithMotionModel()、Relocalization()1 检查是否初始化，未初始化NOT_INITIALIZED则初始化MonocularInitialization()；若已经初始化，开始跟踪。
2 首先检查是否是小模式(OnlyTracking)
//~~若OnlyTracking，只定位;若非OnlyTracking，则：~~
3 若已跟踪丢失，则需Relocalization()进行重定位，若跟踪正常，则使用模型及参考帧进行跟踪计算。若刚初始化无速度估计或者刚进行重定位，则使用TrackReferenceKeyFrame()估计位姿；若已有速度估计，则使用TrackWithMotionModel()估计位姿，估计失败，则使用TrackReferenceKeyFrame()估计位姿；
4 设置参考关键帧；
5 跟踪成功使用地图进行位姿的优化TrackLocalMap()
6 设置是否跟踪成功，当不成功，则下一帧图像需要重定位Relocalization()来进行位姿的粗略估计。if(bOK)mState = OK;elsemState=LOST;
7 更新viewer显示；
8 跟踪成功，则更新运动模型Velocity，并更新相机位姿显示，不成功，则Velocity设为空，则下一帧图像需要使用TrackReferenceKeyFrame()估计位姿。
9 if(NeedNewKeyFrame())CreateNewKeyFrame();
10 清除外点；
11 如果跟踪失败，且地图关键帧数据太少，则系统重置System->Reset()；
12 将当前Frame设为下一帧的LastFrame。

MonocularInitialization() in Track():
并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云得到初始两帧的匹配、相对运动、初始MapPoints
判断是否有初始化器mpInitializer，
Step 1：（未创建）得到用于初始化的第一帧，初始化需要两帧
将当前帧(CurrentFrame)作为初始化帧mInitialFrame，并创建初始化器mpInitializer。
Step 2：（已创建）如果当前帧特征点数大于100，则得到用于单目初始化的第二帧，当满足条件，则可进行特征点匹配
Step 3：在mInitialFrame与mCurrentFrame中找匹配的特征点对
matcher.SearchForInitialization()
Step 4：如果初始化的两帧之间的匹配点太少，删除初始化器mpInitializer，重新初始化，若寻找到足够的匹配点，则进行初始化操作。
初始化操作包括第二帧的位姿估计，及三角化特征点(只有单目需要此操作)
Step 5：初始化操作Initializer->Initialize()：通过H模型或F模型进行单目初始化，得到两帧间相对运动、三角化得到特征点深度。
Step 6：删除那些无法进行三角化的匹配点
Step 7：设置世界坐标零点为相机初始化点，即相机初始化位姿为单位阵(无旋转，0偏移)
InitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F))；
Step 8：CreateInitialMapMonocular()地图进行初始化，将三角化得到的3D点包装成MapPoints添加至地图，将初始化的两帧图像都设为关键帧添加至地图。

Initializer->Initialize() in MonocularInitialization()：通过匹配特征点，使用H/F估计相机位姿，并三角化得到特征点深度threadH(&Initializer::FindHomography,…ref(SH))，计算H并得到得分SH
threadF(&Initializer::FindFundamental,…ref(SF)) ，计算F并得到得分SF
计算得分比值：
RH = SH/(SH+SF);
当RH>0.4 则使用H （ReconstructH）得到Rt及三角化得到特征点深度，否则使用F（ReconstructF）计算Rt及特征点深度。

CreateInitialMapMonocular()in MonocularInitialization()：将初始化的两帧及三角化的地图点添加到地图，并更新共视图关系。
1 为初始化的两帧创建关键帧KeyFrame()，并添加到地图pMap->AddKeyFrame();
2 更新关键帧及地图点对应关系：
KF*->AddMapPoint();为关键帧添加观测到的MapPoint
MP->AddObservation()；为地图点添加观测到该地图点的关键帧；
3 计算特征点的最佳描述子、平均观测方向及观测距离
pMP->ComputeDistinctiveDescriptors();
pMP->UpdateNormalAndDepth();
pMap->AddMapPoint(pMP)
4 更新连接关系，以下函数用来更新统计共视关系
pKFini->UpdateConnections()；
5 进行一次全局BA优化，同时优化所有位姿和三维点
Optimizer::GlobalBundleAdjustemnt(mpMap,20);
6 将关键帧及地图点插入地图
mpMap->AddKeyFrame(pKFini);
mpMap->AddKeyFrame(pKFcur);

TrackLocalMap() in Track()：找到局部关键帧及地图点，并对当前帧相机位姿估计进行优化。
1 --
UpdateLocalMap();
找到一二级共视关键帧及一级共视关键帧的父子关键帧作为局部关键帧，将关键帧中的观测到的点作为局部关键点。
一级共视关键帧：与当前帧有相同共视点的关键帧
二级共视关键帧：与一级共视关键帧有相同共视点的关键帧
2 --
SearchLocalPoints();在局部地图点中寻找匹配点，并添加及投影在当前帧相机视野中的当前帧相机视野中的局部地图点
3 --
Optimizer::PoseOptimization(&mCurrentFrame);优化位姿

NeedNewKeyFrame() in Track()：
很长时间没有插入关键帧、局部地图空闲、跟踪快要跪 跟踪地图MapPoints的比例比较少

Local mapping线程

线程被创建："src/System.cc" System::System()中

mpLocalMapper = new LocalMapping…
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper);

入口函数：

void LocalMapping::Run() {//设置当前LocalMapping线程处于建图状态,不接受Tracking线程再传来的关键帧SetAcceptKeyFrames(false);// step1. 检查缓冲队列内的关键帧 CheckNewKeyFrames()// step2. 处理缓冲队列中第一个关键帧ProcessNewKeyFrame();// step3. 剔除劣质地图点MapPointCulling();// step4. 创建新地图点CreateNewMapPoints();// step5. 将当前关键帧与其共视关键帧地图点融合SearchInNeighbors();// step6. 局部BA优化: 优化局部地图Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame, &mbAbortBA, mpMap);// step7. 剔除冗余关键帧KeyFrameCulling();// step8. 将当前关键帧加入闭环检测中mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);//设置当前LocalMapping线程处于空闲状态,接受Tracking线程传来的关键帧SetAcceptKeyFrames(true)；
}

ProcessNewKeyFrame() in LocalMapping::Run()：
1 计算新到关键帧的词向量
mpCurrentKeyFrame->ComputeBoW();
2 判断地图点是否是新生成//若不是当前关键帧生成的地图点，表示是匹配得到的，则对当前关键帧增加该地图点的观测，同步对地图点的信息(平均观测向量，最佳描述子)进行更新：pMP->AddObservation(mpCurrentKeyFrame, i);pMP->UpdateNormalAndDepth();pMP->ComputeDistinctiveDescriptors();//若是本关键帧生成的(通过之前CreateNewMapPoints三角化生成的)，则添加到待筛选地图点mlpRecentAddedMapPoints中，等待后期MapPointCulling函数的检验。//mlpRecentAddedMapPoints只存放待检验的点，所有经过检验的地图点都会从队列中删除，无论好坏，坏点会在删除时设置坏点标记
mlpRecentAddedMapPoints.push_back(pMP)；
3 更新共视图
mpCurrentKeyFrame->UpdateConnections();
4 将关键帧插入到地图中
mpMap->AddKeyFrame(mpCurrentKeyFrame);

MapPointCulling() in LocalMapping::Run()：筛选mlpRecentAddedMapPoints中地图点，并对应设置坏点标志
1 已在前期程序中确定为坏点，直接从队列删除：pMP->isBad()
2 实际观测到该地图点的帧数/理论上观测到该点的帧数<0.25，设置坏点标志，并从队列删除pMP->GetFoundRatio() < 0.25f
3 当地图点被2次以上观测（地图点从创建帧到当前观测帧已经经历了2帧以上）&&观测到该点的帧数< cnThObs次，第一个条件不满足，则直接不进行第二个条件计算。设置坏点标志，并从队列删除4 经过3帧检验，则认为是好点，不设坏点标志，也从队列中删除。(nCurrentKFid - pMP->mnFirstKFid) >= 2 && pMP->Observations() <= cnThObs

CreateNewMapPoints() in LocalMapping::Run()：
1 找到共视程度最高的nn(单目20/其他10)帧关键帧mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn)
2 取得当前关键帧的相机作为及内参Rcw1 = mpCurrentKeyFrame->GetRotation()；tcw1 = mpCurrentKeyFrame->GetTranslation();
......
3 遍历上述的共视关键帧，搜索当前关键帧与共视关键帧的匹配特征点并用极线约束剔除误匹配，最终三角化
4 计算当前帧与共视帧的F矩阵ComputeF12(mpCurrentKeyFrame,pKF2)
5 利用F矩阵生成新的匹配点对matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices…);
6 根据匹配关系计算地图3D坐标，并创建地图点，为地图点设置属性(更新观测关系，更新秒速子、观测方向和深度等)，并添加到地图中MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
7 将新生成的地图点添加到待筛选队列中，待后续MapPointCulling()检验mlpRecentAddedMapPoints.push_back(pMP);
8 检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点SearchInNeighbors()

SearchInNeighbors() in CreateNewMapPoints()：
1 取当前关键帧的一级共视关键帧vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(10);
2 遍历一级关键帧,寻找二级关键帧for (KeyFrame *pKFi : vpNeighKFs)：vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
3 正向融合: 将当前帧的地图点融合到各共视关键帧中matcher.Fuse(pKFi, vpMapPointMatches);
4 反向融合: 将各共视关键帧的地图点融合到当前关键帧中matcher.Fuse(mpCurrentKeyFrame, vpFuseCandidates);
5 更新当前关键帧的地图点信息for (MapPoint *pMP : vpMapPointMatches): pMP->ComputeDistinctiveDescriptors();pMP->UpdateNormalAndDepth();
6 更新共视图mpCurrentKeyFrame->UpdateConnections();

KeyFrameCulling() in LocalMapping::Run():剔除冗余关键帧
//遍历当前关键帧的所有共视关键帧
记录该共视关键帧的地图点被超过3个其他关键帧(在相同或者更优层)观测到的比例，>90%则将该共视关键帧中删除。

3.4 loop closing线程

线程被创建："src/System.cc" System::System()中

mpLoopCloser = new LoopClosing…
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);

函数入口：

void LoopClosing::Run() {//查询是否有新的关键帧CheckNewKeyFrames()//检测回环，获得回环候选关键帧DetectLoop()//计算回环候选关键帧相似矩阵，并得到最佳回环关键帧ComputeSim3()//回环融合(关键点融合，共视图及生成树更新，本质图优化)CorrectLoop();
}

闭环检测: 若连续4个关键帧都能在数据库中找到对应的闭环匹配关键帧组,且这些闭环匹配关键帧组间是连续的,则认为实现闭环，并更新观测关系。DetectLoop() in LoopClosing::Run()：
1 设置回环检测关键帧不被Local Mapping筛除mpCurrentKF->SetNotErase();
2 检测距离上次回环的时间，太短，不检测回环if (mpCurrentKF->mnId < mLastLoopKFid + 10)
3 检测当前关键帧与共视关键帧的BOW相似度得分。记录最小得分。mpORBVocabulary->score(CurrentBowVec, BowVec);
4 获取回环候选关键帧DetectLoopCandidates(mpCurrentKF, minScore);
5 筛选闭环候选关键帧：连续帧匹配到回环连续帧

ComputeSim3() in LoopClosing::Run()：//创建求解器pSolver = new Sim3Solver();//Sim3迭代求解pSolver->iterate();//根据解值搜索更多更优匹配点matcher.SearchBySim3();//优化Sim3，并根据内点数判断是否回环成功nInliers = Optimizer::OptimizeSim3()

以上图片来自https://blog.csdn.net/ncepu_chen/category_9874746.html

CorrectLoop() in LoopClosing::Run()：
0 更新当前关键帧组与地图点的连接mpCurrentKF->UpdateConnections();
1 将Sim3位姿传播到局部地图点上cvCorrectedP3Dw = Converter::toCvMat();pMPi->SetWorldPos(cvCorrectedP3Dw);
2 将Sim3位姿传播到局部关键帧组中cv::Mat correctedTiw = Converter::toCvSE3();pKFi->SetPose(correctedTiw);
3 更新共视图pKFi->UpdateConnections();
4 将闭环关键帧组地图点融合到当前关键帧上mpCurrentKF->AddMapPoint(pLoopMP, i);pLoopMP->AddObservation(mpCurrentKF, i);
5 将闭环关键帧组地图点融合到局部关键帧上SearchAndFuse(CorrectedSim3);
6 本质图BA优化…Optimizer::OptimizeEssentialGraph();
7 全局优化(2版本才有)thread(&LoopClosing::RunGlobalBundleAdjustment, …);

参考链接

[1] Mur-Artal R , Montiel J M M , Tardos J D . ORB-SLAM: A Versatile and Accurate Monocular SLAM System[J]. IEEE Transactions on Robotics, 2015, 31(5):1147-1163.
[2]https://blog.csdn.net/ncepu_chen/category_9874746.html
[3]https://github.com/electech6/ORB_SLAM2_detailed_comments.git