ORB-SLAM2

初始化过程

系统启动初始化

加载字典
初始化并启动 Tracking() 追踪线程
Tracking() 构造函数中加载相机参数、新建左右相机+初始帧的ORB特征点提取器
启动线程：launches the Local Mapping, Loop Closing and Viewer threads.

传入图像

传图像的for()循环中，只有TrackMonocular()
如果跟踪的时间比采集图像的时间小，则需要等待

初始化帧

（即，如果是第一次运行或者刚复位过的状态，则需要提取的特征点个数是2倍的指定特征点数目）

单目帧构造函数

提取特征点
矫正特征点
分配特征点

// Step 2 获取图像金字塔相关参数
// Step 3 对这个单目图像进行ORB提取特征点
// Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正
// Step 5 计算去畸变后图像边界（通过对图像四个角矫正后得到边界），将特征点分配到网格中

仿函数计算 ORB

// Step 2 构建图像金字塔，且金字塔每层图像的四周都做了19pixel的扩充
// Step 3 计算每一层图像的fast角点，并且将fast角点以四叉树筛选并分发进行均匀化
// Step 5 对每一层图像进行高斯模糊：计算描述子的时候，为了避免图像噪声的影响，使用了高斯模糊
// Step 6 计算高斯模糊后图像的描述子
// Step 7 将所有层特征点坐标统一到第0层图像（原始图像）的坐标系下

特征点数量的分配

总特征点数目根据面积比例（orb中按照面积的开方）均摊到每层图像上。金字塔缩放比例1.2，共8层。

计算金字塔

resize()以1.2倍构建8层金字塔
每层图像都做了四周19pixel的扩充（保证可以在图像边界像素上提取Fast角点）

ComputeKeyPointsOctTree

求金字塔中所有层的FAST特征点；以四叉树筛选并分发特征点；计算特征点的方向

• step1 划分出每一层的cell（30x30）区域
• step2对每个网格提取FAST关键点，先用初始阈值iniThFAST提取，若提取不到关键点，则改用最小阈值minThFAST提取。
• step3 每个四叉树节点区域中只有一个响应值最大的特征点，剔除冗余特征点
• step5 计算FAST特征点的方向

四叉树均匀化

// Step 1 计算四叉树初始节点个数：根据宽高比确定初始节点数目，一般是1或者2
// Step 3 根据每一个特征点的横轴位置，将其分配给其所属的那个图像节点区域的提取器节点（初始提取器节点）
// Step 4 对初始节点容器，标记不可再分裂的节点（即，此节点区域上keypoint个数为1），删除那些没有分配到特征点的节点
// Step 5 根据兴趣点分布,利用4叉树方法对图像进行划分区域
// Step 6 当再划分之后所有的Node数大于要求数目时,就慢慢划分直到使其刚刚达到或者超过要求的特征点个数
// Step 7 保留每个四叉树节点区域中响应值最大的 keypoint

计算FAST方向

加速方式：
除了中间行，对特征点的上下对称的点的方向同时计算（只遍历特征点上方的行，下方的行借助“-”号即可实现）
还使用了fastAtan2()函数（比atan2快3倍以上）

描述子

计算的时候需要将这里选取的随机点点集的x轴方向旋转到特征点的方向。将此点的坐标系从原来的图像坐标系变换到灰度质心得到的坐标系
半径 HALF_PATCH_SIZE = 15；
ceil() floor() 操作会导致圆不对称性，所以只计算1/4圆区域，然后对称过去。

计算去畸变后图像的边界

只在第一帧计算
通过矫正四个边界点得到

特征点分配到图像网格中

如果去畸变特征点在被切割过的图像边界中，则将其放在对应网格中

跟踪当前帧

Track()

单目的地图初始化

使用 F、H矩阵，得到初始两帧的匹配点、相对姿态、匹配点对应的MapPoints、第一帧作为世界坐标系

// Step 1 如果单目初始器还没有被创建，则创建（创建条件为：mCurrentFrame的特征点数 > 100）。如果后面帧判断需要重新初始化，则重新初始化
// Step 2 如果当前帧（系统运行阶段中的某一帧）特征点数太少（不超过100），则需重新构造初始器
// Step 3 在初始化时创建的与当前传入的图像帧中找匹配的特征点对，如果初始化的两帧之间的匹配对个数太少（ <100 ），则认为初始化失败，需要重新初始化
// Step 5 计算H、F矩阵，通过H模型或F模型进行单目初始化，得到两帧间相对运动
// Step 6 初始化成功后，从匹配内点中删除那些 有匹配关系但无法三角化 的匹配点
// Step 7 将初始化的第一帧作为世界坐标系
// Step 8 创建初始化地图点MapPoints

