ORB2-SLAM 重点知识的梳理-CSDN博客

本文链接：https://blog.csdn.net/weixin_43391596/article/details/133091861

对ORB2-SLAM的知识进行梳理，记录学习笔记。

一、图像帧的预处理(Frame)

从相机接收到一帧一帧的图像，需要对源图像进行预处理，构建出一帧Frame，将图像转化成Frame类的图像数据结构，后续通过这个Frame类进行图像操作。
针对不同类型的相机，有不同的Frame构造，如单目、双目、RGBD相机都有与之对应的Frame构建函数。
特征点的提取(ExtractORB)

（1）图像金字塔(图像尺度不变性)

根据图像金字塔的层数、所需提取特征点的数量、金字塔的缩放尺度等重要参数进行图像金字塔
图像金字塔的层数：影响着图像金字塔的大小
所需提取特征点的数量：提取的特征点数量，根据金字塔的层数与缩放尺寸，计算出每一层图像所需要提取多少特征点。最后将所有金字塔提取到的特征点累加在一起，就是我们每张图片所需提取特征点的数量
金字塔的缩放尺度：根据缩放尺度，对图像进行缩放，并且通过缩放尺寸计算出当前金字塔层所需要提取的特征点数量

（2）Fast提取角点(光照不变性)

我们在做特征点提取时，为了将边缘框附近的特征点进行提取，会将原图上下左右进行3像素的扩充。后续我们在做高斯模糊时，也要对图像进行扩充，具体扩充多少，要看高斯核的大小尺寸

（3）灰度质心法(旋转不变性)

（4）四叉树均匀提取特征点

与八叉树类型，将一个图像看做成一个体素，刚开始是一个完整的图像，如果图像内的数量大于4个，则进行细分，将一个体素图像分成四个。以此类推，直到符合我们的停止条件：1、切分的的图像块大于等于我们所需要提取的特征点数量，2、图像块的内部没有特征点，才停止细分。
在划分好的体素图像块中，提取最好的特征点，这样我们就能获得均匀的图像特征点。
对比Opencv的图像提取，Opencv的图像提取，会根据特征点的相应值进行排序，提取响应值较高的特征点，这样发现，提取到的角点容易扎堆，不够均匀，这样对后续图像匹配不利。

（5）提取特征点的描述子信息

在这里进行高斯模糊，是为了剔除图像中的噪声点，从而提高描述子的准确性，而我们不在提取特征点之前使用高斯模式。是因为高斯模式会对消除图像边缘信息，从而影响我们提取特征点

（6）特征点去畸变

使用Opencv的去畸变函数，将所提取到的特征点进行去畸变

（8）将特征点进行网格划分

将图像进行网格划分，且根据图像大小切分成多个cell，记录特征点对于着哪个cell

二、地图初始化

单目相机与双目相机的初始化不同，因为单目相机需要前后两帧才能进行初始化，而双目、RGBD不需要前后两帧图像就能进行初始化。
地图的初始化是为了的到初始的地图点，我们ORB2-SLAM整个框架都围绕这地图点进行的。
单目情况下，仅凭单帧图像是无法生成地图点，需要前后两帧通过三角化才能得到地图点。而双目、RGBD只需要一帧图像就能生成地图点。

单目相机初始化(MonocularInitialization)

(1)前后两帧所提取的特征点要大于一定阈值

因为后续我们通过这两帧进行特征点匹配，所以提取到的特征点数量一定要多，这样我们能初始化的地图点也会多

(2)进行特征点匹配(SearchForInitialization)

已知当前帧和上一帧(参考帧)的特征点信息，进行特征点匹配。
这边不使用暴力匹配法，因为暴力匹配法太耗时了。这里直接输入参考帧的特征点坐标是因为两帧之间的运动相对较小，所以图像之间的变化不会差异很大，且不知道当前帧与之参考帧的变换矩阵，而后续一般都是通过变换矩阵输入投影到当前帧的坐标。
我们在图像预处理的时候已经将图像切分成个个cell，我们也知道特征点所对应的cell，以参考帧的特征点为坐标，输入的参数r为半径，在这个参考帧的特征点坐标为圆形，画一个圆，通过这个圆包含那些cell，获取这些cell的特征点，这些特征点就是我们的候选点(GetFeaturesInArea)
得到圆内的候选点后，对每个候选点再通过与参考帧的特征点进行描述子的计算，找到最优的和次优的特征点，对比最优和次优，达到一定阈值则认为是合格的匹配点

(3)已知匹配关系，计算单应矩阵(H)或者基本矩阵(F)(Initialize)