单目初始器？Initializer(mCurrentFrame,1.0,200);

// 从当前帧中获取相机的内参数矩阵、去畸变后的特征点
// 设定估计误差 sigma:1.0
// RANSAC迭代次数 200

求单目初始化两帧的匹配点

// Step 1 构建两图像中成功匹配的一对特征点间的旋转角度直方图(30个矩阵块，每个块最大高度=500)
// Step 2 根据F1中的各个特征点坐标在F2中对应的特征点的窗口半径内搜索候选匹配特征点
// Step 3 遍历搜索窗口中所有的候选匹配点，找到最优和次优匹配点（依据特征点之间的汉明距离）
// Step 4 进一步确定候选特征点是否为正确的匹配点：满足最优/次优比例阈值、删除重复匹多次的点
// Step 5 依据 F1中的特征点 和 F2中最匹配的特征点 之间的旋转角度差，将其分配到所在的直方图中
// Step 6 根据旋转直方图，剔除整张图像中“非主流旋转方向”的匹配点对
// Step 7 保存F2图像中最后通过严格筛选的特征点（与F1中特征点匹配的特征点）

候选匹配特征点

获取 以当前特征点为中心，以r为半径的圆形内在F1参考帧的特征点所属的金字塔层找当前帧特征点的候选点

// Step 1 计算半径为r=100pixel圆搜索区域的左右上下边界所在的网格的列和行的id（网格查找法的加速优势：为空的网格可直接跳过，不用再遍历内部所有像素来实现加速）
// Step 2 遍历圆形区域内的所有网格，寻找满足条件的候选特征点

直方图剔除非主特征点

筛选出直方图块中最高的三个块中的特征点（即，旋转角度差分布最多的三个直方图块），其他块中的特征点都认为是是误匹配，需要剔除
如果 最多、次多、次次多 的三个直方图块相差太大，则只存储最多的块索引

初始化两帧间的位姿

计算H、F，用质量最佳的来恢复出最开始两帧之间的相对姿态，并进行三角化得到初始地图点

// Step 2 在所有匹配特征点对中随机选择8对匹配特征点，用于估计H、F矩阵
// Step 3 分别用两个线程计算H、F矩阵
// Step 4 根据H、F矩阵得分比例来判断选取哪个来求位姿R,t
// Step 5 根据H、F矩阵来求位姿R,t（偏向于用H）

H、F选择

FindHomography()

迭代获取最优的单应性矩阵，并返回这个矩阵的得分

// Step 1 将当前帧和参考帧中的特征点坐标进行归一化
// 进行RANSAC迭代200次
// Step 2 每次随机选择8个归一化之后的点对使用八点法用SVD方法计算单应矩阵
// Step 4 利用特征点对的双向重投影误差为当次RANSAC的单应矩阵结果评分（图像中N个特征点中所有内点之于 H 矩阵的分数总和）
// Step 5 进行多次迭代求不同的H，取出得分最高的那次的H

八点法+SVD 求解H

（4对点就够了，但为了统一，使用8对点）

构造用于计算的矩阵 A： AX=0
使用opencv提供的奇异值分解函数
右奇异值矩阵的最后一行即为最优解（第9个奇异向量）

为200次的H评分

依据卡方检验剔除外点；借助特征点对的双向重投影误差对给定的 H 矩阵（由随机选择的8对点得到）打分

// Step 2.1 提取参考帧和当前帧之间的特征匹配点对
// Step 2.2 通过H矩阵计算 img2 到 img1 的重投影误差；用卡方检验值作为判断阈值，满足阈值的话则累加得分
// Step 2.3 通过H矩阵计算 img1 到 img2 的重投影误差；用卡方检验值作为判断阈值，满足阈值的话则累加得分
// Step 2.4 如果 从img2到img1 和 从img1到img2 的重投影误差均满足卡方阈值要求，则说明是 Inlier point
// Step 3 返回图像中N个特征点中所有内点之于 H 矩阵的分数（卡方阈值-各点加权误差）总和