已知两帧特征点的匹配关系，也知道H矩阵需要4个点进行求解，F矩阵需要8个点进行求解。在所有匹配特征点对中随机选择8对匹配特征点为一组，用于估计H矩阵和F矩阵，用于后续计算得分时进行RANSAC迭代。选取的组数量与RANSAC迭代数量一致。如果RANSAC迭代20次，就会选取20组，每次选用不同的匹配点进行求解。
- H矩阵的求解原理

[图片]

一对点提供两个约束等式，单应矩阵H总共有9个元素，8个自由度（尺度等价性），所以需要4对点提供8个约束方程就可以求解。
- F矩阵的求解原理：

[图片]

一对点提供一个约束方程，基础矩阵F总共有9个元素，7个自由度（尺度等价性，秩为2），所以8对点提供8个约束方程就可以求解F

得分计算：
求解出来的H矩阵和F矩阵，通过重投影误差来当选评分(CheckHomography)，且对这些内点进行标记。

(4)通过单应矩阵(H)或者基本矩阵(F)得到两帧之间的相对位姿，进行三角化得到地图点

- 单应矩阵(H)：
- 单应矩阵能得到八组解，也就是能得到八个位姿，通过这八个位姿，通过三角化得到地图点，那个位姿得到的地图点多，就用使用那个位姿当做当前帧的位姿
基本矩阵(F)：
- 单应矩阵能得到四组解，也就是能得到四个位姿，通过这四个位姿，通过三角化得到地图点，那个位姿得到的地图点多，就用使用那个位姿当做当前帧的位姿，
- 三角化得到地图点：
  已知投影矩阵，匹配点信息，计算三维坐标

(5)将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵

将第一帧作为世界坐标系的原点。

(6)创建初始化地图点(CreateInitialMapMonocular)

将单目初始化成功的两帧当做关键帧，将关键帧插入到地图中，将关键帧的描述子转化成词袋bow，用于后续快速匹配。
用初始化得到的3D点来生成地图点MapPoints
全局BA优化，同时优化所有位姿和三维点,因为只有两帧，所以进行全局BA优化速度很快，这样得到更加准确的位姿和地图点
将关键帧插入局部地图，更新归一化后的位姿、局部地图点

双目、RGBD相机初始化(StereoInitialization)

双目和RGBD我们能直接获得匹配点的深度信息，所以可以直接生成地图点，进行地图的初始化
双目相机能直接通过左右目相机进行匹配，得到匹配关系，通过左右相机的基线，就能恢复深度信息，进而得到对应的三维点信息
RGBD能直接得到图像的深度信息，所以每个特征点都能获取到深度值，从而得到对应的三维点信息

地图点和特征点的区别？

地图点是三维点，来自真实世界的三维物体，有唯一的ID，不同帧里的特征点可能对应三维空间中同一个三维点。
特征点是二维点，是特征提取得到的图像上的像素点，特征点通过三角化可以变成三维空间的地图点。

三、跟踪线程(Tracking)

ORB-SLAM2跟踪部分主要包括两个阶段，第一个阶段包括三种跟踪方法：用参考关键帧来跟踪、恒速模型跟踪、重定位跟踪，它们的目的是保证能够“跟的上”，但估计出来的位姿可能没那么准确。第二个阶段是局部地图跟踪，将当前帧的局部关键帧对应的局部地图点投影到该帧，得到更多的特征点匹配关系，对第一阶段的位姿再次优化得到相对准确的位姿。

(1)参考关键帧跟踪(TrackReferenceKeyFrame)

没有速度信息的时候、刚完成重定位、或者恒速模型跟踪失败后使用，大部分时间不用。只利用到了参考帧的信息。
参考关键帧：就是最新加入的关键帧就是当前帧的参考关键帧

具体流程：
- 将当前帧的描述子转化为BoW向量
- 通过词袋BoW加速当前帧与参考帧之间的特征点匹配(SearchByBoW)，取出关键帧的地图点，将地图点投影到当前帧上，当前帧以这个投影坐标为圆心，进行半径为r的范围搜索，搜索得到圆内的所有特征点，在通过描述子进行筛选，得到对应的匹配点。
- 将上一帧的位姿态作为当前帧位姿的初始值
- 通过优化3D-2D的重投影误差来获得位姿(仅优化位姿)
- 剔除优化后的匹配点中的外点
如果内点的数量大于10则认为跟踪成功

(2)恒速模型跟踪(TrackWithMotionModel)

大部分时间都用这个跟踪，只利用到了上一帧的信息。

具体流程：

更新上一帧的位姿:通过上一帧的参考关键帧位姿与上一帧的参考关键帧到上一帧的变换矩阵，计算出上一帧的位姿(关键帧会持续优化位姿，所以通过关键帧求得位姿会更加准确)
对于双目或RGB-D相机，还会根据深度值生成临时地图点。我们的地图点只有在初始化、产生关键帧、局部建图线程中关键帧之间进行生成。其他情况下都是生成临时地图点。这里生成临时地图点是因为上一帧可能只是普通帧，无法生成地图点，所以在这里生成临时地图点，为后续3D-2D优化时，提供更多的地图点进行优化，得到更加准确的值。(我们进行3D-2D优化的地图点，都是关键帧生成的地图点，会出现当前帧里地图内的关键帧很远时，很多地图点投影不到当前帧，所以通过临时生成临时地图点，保证我们在3D-2D优化时有足够多的地图点进行优化)
- 根据上一帧估计的速度，用恒速模型得到当前帧的初始位姿(上一帧的位姿乘上速度)，估计一个当前帧的位姿。
- 用上一帧地图点进行投影匹配，将上一帧的地图点投影到当前帧，得到当前帧的坐标，通过坐标画圆进行搜索，得到圆的候选点，这里有个问题，因为相机是在运动，会向前运动和向后运动，根据相机的运动来判断搜索的金字塔尺度。当向前运动时，地图点会变大，所以要在更高的金字塔尺度上进行搜索，如果向后运动，地图点会变小，所以在更小的金字塔尺寸进行搜索。得到圆内的所有候选点，进行描述子的比较筛选，得到与地图点对应匹配点
- 利用3D-2D投影关系，优化当前帧位姿
- 剔除地图点中外点(如果优化后判断某个地图点是外点，清除它的所有关系)
- 匹配超过10个点就认为跟踪成功

(3)重定位跟踪(Relocalization)

跟踪丢失的时候使用，很少使用。利用到了相似候选帧的信息

具体流程：
- 计算当前帧特征点的词袋向量
- 用词袋找到与当前帧相似的候选关键帧
- 遍历所有的候选关键帧，取出关键帧的地图点，通过词袋将当前帧与地图点快速匹配，用匹配结果初始化PnP Solver
- 通过EPnP算法估计姿态，迭代5次
- 如果EPnP 计算出了位姿，对内点进行BA优化
- 如果内点较少，通过投影的方式将关键帧中未匹配的地图点投影到当前帧中(将当前地图点投影到当前帧上，进行画圆搜索对应的匹配点), 生成新的匹配，再进行优化求解。根据投影匹配的结果，再次采用3D-2D pnp BA优化位姿
- 如果BA后内点数还是比较少(<50)但是还不至于太少(>30)，可以挽救一下, 再次通过投影的方式将关键帧中未匹配的地图点投影到当前帧中，生成新的匹配，在进行优化求解
- 如果还是不满足内点的数量，则认为重定位失败
  (4)局部地图跟踪(TrackLocalMap)
  前面3种跟踪方式得到当前帧地图点后的后处理，每次跟踪都使用。前提是必须知道当前帧的位姿和地图点（尽管不准确），利用到了当前帧的两级共视关键帧的信息，使得位姿更加准确。
  具体流程：
- 以当前帧为主，找到当前帧的一级、二级共视关键帧(二级共视关键帧就是一级共视关键帧的共视关键帧)
- 找到这些一级、二级共视关键帧的地图点
- 排除当前帧的地图点(当前帧的地图点可能也在这局部地图点中，所以剔除这些当前帧已经能匹配上的地图点，寻找那些不能匹配的地图点)，将地图点投影到当前帧上，剔除当前帧看不见的地图点。通过投影的方式将局部地图中未匹配的地图点投影到当前帧中，生成新的匹配。
- 通过前面的操作新增了更多的匹配关系，BA优化得到更准确的位姿
  (5)3D-2D的重投影优化函数(PoseOptimization)
只进行位姿优化，不优化地图点
3D-2D重投影优化的误差计算：
- 地图点匹配的当前帧的像素点是固定的
- 地图点通过当前帧的位姿投影到当前帧中
- 地图点投影的像素坐标与当前帧匹配的像素坐标做差，作为误差。

四、关键帧(KeyFrame)

(1)什么是关键帧