FindFundamental()

与 FindHomography() 类似
区别：基础矩阵F的秩为2,而第一次SVD结果不一定秩为2,所以需要再次SVD分解,再将分解结果的第三行强制置0

F解算R、t

// Step 2 根据F和相机的内参计算E
// Step 3 SVD分解E矩阵得到四组R,t
// Step 4.1 使用匹配点分别检查H、F四个R、t解
// Step 4.3 四个R、t中，最多可三角化点数要不小于：0.9倍的匹配内点数、50；次多的3D点个数不能大于0.7倍最多的
// Step 4.5 满足：有效重建点数最多；有效3D点中，最大的三角化视差角大于阈值(角度越大3D点越稳定)
// 确定R、t

检查H、F四个R、t解

用R，t 对特征匹配点三角化，得到3D点中有效的3D点个数

// Step 1 计算空间点到两个相机的投影矩阵
// Step 2 遍历所有特征点对，进行三角化，得到三角化测量之后的3D点坐标
// Step 3 第一关：检查三角化的三维点坐标是否为非无穷值
// Step 4 第二关：通过三维点深度值正负、两相机光心视差角大小来检查是否合法
// Step 5 第三关：反过来计算空间点在参考帧、当前帧上的两个重投影误差，如果大于阈值则舍弃
// Step 6 统计检验合格的3D点个数，记录这些3D点在两帧图像中的视差角
// Step 7 取角度排名50的那个较小的视差角，并且转换成角度制

DecomposeE

三角测量

给定空间点到平面的投影矩阵P1,P2和图像上的匹配特征点kp1,kp2，计算三维点坐标

创建初始化地图点

单目相机成功初始化后用三角化得到的点生成MapPoints

// Step 1 将初始关键帧、当前关键帧的描述子转为BoW，为了后期匹配做加速计算
// Step 2 将关键帧插入到地图（认为单目初始化时候的参考帧和当前帧都是关键帧）
// Step 3 用初始化得到的3D点来生成地图点MapPoints
// Step 4 全局BA优化，同时优化所有位姿和三维点
// Step 5 取场景的中值深度，用于单目尺度归一化
// Step 6 将两帧之间的变换归一化到平均深度1的尺度下
// Step 7 把3D点的尺度也归一化到1
// Step 8 将关键帧插入局部地图，更新归一化后的位姿、局部地图点

为地图点添加描述子

由于一个3D的MapPoint会被许多相机观测到，则挑选最合适的那个描述子来表示这个点：计算当前点所有描述子之间的两两距离，最好的那个描述子与其他描述子应该具有最小的距离中值

更新地图点的平均观测方向

对所有能观测到地图点的相机，从这些相机光心分别做到该地图点的向量（归一化为单位向量），然后将这些向量相加得到该地图点的朝向

关键帧间连接关系（更新图的连接）

// Step 1 统计当前帧中，有多少其他关键帧与当前帧存在共视关系
// Step 2 找到共视权重最大的关键帧、共视权重大于阈值15的所有关键帧
// Step 3 如果没有满足阈值15的关键帧可连接，则连接权重最大的那个关键帧
// Step 4 建立 共视程度满足阈值的关键帧对 的连接关系并保存其权重
// Step 5 更新树的连接关系，确定关键帧间的父子关系（当前关键帧的父关键帧为共视程度最高的那个关键帧）

全局BA同时优化地图中所有位姿和三维点

两个地方使用：

• 1、单目初始化：CreateInitialMapMonocular函数
• 2、闭环优化：RunGlobalBundleAdjustment函数

跟踪线程

替换上一帧地图点？

局部建图线程可能会对上一帧的地图点进行替换.在这里检查是否需要替换，如果需要则替换掉

参考关键帧跟踪模式

如果上一帧到当前帧的变换为NULL（说明是刚初始化开始、恒速模型跟踪已经跟丢了）、刚完成重定位 则使用参考关键帧来恢复位姿：对参考关键帧的MapPoints进行跟踪（关键帧模式），得到当前帧的位姿。（一般不用）