通俗来说，关键帧就是几帧普通帧里面具有代表性的一帧
相近帧之间信息冗余度很高，关键帧是取局部相近帧中最有代表性的一帧，可以降低信息冗余度。举例来说，摄像头放在原处不动，普通帧还是要记录的，但关键帧不会增加。
关键帧选择时还会对图片质量、特征点质量等进行考察，在BundleFusion、RKD SLAM等RGB-D
SLAM相关方案中常常用普通帧的深度投影到关键帧上进行深度图优化，一定程度上关键帧是普通帧滤波和优化的结果，防止无用的或错误的信息进入优化过程而破坏定位建图的准确性。
如果所有帧全部参与计算，不仅浪费了算力，对内存也是极大的考验，这一点在前端vo中表现不明显，但在后端优化里是一个大问题，所以关键帧主要作用是面向后端优化的算力与精度的折中，使得有限的计算资源能够用在刀刃上，保证系统的平稳运行。假如你放松关键帧选择条件，大量产生的关键帧不仅耗计算资源，还会导致local
mapping 计算不过来，出现误差累积.
在后续我们的局部地图线程和闭环检测线程，都是使用关键帧来进行操作，所以关键帧非常重要

(2)如何选择关键帧

选择关键帧主要从关键帧自身和关键帧与其他关键帧的关系2方面来考虑。
关键帧自身质量要好，例如不能是非常模糊的图像、特征点数量要充足、特征点分布要尽量均匀等等；
关键帧与其他关键帧之间的关系，需要和局部地图中的其他关键帧有一定的共视关系但又不能重复度太高，以达到既存在约束，又尽量少的信息冗余的效果。

选取的指标主要有：
- 距离上一关键帧的帧数是否足够多（时间）。比如我每隔固定帧数选择一个关键帧，这样编程简单但效果不好。比如运动很慢的时候，就会选择大量相似的关键帧，冗余，运动快的时候又丢失了很多重要的帧。
- 距离最近关键帧的距离是否足够远（空间）/运动比如相邻帧根据pose计算运动的相对大小，可以是位移也可以是旋转或者两个都考虑，运动足够大（超过一定阈值）就新建一个关键帧，这种方法比第一种好。但问题是如果对着同一个物体来回扫就会出现大量相似关键帧。
- 跟踪局部地图质量（共视特征点数目）记录当前视角下跟踪的特征点数或者比例，当相机离开当前场景时（双目或比例明显降低）才会新建关键帧，避免了第2种方法的问题。缺点是数据结构和逻辑比较复杂。
- 跟踪线程选择关键帧标准较宽松，局部建图线程再跟据共视冗余度进行剔除，尤其是在回环检测中使用了以关键帧为代表的帧“簇”的概念，回环筛选中有一步将关键帧前后10帧为一组，计算组内总分，以最高分的组的0.75为阈值，滤除一些组，再在剩下的组内各自找最高分的一帧作为备选帧
  (3)ORB2-SLAM的关键帧检测(NeedNewKeyFrame)
  具体流程：
- 纯VO模式下不插入关键帧
- 如果局部地图线程被闭环检测使用，则不插入关键帧
- 如果距离上一次重定位比较近，并且关键帧数目超出最大限制，不插入关键帧
- 查询局部地图线程是否繁忙，当前能否接受新的关键帧
- 对于双目或RGBD摄像头，统计成功跟踪的近点的数量，如果跟踪到的近点太少，没有跟踪到的近点较多，可以插入关键帧
- 双目或RGBD情况下：跟踪到的地图点中近点太少同时没有跟踪到的三维点太多，可以插入关键帧了
- 设定比例阈值，当前帧和参考关键帧跟踪到点的比例，比例越大，越倾向于增加关键帧
- 如果很长时间没有插入关键帧，可以插入
- 如果满足插入关键帧的最小间隔并且localMapper处于空闲状态，可以插入
  单目相机的地图点生成，只有在初始化、关键帧与共视关键帧进行三角化生成，其余都不会生成地图点，所以对于单目相机来说，插入关键帧是很频繁的，他的插入关键帧的阈值很低。

(4)ORB2-SLAM的关键帧创建(CreateNewKeyFrame)

将当前帧构造成关键帧
- 对于双目或rgbd摄像头，为当前帧生成新的地图点，这里的地图点不是临时的，是全局地图中新建地图点，用于跟踪；单目无操作
- 插入关键帧，关键帧插入到列表关键帧队列中中，等待local mapping线程检查(局部建图线程会对判断这个关键帧是否冗余关键帧，对地图点也会进行检查，对地图点做融合)

(5)共视图、本质图、生成树、父子关键帧

共视图(共视关键帧)：

共视图是无向加权图，每个节点是关键帧，如果两个关键帧之间满足一定的共视关系（至少15个共同观测地图点）他们就连成一条边，边的权重就是共视地图点数目

共视图的作用：

跟踪局部地图，扩大搜索范围 (UpdateLocalKeyFrames)
局部建图里关键帧之间新建地图点(CreateNewMapPoints)(SearchlnNeighbors)
闭环检测、重定位检测(DetectLoop)(CorrectLoop)
优化(OptimizeEssentialGraph)
本质图：

共视图比较稠密，本质图比共视图更稀疏，这是因为本质图的作用是用在闭环矫正时，用相似变换来矫正尺度漂移，把闭环误差均摊在本质图中。本质图中节点也是所有关键帧，但是连接边更少，只保留了联系紧密的边来使得结果更精确。本质图中包含
- 生成树连接关系
- 形成闭环的连接关系，闭环后地图点变动后新增加的连接关系
- 共视关系非常好（至少100个共视地图点）的连接关系
  父子关键帧：
  关键帧的父关键帧就是与当前关键帧共视程度最高的关键帧
  生成树：
  子关键帧和父关键帧构成

五、局部建图线程(LocalMapping)

局部建图线程的主要作用：

承上启下。接收跟踪线程输入的关键帧并进行局部地图优化、删除冗余关键帧等；将优化后的关键帧发送给闭环线程。
实现中期数据关联。跟踪线程中仅使用了相邻普通帧或关键帧的信息，而且只优化当前帧的位姿，没有联合优化多个位姿，没有优化地图点。局部建图线程里满足一定共视关系的多个关键帧及其对应的地图点都参与优化。使得关键帧的位姿和地图点更加准确。
利用共视关键帧之间重新匹配得到更多新的地图点，增加地图里地图点的数目，可以提高跟踪的稳定性。
删除冗余关键帧可以降低局部BA的规模和次数。提高实时性

跟踪线程中，只跟上一帧的数据进行关联处理，而局部建图线程中，使用全部的关键帧进行关联处理

(1)处理列表中的关键帧(ProcessNewKeyFrame)

从缓冲队列中取出一帧关键帧
计算该关键帧特征点的词袋向量
当前处理关键帧中有效的地图点，更新normal，描述子等信息.
如果这些有效的地图点是局部跟踪地图生成的局部临时地图点，新增加匹配信息。现在普通帧转化成关键帧了，所以得为这些新增的地图点新增观测，能被当前关键帧观测到，重新计算地图点的平均观测方向和距离，更新地图点的最佳描述子。
如果这些有效的地图点来自双目或RGBD在创建关键帧中新生成的地图点，或者是CreateNewMapPoints 中通过三角化产生，将这些新生成的地图点放入mlpRecentAddedMapPoints，等待后续MapPointCulling函数的检验。

(2)根据地图点的观测情况剔除质量不好的地图点(MapPointCulling)

检查新增地图点，根据地图点的观测情况剔除质量不好的新增的地图点，
mlpRecentAddedMapPoints：存储新增的地图点，这里是要删除其中不靠谱的(这个新增的地图点是在ProcessNewKeyFrame函数中添加的)

遍历检查新添加的地图点
已经是坏点的地图点仅从队列中删除
跟踪到该地图点的帧数相比预计可观测到该地图点的帧数的比例小于25%，从地图中删除
从该点建立开始，到现在已经过了不小于2个关键帧，但是观测到该点的关键帧数量少于一定阈值，从地图删除
从建立该点开始，已经过了3个关键帧而没有被剔除，则认为是质量高的点

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

在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻关键帧
遍历相邻关键帧，搜索匹配并用极线约束剔除误匹配，最终三角化
判断相机运动的基线是不是足够长，如果是双目相机，关键帧间距小于本身的基线时不生成3D点，因为太短的基线下能够恢复的地图点不稳定
根据两个关键帧的位姿计算它们之间的基础矩阵
通过词袋对两关键帧的未匹配的特征点快速匹配，用极线约束抑制离群点，生成新的匹配点对
对每对匹配通过三角化生成3D点,和 Triangulate函数差不多。这里双目或RGBD会进行夹角判断，因为双目和RGBD都能直接获得这个点的深度信息，所以这里先以当前帧的的深度信息进行计算视差角余弦，再通过共视关键帧的深度信息计算视差角余弦，选取小的视差角余弦。这个视差角余弦会与匹配点的夹角进行判断，如果视差角余弦的角度更小，则使用双目或RGBD恢复三维点，否则使用匹配点的三角化恢复三维点。
检测生成的3D点是否在相机前方,不在的话就放弃这个点
计算3D点在当前关键帧下的重投影误差，如果误差小于卡方检验，则认为是个合格的三维点
三角化生成3D点成功，构造成MapPoint