// Step 1：将当前帧的所有描述子转化为BoW向量
// Step 2：通过特征点的BoW加快当前帧与参考关键帧之间的特征点匹配（要求匹配数>=15）
// Step 3: 将上一普通帧的位姿作为当前帧位姿的初始值，优化时收敛快一些；存储与关键帧匹配的地图点
// Step 4: 使用g2o优化3D-2D的重投影误差来获得当前帧的位姿（只对当前的位姿做优化，没有对3D点优化）
// Step 5：剔除BA优化后的outlier匹配点MapPoints（优化的过程中标记出了外点）

返回值：return nmatchesMap>=10; // 成功匹配的点数需要 >10

PoseOptimization？

这里只对位姿优化；主要用于Tracking线程中：运动跟踪、参考帧跟踪、地图跟踪、重定位

总共优化四次，每次优化迭代10次；每次优化后，将观测分为outlier和inlier，outlier不参与下次优化

//最后得到优化后的位姿
// 返回内点数=总个数-外点数

恒速模型

（大部分时间用这个跟踪）使用恒速度模型（假设相邻帧间物体匀速）对上一帧的地图点进行跟踪，优化当前帧位姿；

// Step 1：依据上一帧的参考帧更新上一帧的位姿；根据恒速模型求当前帧初始位姿；如果是双目或rgbd摄像头，还会生成一些临时地图点
// Step 2：根据上一帧地图点对当前帧进行投影匹配
// Step 3：使用这些匹配点优化当前帧位姿（较为粗糙）
// Step 4：剔除优化过程中判定为外点的那些地图点
// Step 5：最终得到的有效匹配数超过10个点就认为成功

重定位模式

上面两个都没有跟踪成功导致位置丢失时，利用其相似候选帧找到最近的关键进而实现重定位；因为没有好的初始位姿信息，所有要用EPNP解算出初始位姿供G2O使用

// Step 1： 计算当前帧特征点的Bow
// Step 2：找到与当前帧相似的候选关键帧组(当前帧 的共视关键帧 的前十个共视关键帧组中，最强的那个关键帧 组成的集合)
// Step 3：遍历所有的候选关键帧，借助BoW进行快速匹配得到匹配点，用匹配结果初始化 PnP Solver(每个关候选键帧有一个）
// Step 4: 通过下面几个Step步骤处理,直到找到能够匹配上的关键帧
// Step 4.1：通过EPnP算法估计姿态，迭代5次，如果计算出了位姿，则对内点进行BA优化
// Step 4.3：如果优化后剩下的内点数较少<50，则通过空间点投影的方式对之前未匹配的点再尝试进行匹配以得到更多的点，再对位姿优化
// Step 4.4：如果4.3的BA后剩余内点数还是比较少(<50)但是还不至于太少(>30)，再用 更小的窗口搜索、更严格的阈值 重新生成匹配关系并尝试挽救一下，如果挽救成功则在优化
//step5 如果到当前遍历的这个候选关键帧时，已经有足够多(50个)的内点了,那么就认为重定位成功，直接跳出（不再遍历其他的候选关键帧了）

重定位候选关键帧

在重定位中找到与该帧相似的候选关键帧（从所有关键帧中查找）

// Step 1：找出当前帧的共视关键帧 作为初步的重定位候选帧
// Step 2：找出所有重定位候选帧中与当前帧F具有共同单词最多的那个帧，并得到这个帧与F所共有的单词数（后面用来决定相对阈值：0.8倍）
// Step 3：遍历所有重定位候选帧，挑选出共有单词数大于阈值minCommonWords的关键帧，并计算这些帧与当前帧F的相似度
// Step 4：计算这些候选帧的 每个候选帧的共视关键帧组(组中的关键帧必须也要能观测到当前帧的地图点) 的总得分，并得到分数最高的那个组的得分，再以此计算阈值 minScoreToRetainminScoreToRetain：0.75
// Step 5：得到所有 组内总得分大于阈值的那些组的组内得分最高的那个关键帧，作为最终的候选关键帧组

词袋匹配 跟踪SearchByBoW

重投影匹配

// Step 1 建立旋转直方图，用于检测旋转一致性
// Step 2 遍历关键帧中的每个地图点，通过相机投影模型，得到投影到当前帧的像素坐标
// Step 3 在100pixel的候选圆形区域中搜索候选匹配点
// Step 4 遍历候选匹配点，计算描述子的汉明距离，寻找最小的最佳匹配点
// Step 5 计算得到的所有匹配点对的旋转角度，并归类到对应直方图块中
// Step 6 进行旋转一致检测，剔除整张图中不一致的匹配点