(4)检查并融合当前关键帧与相邻关键帧帧（两级相邻）中重复的地图点(SearchInNeighbors)

已经处理完队列中的所有关键帧。

获得当前关键帧在共视图中权重排名前nn的邻接关键帧，当前关键帧的邻接关键帧，称为一级相邻关键帧，也就是邻居，与一级相邻关键帧相邻的关键帧，称为二级相邻关键帧，也就是邻居的邻居
存储一级相邻关键帧及其二级相邻关键帧，以一级相邻关键帧的共视关系最好的5个相邻关键帧作为二级相邻关键帧
将当前帧的地图点分别投影到两级相邻关键帧，寻找匹配点对应的地图点进行融合，称为正向投影融合
将地图点投影到关键帧中进行匹配和融合；融合策略如下：
如果地图点能匹配关键帧的特征点，并且该点有对应的地图点，那么选择观测数目多的替换两个地图点
如果地图点能匹配关键帧的特征点，并且该点没有对应的地图点，那么为该点添加该投影地图点
将两级相邻关键帧地图点分别投影到当前关键帧，寻找匹配点对应的地图点进行融合，称为反向投影融合
遍历每一个一级邻接和二级邻接关键帧，收集他们的地图点存储到 vpFuseCandidates
进行地图点投影融合,和正向融合操作是完全相同的。不同的是正向操作是"每个关键帧和当前关键帧的地图点进行融合",而这里的是"当前关键帧和所有邻接关键帧的地图点进行融合"
更新当前帧地图点的描述子、深度、平均观测方向等属性
更新当前帧与其它帧的共视连接关系

(5)局部地图的BA(LocalBundleAdjustment)

由局部建图线程调用,对局部地图进行优化的函数

将当前关键帧及其共视关键帧加入局部关键帧，找到关键帧连接的共视关键帧（一级相连），加入局部关键帧中
遍历局部关键帧中(一级相连)关键帧，将它们观测的地图点加入到局部地图点
得到能被局部地图点观测到，但不属于局部关键帧的关键帧(二级相连)，这些二级相连关键帧在局部BA优化时不优化
构造g2o优化器
添加待优化的位姿顶点：局部关键帧的位姿
添加不优化的位姿顶点：固定关键帧的位姿，注意这里调用了vSE3->setFixed(true)，不优化为啥也要添加？回答：为了增加约束信息
添加待优化的局部地图点顶点
在添加完了一个地图点之后, 对每一对关联的地图点和关键帧构建边(二元边)
分成两个阶段开始优化。
检测outlier，并设置下次不优化
排除误差较大的outlier后再次优化 – 第二阶段优化
在优化后重新计算误差，剔除连接误差比较大的关键帧和地图点
优化后更新关键帧位姿以及地图点的位置、平均观测方向等属性

局部地图优化是处理完所有关键帧队列才进行的。所以生成了很多新的地图点。通过新的地图点匹配信息进行优化
局部地图的优化是同时优化位姿和地图点，也就是构建了位姿、地图点的G2O顶点，也定义了二元残差边，优化类似EM算法，先通过3D-2D优化位姿，得到更准确的位姿，在优化地图点，得到更加准确的地图点，在通过更准确的地图点在进行优化位姿，以此类推持续迭代优化。
针对当前帧关键帧进行优化，优化的目标是当前帧与当前帧的一级共视关键帧的位姿和这些关键帧下的所有地图点，二级共视关键帧的位姿不进行优化，只添加约束信息，我个人认为跟防止过拟合的性质是一样的，增加这些二级共视关键帧的位姿进行约束，让他优化的过程中，位姿不会太偏。
优化过程：
取出当前帧的一级共视关键帧————构建局部地图(当前帧与一级共视关键帧的所有地图点)————取出二级共视关键帧————构建G2O优化器————将当前帧与一级共视关键帧的位姿、局部地图点设置成进行迭代优化顶点————将二级共视关键帧设置成不进行优化的的顶点，只为约束优化过程中的位姿————第一次优化添加核函数，减少外点的影响————第一次优化完毕，去除外点————在进行第二次优化，不加入核函数，因为第一次优化已经过滤掉外点了————得到优化后的位姿和地图点————替换之前的位姿和地图点

(6)检测并剔除当前帧相邻的关键帧中冗余的关键帧(KeyFrameCulling)

检测当前关键帧在共视图中的关键帧，根据地图点在共视图中的冗余程度剔除该共视关键帧
冗余关键帧的判定：90%以上的地图点能被其他关键帧（至少3个）观测到

根据共视图提取当前关键帧的所有共视关键帧
提取每个共视关键帧的地图点
遍历该共视关键帧的所有地图点，其中能被其它至少3个关键帧观测到的地图点为冗余地图点
如果该关键帧90%以上的有效地图点被判断为冗余的，则认为该关键帧是冗余的，需要删除该关键帧

(7)将当前帧加入到闭环检测队列中

六、闭环检测及矫正线程(LoopClosing)

闭环检测，是判断当前关键帧是否发生回环，即当前关键帧的场景与之前的关键帧场景相同，形成回环，从而消除累计误差。

(1)闭环检测(DetectLoop)

闭环检测需要连续检测三次，且这三次的子候选组内都有相同的关键帧，这也才算检测到闭环。

从闭环缓冲队列中取出一帧回环关键帧
如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
遍历当前回环关键帧所有连接（>15个共视地图点）关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分minScore(GetVectorCovisibleKeyFrames)
- 取最低的相似得分，作为后续选取闭环候选帧的阈值
在所有关键帧中找出闭环候选帧（注意不和当前帧连接）(DetectLoopCandidates):
- 找出和当前帧具有公共单词的所有关键帧，不包括与当前帧连接的关键帧
- 统计上述所有闭环候选帧中与当前帧具有共同单词最多的单词数，用来决定相对阈值
- 确定最小公共单词数为最大公共单词数目的0.8倍(minCommonWords)
- 遍历上述所有闭环候选帧，挑选出共有单词数大于minCommonWords且单词匹配度大于minScore存入lScoreAndMatch
  1. 对这些关键帧进行BOW相似值计算，得分必须大于回环关键帧与一级共视关键帧的最低得分，因为闭环帧肯定是回环关键帧十分相似，且不在一级共视关键帧中，得到符合的关键帧
- 计算上述候选帧对应的共视关键帧组的总得分，得到最高组得分bestAccScore，并以此决定阈值minScoreToRetain(单单计算当前帧和某一关键帧的相似性是不够的，这里将与关键帧相连（权值最高，共视程度最高）的前十个关键帧归为一组，计算累计得分)
  1. 取出候选帧的一级共视关键帧，如果这个共视关键帧也在这个候选帧内，且相似得分大于minScore，则进行加分。
  2. 统计最高的得分，设定最低的阈值为最高得分的0.75(minScoreToRetain)，只保留大于minScoreToRetain阈值的候选帧
- 只取组得分大于阈值的组，得到组中分数最高的关键帧们作为闭环候选关键帧
在候选帧中检测具有连续性的候选帧
- 遍历刚才得到的每一个候选关键帧
- 将自己以及与自己相连的关键帧构成一个“子候选组”
  - 候选关键帧与自己的一级共视关键帧组成一个子候选组
- 遍历前一次闭环检测到的连续组链
- 遍历每个“子候选组”，检测子候选组中每一个关键帧在“子连续组”中是否存在
  - 闭环检测需要连续检测三次，且这三次的子候选组内都有相同的关键帧，这也才算检测到闭环。
- 如果判定为连续，接下来判断是否达到连续的条件
- 如果该“子候选组”的所有关键帧都和上次闭环无关（不连续），vCurrentConsistentGroups 没有新添加连续关系
- 如果达到连续的条件，则认为检测到回环

(2)计算当前关键帧和闭环候选帧的Sim3变换(ComputeSim3)

什么是Sim3变换：

[图片]

已知至少三个匹配的不共线三维点对，求他们之间的相对旋转、平移、尺度因子。

为什么要计算Sim3变换：

均摊误差、自然过渡。
当前关键帧、闭环关键帧之间其实是隔了很多帧的，他们的pose都是由邻近信息得到的，经过了很久后，很可能有累计的位姿误差（单目尺度漂移所以有尺度误差，双目或RGBD认为没有尺度误差），如果你闭环时直接根据各自的位姿强制把相隔了很久的两个位姿接上，很可能会导致明显的错位。

遍历闭环候选帧集，初步筛选出与当前关键帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver
- 从筛选的闭环候选帧中取出一帧有效关键帧pKF
- 将当前帧 mpCurrentKF 与闭环候选关键帧pKF匹配
- 为保留的候选帧构造Sim3求解器
对每一个候选帧用Sim3Solver 迭代匹配，直到有一个候选帧匹配成功，或者全部失败
- 取出为当前候选帧构建的 Sim3Solver 并开始迭代，最多迭代5次，返回的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换（T12）
  - Sim3得到的是候选帧到当前帧的位姿