跟踪局部地图

在跟踪得到当前帧初始姿态后，对local map跟踪得到更多的匹配，并优化当前位姿。
对Local Map的MapPoints进行跟踪，如果没有这个函数，只用前面的三个跟踪，非常容易跟丢

// Step 1：更新局部地图（局部关键帧和局部地图点）
// Step 2：在局部地图中找出在当前帧中存在的MapPoints, 其实也就是借助局部地图点实现当前帧在局部地图中的跟踪
// Step 3：更新局部地图点后对位姿再次g2o优化（当前帧T为定点、重投影误差为边）
// Step 4：更新当前帧中所有的MapPoints被观测次数，并统计跟踪到的局部地图点数
// Step 5：根据跟踪匹配数目决定是否跟踪成功

• 如果最近发生了重定位,那么至少成功匹配50个点才认为是成功跟踪
• 如果是正常的状态的话只要跟踪的地图点大于30个就认为成功了

局部关键帧

// Step 1：遍历当前帧的MapPoints，得到所有能观测到这些MapPoints的关键帧和对应权重（共视关键帧）
// Step 2：更新局部关键帧：step1得到的共视关键帧、各个共视关键帧的前十个共视关键帧、共视关键帧的父、子关键帧
// Step 3：更新当前帧的参考关键帧 为 那个与当前帧共视程度最高的关键帧

局部地图点

先把局部地图清空，然后将所有局部关键帧的有效地图点添加到局部地图中

局部地图点跟踪时的地图点有效性判断

// Step1 获得所有局部地图点的世界坐标
// Step 2 关卡一：检查这个地图点在当前帧的相机坐标系下，如果是负的深度，表示出错
// Step 3 关卡二：将MapPoint投影到当前帧的像素坐标(u,v), 并判断是否在图像有效边界范围内
// Step 4 关卡三：计算MapPoint到相机中心的距离, 并判断是否在尺度变化距离内[minDistance maxDistanc]
// Step 5 关卡四：计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值, 若小于设定阈值，返回false
// Step 6 根据地图点到光心的距离来预测地图点在这一帧中的尺度层级：ln(d_max/d/scale)

判断是否需要插入关键帧

// Step 1：纯VO模式下不插入关键帧：纯VO模式是不做局部建图的线程，所以就不插入关键帧
// Step 2：如果局部地图线程正在被闭环检测使用，则不插入关键帧
// Step 3：在系统采集完30个关键帧后（1s采集30个普通帧），如果当前帧与上一次重定位帧比较近，则不插入关键帧
// Step 4：得到参考关键帧跟踪到的地图点数量（用来判断跟踪的质量：即判断是否移出了当前区域）
// Step 5：查询局部地图管理器是否繁忙,也就是当前局部建图线程是否允许关键帧输入（因为可能在做关键帧之间的BA时，会再生成新的地图点）
// Step 6：对于双目或RGBD摄像头，在有效深度值范围内的所有地图点中，统计跟踪到的地图点和未跟踪成功的点的数量
// Step 7：决策是否需要插入关键帧
// Step 7.2：已经有1s的时间没有插入关键帧了，可以插入
// Step 7.3：满足插入关键帧的最小间隔并且局部地图管理器处于空闲状态(当前局部建图线程允许关键帧输入)，可以插入
// Step 7.4：在双目、RGB-D时，当前帧跟踪到的点比参考关键帧的0.25倍还少，或者满足bNeedToInsertClose
// Step 7.5：和参考帧相比当前跟踪到的点太少 或者 满足bNeedToInsertClose；同时跟踪到的内点还不能太少
// Step 7.6：local mapping空闲时可以直接插入，否则中断 BA

创建新的关键帧和地图点

如果是双目、rgbd，则同时还要对不存在地图点的特征点生成临时的MapPoints

// Step 1：将当前帧构造成关键帧
// Step 2：将当前关键帧设置为当前帧的参考关键帧
// Step 3：对于双目或rgbd摄像头，为当前帧生成新的地图点；单目无操作
// Step 3.1：得到当前帧有深度值的特征点（不一定是地图点）
// Step 3.2：按照深度从小到大排序(双目或rgbd中，点越近，则深度值越准，所以优先生成近的地图点)
// Step 3.3：如果当前地图点为NULL 或者 地图点虽然被创建，但是没有相机观测到这个地图点,那么就生成一个临时的地图点
// Step 3.4：停止新建地图点必须同时满足以下条件：1、当前的点的深度已经超过了设定的深度阈值（35倍基线）2、nPoints已经超过100个点，说明距离比较远了，可能不准确 3、所有有深度值的特征点都遍历完毕
// Step 4：将当前帧插入局部地图中成为关键帧

跟踪

track包含两部分：估计运动、跟踪局部地图
流程：

1. 首先使用参考关键帧来跟踪，跟踪一帧之后就有速度信息，就用横速模型跟踪，若横速模型偶然失效，就用参考关键帧跟踪（用了上一个关键帧信息、前一普通帧的信息），
2. 如果上面都失败，就使用重定位（使用与它有公共单词的关键帧，范围很大，可能会离的有点远）；
3. 得到初始位姿后，跟踪局部地图产生更多的地图点，优化得到更精确的位姿；
4. 最后插入关键帧；

Local Mapping

局部建图

// Step 1 告诉Tracking线程，LocalMapping正处于繁忙状态，暂时不接受Tracking送过来的关键帧
// Step 2 处理缓冲列表中的关键帧：计算BoW、更新观测、描述子、共视图，插入到局部地图等
// Step 3 根据地图点的观测情况剔除质量不好的地图点
// Step 5 因为上面对地图点做了多次的剔除和新增，这里检查并融合当前关键帧与相邻关键帧（两级相邻）中重复的地图点
// Step 6 当局部地图中的关键帧大于2个的时候进行局部地图的BA（优化一级局部关键帧位姿+地图点；而二级局部关键帧没有优化，只用来做约束）
// Step 7 检测并剔除 当前帧相邻的关键帧中 冗余的关键帧（删除的是相邻关键帧）
// Step 8 将当前帧加入到闭环检测队列中
// 查看是否有复位线程的请求
// 可以重新接收来自Tracking送过来的关键帧了（在前面关掉了给局部建图线程送入关键帧的允许）

处理关键帧

// Step 1：从缓冲队列中取出一帧关键帧
// Step 2：计算该关键帧特征点的Bow信息
// Step 3：遍历当前处理的关键帧中有效的地图点，为当前地图点添加观测、更新平均观测方向和观测距离范围、地图点最佳描述子
// Step 4：更新关键帧间的连接关系
// Step 5：将该关键帧插入到地图中

剔除地图点

检查mlpRecentAddedMapPoints中新增的那些地图点，根据这些地图点的观测情况剔除质量不好的新增的地图点

// Step 1：根据相机类型设置不同的观测阈值
// Step 2：遍历检查新添加的那些MapPoints 来确定是否需要剔除
// Step 2.1：已经是坏点的MapPoints直接删除
// Step 2.2：如果 跟踪到该MapPoint的Frame数 与 预计应该可观测到该MapPoint的Frame数 的比例小于25%，则删除
// Step 2.3：从该点建立开始，到现在已经过2个关键帧了 且 观测到该点的关键帧数却不超过观测阈值数cnThObs，那么删除该点
// Step 2.4：从建立该地图点开始，已经过了3个关键帧仍没有被剔除，则认为是质量高的点

场景深度中值

因为基线的长度是否有效与场景地图点深度相关，所以这里借助关键帧所有地图点中 中值深度的那个地图点z值来判断 这两个用于创建新地图点的关键帧之间的距离（两相机基线）是否足够长
不够长的化，就不对这两关键帧三角化出地图点

三角化生成新地图点

用当前关键帧与相邻关键帧通过三角化产生新的地图点，使得跟踪更稳

// Step 1：在当前关键帧的那些共视关键帧中找到共视程度最高的nn帧相邻关键，并遍历
// Step 3：判断遍历到的共视关键帧和当前关键帧的基线距离（相机运动产生）是不是足够长
// Step 4：根据两个关键帧的位姿和内参K计算它们之间的基础矩阵F
// Step 5：通过BoW对两关键帧未匹配的特征点快速匹配，生成新的匹配点对，用极线约束抑制离群点
// Step 6.2：利用匹配点反投影得到视差角（因为三角化要求的角度 ∠(O1_P3d_O2) 太小则认为是不准的）
// Step 6.4：三角化恢复3D点
// Step 6.5：检测生成的3D点是否在相机前方
// Step 6.6：计算3D点在当前关键帧下的重投影误差，并卡方检验
// Step 6.7：检查两帧中特征点所在金字塔层的尺度连续性
// Step 6.8：三角化生成3D点成功，构造成MapPoint