- 如果计算出了Sim3变换，继续匹配出更多点并优化。因为之前 SearchByBoW 匹配可能会有遗漏
  - 之前是通过SearchByBoW进行匹配的，现在更准确的位姿，通过SearchBySim3进行匹配
  - 通过Sim3变换，投影搜索pKF1的特征点在pKF2中的匹配，同理，投影搜索pKF2的特征点在pKF1中的匹配，只有互相都成功匹配的才认为是可靠的匹配
  - 互相匹配是指pKF1投影到pKF2 与 pKF2投影到pKF1 所筛选出来的匹配特征点是一致的。
- 得到更多的匹配信息，用新的匹配来优化 Sim3，得到更加准去的Sim3位姿，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
  (3)闭环矫正(CorrectLoop)

通过Sim3变化得到了当前帧更加准确的位姿
根据共视关系更新当前关键帧与其它关键帧之间的连接关系
- 通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的地图点，
通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的地图点
- 当前帧与世界坐标系之间的Sim变换在ComputeSim3函数中已经确定并优化，通过相对位姿关系，可以确定这些相连的关键帧与世界坐标系之间的Sim3变换
- 取出当前关键帧及其共视关键帧，称为“当前关键帧组
- 通过Sim3（认为是准的）来进行位姿传播，得到当前关键帧的共视关键帧的世界坐标系下Sim3 位姿
  - 获得当前帧与共视关键帧的相对位姿，现在我的得到更加准确的当前帧的位姿，我们通过当前帧的Sim3位姿的逆乘上之前的相对位姿，得到更加准确的共视关键帧位姿。
  - 得到矫正的当前关键帧的共视关键帧位姿后，修正这些共视关键帧的地图点
  - 将共视关键帧的Sim3转换为SE3，根据更新的Sim3，更新关键帧的位姿
  - 根据共视关系更新当前帧与其它关键帧之间的连接
检查当前帧的地图点与经过闭环匹配后该帧的地图点是否存在冲突，对冲突的进行替换或填补
将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中，进行匹配，融合，新增或替换当前关键帧组中KF的地图点，因为闭环相连关键帧组mvpLoopMapPoints 在地图中时间比较久经历了多次优化，认为是准确的，而当前关键帧组中的关键帧的地图点是最近新计算的，可能有累积误差
- 遍历矫正后当前关键帧的共视关键帧
- 将mvpLoopMapPoints投影到pKF帧匹配，检查地图点冲突并融合
- 遍历闭环帧组的所有的地图点，替换掉需要替换的地图点
更新当前关键帧组之间的两级共视相连关系，得到因闭环时地图点融合而新得到的连接关系
- 遍历当前帧相连关键帧组（一级相连）
- 得到与当前帧相连关键帧的相连关键帧（二级相连）
- 更新一级相连关键帧的连接关系(会把当前关键帧添加进去,因为地图点已经更新和替换了)
- 取出该帧更新后的连接关系
- 从连接关系中去除闭环之前的二级连接关系，剩下的连接就是由闭环得到的连接关系
- 从连接关系中去除闭环之前的一级连接关系，剩下的连接就是由闭环得到的连接关系
进行本质图优化，优化本质图中所有关键帧的位姿

(4)本质图优化(OptimizeEssentialGraph)

仅优化所有关键帧位姿，不优化地图点，(没有将初始帧锁住，只锁住了回环帧，所以优化了初始帧)
因为我们已经有的Sim3的当前帧关键组的位姿，通过这个关键组的位姿，优化所有关键帧的位姿，优化的误差函数就是：未Sim3的关键帧位姿与其他关键帧的相对位姿与经过Sim3的关键帧的位姿与其他关键帧的相对位姿进行残差计算，得到更加准确的关键帧位姿。
将优化后的关键帧位姿进行更新。
根据优化后的的位姿，更新地图点。

(5)全局优化(RunGlobalBundleAdjustment)

经过本质图优化，我们只优化了关键帧的位姿，得到更加准确地关键帧位姿，在通过位姿重新计算得出地图点坐标，那么地图点的坐标进行了更新，就能获得新的匹配点，从而进行3D-2D重投影，得到更加准确的位姿
所以我们对整个关键帧进行位姿、地图点的优化，跟局部建图线程一致，得到更加准确地位姿和地图点。
- 执行全局BA，优化所有的关键帧位姿和地图中地图点(GlobalBundleAdjustemnt)
- 遍历并更新全局地图中的所有spanning tree中的关键帧
- 遍历当前关键帧的子关键帧
- 遍历每一个地图点并用更新的关键帧位姿来更新地图点位置