BOW匹配新地图点、极线约束抑制离群点

// Step 1 计算关键帧KF1的相机光心在KF2图像平面的二维像素坐标作为极点e2（以相机KF2作为坐标系）
// Step 2 利用BoW找出属于同一节点下的那些特征点，计算相互汉明距离，以最小汉明距离为最佳匹配点
// Step 2.7 极点e2到F2中最佳匹配特征点的距离如果小于阈值th,认为kp2对应的MapPoint距离pKF1相机太近，跳过该匹配点对
// Step 2.8 计算F2中最佳匹配特征点到kp1对应在F2中的极线的距离是否小于阈值剔除
// Step 3 用旋转差直方图来筛掉错误匹配对

三级局部关键帧地图点融合

// Step 1：获得当前关键帧在共视图中权重排名前nn的邻接关键帧作为一级相邻关键帧
// Step 2：将一级相邻关键帧的前5个相邻帧作为二级相邻关键帧
// Step 3：将当前帧的地图点分别与一级二级相邻关键帧地图点进行融合 – 正向
// Step 4：将一级二级相邻关键帧地图点分别与当前关键帧地图点进行融合 – 反向
// Step 5：更新当前帧地图点的描述子、深度、观测主方向等属性
// Step 6：更新当前帧的MapPoints后更新共视图

两帧地图点融合

// Step 1 判断当前帧地图点的有效性，当前帧地图点有效才进行融合
// Step 2 得到地图点投影到当前关键帧上的图像坐标
// Step 3 判断地图点到关键帧相机光心距离是否在有效范围内
// Step 4 地图点的平均观测方向（正视程度）要小于60°（即，当前相机观测的这个点不能太偏了）
// Step 5 在待匹配帧的投影点附近搜索窗口内找到候选匹配点的索引
// Step 6 先对搜索窗口里所有的候选点卡方检验剔除大误差点，再找出最佳匹配点（即，描述子距离最小的点）
// Step 7 如果两个最佳匹配点各自都有有效地图点，选则被观测次数最多的那个地图点（越多越准确）作为最终地图点，来实现融合

局部地图BA LocalBundleAdjustment？

关键帧剔除

// Step 1：根据共视图提取当前关键帧的所有共视关键帧（一级）
// Step 2：提取每个一级共视关键帧的所有地图点
// Step 3：遍历该一级共视关键帧的所有帧的所有地图点并求和，在这些地图点中，求出有多少地图点能被其它至少3个关键帧（地图点在与一级关键帧的相同或者更低金字塔层级中）观测到
// Step 4：如果有90%以上的地图点能被三个以上关键帧观测到，则被判断为冗余的，则删除该关键帧（删除的是一级共视关键帧）

闭环队列插入关键帧

局部建图中剔除冗余关键帧后的所有关键帧都会加入
第0个关键帧不能够参与到回环检测的过程中,因为第0关键帧定义了整个地图的世界坐标系

闭环

闭环候选帧

// Step 1：找出和当前帧具有公共单词的所有关键帧（即 绿色的那些帧；不包括与当前帧连接的关键帧）
// Step 2：绿色的那些关键帧中，得到最多共视单词数，并用来计算相对阈值
// Step 3：遍历上述所有闭环候选帧，只保留 共有单词数大于阈值 且 单词匹配度大于minScore 的候选帧
// Step 4：遍历经过筛选后的绿色关键帧，对每个关键帧取出其前十个共视关键帧，分别计算这十个得分之和，取出最高组得分并用于计算阈值
// Step 5：只取组得分大于阈值的组，并将 这个组的10个关键帧中 分数最高的那个关键帧作为闭环候选关键帧

闭环检测

// Step 2：如果当前帧距离上次闭环没多久（小于10帧）、整个map地图中关键帧总数还没有10帧，则不进行闭环检测
// Step 3：遍历那些与当前关键帧有连接的所有关键帧（>15个共视地图点），计算当前关键帧与这些共视关键帧的bow相似度得分（回环条件：当前帧与相似帧的相似度得分 要比 当前帧与所有其共视关键帧的相似度 都要高）
// Step 4：在所有关键帧中找出与该关键帧可能存在闭环关系的关键帧作为闭环候选帧
// Step 5.1：遍历闭环候选关键帧
// Step 5.2：将 当前闭环候选关键帧 以及 那些与其相连的关键帧 构成一个“子候选组”
// Step 5.3：遍历前一次闭环检测到的“子连续组”，查看“子连续组”和“子候选组”中是否共同有同一关键帧
// Step 5.5：如果上面判定有连续（即 “子连续组”和“子候选组”有同一关键帧），则需要判断是否达到连续的条件（连续次数>=3）
// Step 5.6：用当前的连续组 更新 上一次连续组，准备检测下一轮连续性

Sim3计算

Sim3是为了借助求解：旋转（根据两个三角形各自的法向量求得） 、尺度因子（根据相似三角形面积得到）、平移（根据旋转和尺度把两三角形平行放置，通过计算距离得到） ，来解决两坐标系间的相似变换问题，只要能得到两个坐标系下3对匹配好的不共线的三维点的坐标，就能解出Sim3相似变换

• 优势：
  1、给定两个坐标系下的至少3个匹配三维点，只需一步即可求得变换关系，不需要迭代，速度很快。
  2、不是数值解，不需要像迭代方法那样需要找一个好的初始解。闭式解可以直接求得比较精确的结果：旋转、平移、尺度
• 为何用更多点对
  1、旋转的结果和选择点的顺序关系密切，以不同的点作为坐标系原点，得到的结果不同。
  2、数据会有误差，且能得到很多三维匹配点对，所以考虑定义一个误差，使用最小二乘法来得到更稳定、更精确的结果：即 寻找合适的尺度因子、旋转和平移，使得它在所有数据上的误差最小。
• 数学推导：
  
• 尺度s：方程可以分解以尺度s为自变量的一元二次方程，要使得该方程误差最小，尺度=-b/2a；虽然上面这个误差函数会导致尺度不稳定，即反过来变换的尺度不是倒数1/s，但是orb里面没有管。
• 旋转R：对D进行特征值分解，求得最大特征值对应的特征向量就是待求的用四元数表示的旋转
• 平移t：根据旋转 和尺度 计算平移t
  
• 迭代次数的估计
• Sim3的逆变换矩阵
  

Sim3计算匹配点对

// Step 2：找出之前已经使用SearchByBoW匹配的特征点，这些点不用再使用Sim3匹配了
// Step 3：通过Sim3变换，对 pKF1 中还未匹配的特征点，在 pKF2 中找出匹配特征的点（之前使用SearchByBoW进行特征点匹配后的漏匹配）
// Step 3.1：通过Sim变换，将pKF1的地图点投影到pKF2中的图像坐标
// Step 3.2：预测投影的点在图像金字塔哪一层（层数越高越模糊，稳定性越低），用来计算特征点搜索半径
// Step 3.3：在 搜索区域内所有候选匹配特征点 中找出最佳匹配点（Hamming distance）
// Step 4：通过Sim3变换，对 pKF1 中还未匹配的特征点，在 pKF2 中找出匹配特征的点
// Step 5： 一致性检查：如果camera1与camera2中匹配的两个特征点也是camera2向camera1匹配的特征点，则认为这对特征点是可靠的匹配

g2o优化Sim3 OptimizeSim3

Sim3重投影地图点得匹配点

根据Sim3变换，将闭环关键帧及其共视关键帧的所有地图点（不考虑当前KF已经匹配的地图点）投影到当前KF，得到新的匹配点对

// Step 1 分解Sim变换矩阵
// Step 2 遍历闭环关键帧及其共视关键帧的所有地图点（不考虑当前KF中已经匹配的地图点），并将地图点向当前关键帧投影
// Step 2.1 跳过坏点、当前KF已经匹配过的地图点
// Step 2.2 投影到当前KF的图像坐标并判断是否有效：在图像范围内、地图点到相机光心的距离是否合理、观察角度小于60°
// Step 2.3 找出搜索半径内的 所有候选匹配点中的 最佳匹配点
// step 3 如果最佳匹配点汉明距离小于50，则视为有效匹配点

闭环验证

闭环矫正