算法原理

算法由三大部分组成，分别是 Tracking、LocalMapping、LoopClosing 三个线程

Tracking

1. 特征点的提取（ORB Extraction） 在此之前，特征点的提取方法有sift，surf等，但相比ORB都比较耗时。ORB算法先提取FAST角点，再计算BRIEF描述子，最终利用Bow词袋模型来匹配特征点。

   • 建立图像金字塔：将原图像按不同比例缩小，得到多张图片，分别对每张图片进行下述操作。

   • FAST角点提取（对所有像素点进行提取）

     • 将图像转为灰度图，选取像素 $p$， 假设他的亮度为 $I_p$
     • 设定一个阈值 $T$，其大小与 $I_p$ 成正比
     • 以 $p$ 为中心，半径为3的圆上选16个像素点
     • 如果有连续$N$个点亮度大于 $I_p+T$ 或小于$I_p-T$，则 $p$ 被认为是一个FAST角点 

   ---

   • BRIEF描述子（对所有的FAST角点计算描述子）

     • 计算每个FAST角点圆心到质心点的方向

     • 将每个角点对应的圆域进行旋转，旋转至所有角点对应的方向相同。 

     • 圆域内选取N个点对（默认指定了128对点），通过将亮度的比较结果，将角点映射为：只由0、1组成的128维向量，将此向量作为BRIEF描述子

     • 最后，对每个FAST角点，都生成了128维描述子。

     • 当被拍摄物体放大或者缩小时，FAST角点会产生不同的提取结果（比如缩小会引起角点消失），但在图像金字塔的其他层中依然可以提取到缩放前对应的FAST角点，从而具有良好的尺度不变性。当被拍摄物体发生旋转时，其FAST角点的方向也会随之旋转，计算BRIEF描述子前，将所有FAST角点旋转至同一方向，相当于将所有拍摄对象都旋转到同一方向，从而具有良好的旋转不变性

   • BoW 词袋模型（对图片、特征进行查找和索引）
     • 词袋模型作为所有描述子的字典， 用于量化两张图或两个特征点的相似性
     • 字典的数据结构为 k叉树，上层是下层的聚类，最底层的每个叶子节点对应的是最小特征单元：BRIEF描述子
     • 获得描述子后，通过次查找k叉树，来得到此描述子的匹配点，降低了查找匹配的复杂度

2. 旋转不变性来自：提取FAST特征时，计算了每个角点的中心点到质心点的方向向量，即使物体发生了旋转，也可利用方向向量来识别。

3. 尺度不变性来自：对每一帧，建立4层的高斯金字塔，分别在每层金字塔上做特征提取，即使A图中的特征在B图中变小，也可以在A图金字塔高层提取到B图中同尺度的特征

   ---

4. 点云初始化 这部分在论文中没有提及。发生在整个算法刚开始运行时，点云的初始化是此后每一帧能成功定位的前提

   • 作用：特征匹配、三角化，得到初始点云图，对于单目来说，就是将P2P问题转为PnP问题
   • 步骤(以单目为例）：
     • 初始选取2个特征点较多的帧（ $\mathrm{特}\mathrm{征}\mathrm{点}\mathrm{数}>100$ )，并在词袋中进行特征匹配
     • 并行计算单应性矩阵 $H$ 和基础矩阵 $F$ 。 当对极约束不成立的时候（相机只有旋转没有平移），或场景都在同一平面时，用单应性矩阵来计算两帧之间的变换矩阵。否则用基础矩阵计算变换矩阵
     • 将第一帧的位姿设置为基准位姿，第二帧的位姿为上一步所求出的变换矩阵
     • 创建地图点对象MapPoint，恢复出所有匹配点对应的世界坐标

   ---

5. 位姿初始化（Initial Pose Estimation） 对 $CurrFrame$ (此刻的帧)位姿优化之前，先要有一个大概的估计，作为之后优化位姿的初值。根据当前跟踪状态的不同，一共有三种初始化方法：根据参考关键帧估计，根据匀速运动模型估计，重定位。除此之外，还要对此帧观测到的特征点进行估计

   • 作用：估计帧的位置，以及观测到的局部地图点
   • 步骤：

     • 跟踪状态为LOST时：

       • 用重定位估计位姿（Relocation），用词袋模型寻找与 $CurrFrame$ 最相似的帧 $RelocFrame$，进行特征匹配
       • 统计 $CurrFrame$ 上匹配点数量，数量太少则认为匹配效果差，保持状态为LOST，结束对此帧的处理，等待下一帧；数量足够的话，进行一次仅优化位姿的BA（motion-only BA），数量不够则舍弃此帧

     • 跟踪状态NORMAL时：

       • 根据匀速运动模型估计位姿，默认上一帧到 $CurrFrame$ 的变换矩阵等于上上帧到上一帧的变换矩阵。（track with Motion model）
       • 将上一帧（LastFrame）观测到的MapPoint 投影到估计的位姿上，观测有多少投影点与特征点位置重合，重合则认为是一对匹配点，如果重合的足够多，则进行一次仅优化位姿的BA，如果重合的个数太少，表明匹配失败改用参考关键帧估计；

     • 根据参考关键帧估计位姿：（Track with RefFrame）

       • 用词袋模型，将 $CurrFrame$ 与参考关键帧（最新的关键帧）进行特征匹配
       • 匹配后执行一次仅优化位姿的BA（motion-only BA）
       • 统计匹配特征点数，数量少则认为匹配失败，将状态改为LOST，结束对此帧的处理，等待下一帧；
   • 需要注意：初始化后的$Frame$ 中仍会有大量特征点没建立匹配 ，这也体现了tracking线程的重要思想：在尽可能少的建立匹配的情况下，初始化出相机的位姿。

   ---

6. 局部位姿优化（Track Local Map） 经过上面的初始化，失败的 $Frame$ ，说明其与地图联系不够紧密，无法建立稳健的约束和估计；而与地图联系紧密的$Frame$ 则初始化成功。 但刚刚初始化所得位姿仅依赖于与某一帧（前一帧或参考关键帧）中的匹配关系，置信度较低。 接下来，我们在局部图中寻找更多的匹配关系，利用这些匹配关系进一步优化$CurrFrame$的位姿。

   • covisibility graph定义：covisibility graph，是无向加权图，如果两个关键帧之间有共视关系（共同观测到>15个地图点）就连成一条有权边，这些边和所有关键帧节点组成共视图。
   • 在共视图中，针对$CurrFrame$定义一组关键帧为局部关键帧，局部关键帧由四种邻居组成。这四种邻居为：
     • 与 $CurrFrame$ 有共视点的帧（一级共视帧）
     • 与 一级共视帧 有共视点的前十个帧（二级共视帧）（按共视点个数排序）
     • 一级共视帧的子关键帧
     • 一级共视帧的父关键帧（与一级共视帧共视程度最高的帧）
   • 将 $CurrFrame$、局部关键帧 、以及这些帧观测到的 MapPoint作为：局部图 （$localmap$）
   • 将$localmap$ 中的MapPoint向 $CurrFrame$ 投影
   • 对 $CurrFrame$再进行一次仅位姿BA（motion-only BA）优化，估计出各个局部关键帧的位姿

   ---

7. 关键帧的审核与生成 (New Keyframe Decision)

   • 检测系统是否允许关键帧的插入
     • only_Tracking 模式下，不需要关键帧
     • $localmap$ 被 loopclosing线程占用，不能插入关键帧
     • 关键帧总数较多，且距离上一个关键帧距离很近，不需要插入关键帧
   • 审核$CurrFrame$是否有资格做关键帧
     • 记录$CurrFrame$ 观察到的MapPoint总数，$nu{m}_{curr}$
     • 记录$RefFrame$ 观察到的MapPoint总数，$nu{m}_{ref}$
     • 如果$\frac{nu{m}_{curr}}{nu{m}_{ref}}$小于一定阈值，则允许 $CurrFrame$作为一个关键帧
   • 将 $CurrFrame$ 设置为关键帧
     • 将自己生成的关键帧$PkF$ 作为$CurrFrame$ 的参考关键帧
     • 在地图中新建地图点 

   ---

• 总结
• Tracking线程根据状态的不同，以三种方式来初始化每一帧的位置和姿态，这三种方式速度由慢到快为：
• 回顾Tracking过程
  • 整个过程可以看做三步：获取位置、优化位置、关键帧审核
  • 在尽可能少建立匹配或者不建立匹配的情况下，进行位姿初始化（获取位置）
  • 尽可能建立一个：体量尽量小、但与$CurrFrame$关系足够多（四种邻居）的局部图，在此局部图的基础上进行图优化BA （优化位置）
  • 在跟踪到的点足够多的情况下（>50个），尽可能多地生成关键帧，因为后面的关键帧删除策略很严格，这里关键帧的生成比较宽松（关键帧审核）
• 可以看出，Tracking的一个重要特点是提速（不到万不得已，不进行耗时的匹配操作）

LocalMapping

1. 当待处理$KeyFrame$队列非空， localMapping线程就开始工作、

2. 将此KeyFrame插入共视图中（KeyFrame Insertion）

   • 计算此KeyFrame对应的词袋向量（应该是在回环检测会用到）
   • 获取localMap中，被此KeyFrame观测到的点（在Tracking线程中的位姿初始化步骤已经被初始化），更新此KeyFrame的平均观测方向，以及各个MapPoint的最佳描述子
   • 根据共视程度创建有权边，将KeyFrame插入共视图中

---

4. 删除多余的MapPoint 此前LocalMapping线程可能删除了一些冗余关键帧，导致对应的地图点接近于“无帧观测”的状态，不满足以下2条件中任意一条的MapPoint会被移除
   • 对于localmap中的任意地图点$P$，如果 $\frac{\mathrm{实}\mathrm{际}\mathrm{观}\mathrm{测}\mathrm{到}P\mathrm{的}\mathrm{帧}\mathrm{数}\mathrm{量}}{\mathrm{预}\mathrm{测}\mathrm{会}\mathrm{观}\mathrm{测}\mathrm{到}P\mathrm{的}\mathrm{帧}\mathrm{数}\mathrm{量}}$>25%
   • 如果观测到$P$的是一个关键帧，则至少三个连续关键帧都观测到$P$

---

5. 创建新的MapPoint
   • 创建MapPoint点：对当前关键帧（$K_c$）的未匹配特征点，在所有其他关键帧中寻找特征匹配（利用词袋模型），得一定数量的最佳共视关键帧（$K_i$），相互匹配后进行三角化，计算出新$MapPoint$ $P$ 的位置
   • 构建MapPoint、$K_c$与全局map的联系：得出MapPoint的位置后，还需要将$P$与全局map融合，也就是建立$P$与其他关键帧的联系
     • 定义一级二级关键帧：
       • 一级关键帧：在共视图中挑选10个共视程度最佳的邻居
       • 二级关键帧：对每个一级关键帧，分别挑选5个共视程度最高的邻居
     • 正向投影融合：此时$P$点只被$K_c$和$K_i$两个帧所观测到，还需将$P$分别投影到一、二级关键帧，观察是否与其他关键帧中的特征点匹配 投影至一、二级关键帧上后，如果投影点恰好与某个特征点重合，会有两种情况
       • 特征点没有对应的MapPoint： 设置$P$为此特征点对应的MapPoint
       • 特征点有对应的MapPoint $P^{\prime }$：删除$P、P^{\prime }$其中之一，即被观测帧数最少的
       • 此时当前帧$K_c$的所有地图点已与localmap融合、建立联系；
     • 反向投影融合：将一、二级关键帧观测的特征点投影到$K_c$，投影后同样会有两种情况，处理方式同上
     • 至此，当前帧$K_c$的所有地图点已与全局map融合；一、二级关键帧观测的特征点也与$K_c$建立匹配关系

---

6. 进行全局BA优化，优化所有的$MapPoint$、所有的$KeyPoint$

---

7. 删除冗余的关键帧 为了防止关键帧数量无上限增长，需要不停检测冗余的关键帧，如果$K_c$中 90%的点已经在其他帧中被观测到，就将冗余关键帧$K_c$删除，方法如下：
   • 首先遍历共视图中的邻居$K_{ci}$，再遍历$K_c$的每个MapPoint $P_i$ ( i=0,1,...n)
   • 记录被观测数大于3的$P_i$，如果此类$P_i$占所有$P_i$超过90% ，就将$K_c$删除。

LoopClosing

① 检测闭环

关键帧 与 其 共视帧 组成一个候选关键帧组 ， 不同候选关键帧组中有相同的关键帧，则称这两个组是连续的。

1. 从队列头中取出一个关键帧, 作为当前检测闭环的关键帧；
2. 如果距离上次闭环没过多久, 或者map中关键帧总共还没有10帧, 则不进行闭环检测；
3. 计算这个关键帧与它所有的共视帧的Bow相似度得分, 得到最低得分minScore；
4. 计算这个关键帧与关键帧数据库中所有帧的相似度得分, 将得分大于minScore的作为候选关键帧，放入候选关键帧组 列表中；
5. 在候选关键帧中检测具有连续性 > 3 的候选帧；
6. 将满足要求的关键帧存放在列表 mvpEnoughConsistentCandidates 中(最优质关键帧组)。

② 计算sim3 相似变换

1. 遍历上一步得到的每一个闭环候选关键帧；
2. 通过Bow加速得到 闭环候选关键帧 与 当前关键帧 之间的匹配点；
3. 如果匹配点个数 小于 20， 则剔除该 闭环候选关键帧；
4. 计算闭环候选关键帧 与 当前关键帧 之间的 Sim3 变换；
5. 通过RANSAC 算法迭代五次，求出候选帧到当前关键帧的Sim3变换Scm；
6. 通过Sim3变换把候选帧的特征点变换到当前关键帧中，反之同理，求出相互匹配的特征点；
7. 再通过上一步 求出的特征点，通过g2o来优化刚刚的Scm，得到mg2oScw；算出一个好的，为mpMatchedKF，就退出循环；
8. 将上面能算的好的mpMatchedKF，将其与其共视关键帧都放入列表vpLoopConnectedKFs中，它们对应的地图点放入列表mvpLoopMapPoints中；
9. 再将mvpLoopMapPoints中的地图点，通过Sim3变换投影到当前关键帧，搜索更多的匹配对；
10. 如果匹配对超过40，回环成功。最终的形成回环的帧是mpMatchedKF。

③ 回环矫正

1. 需要请求局部地图停止，防止它向缓冲区插入新的关键帧。
2. 因为，步骤四中的6，将当前关键帧通过一次Sim3变换一次，所以需要对当前关键帧通过共视关系更新它的连接关系；
3. 遍历当前关键帧组，通过关键帧和共视帧的对应关系，很容易求出共视帧的Sim3变换，当前关键帧到其共视帧的Sim3变换g2oSic * mg2oScw，将这个通过g2o优化好的Sim3变换存入到列表CorrectedSim3中。与直接求出这一帧的相机坐标系位姿GetPose()变换，转换成Sim变换，存入到列表NonCorrectedSim3中；
4. 遍历当前关键帧的共视关键帧，修正这些共视关键帧的地图点。变换关系：这个点的世界坐标系 --(NonCorrectedSim3中的变换)--> 未矫正的相机坐标系 --(CorrectedSim3中的逆变换)--> 矫正后的世界坐标系。将这个共视关键帧的位姿设置为CorrectedSim3中的变换；
5. 检查当前帧地图点与经过闭环匹配后该帧的地图点是否存在冲突，对冲突进行替换或者填补。同时遍历匹配点和当前关键帧的地图点，如果当前关键帧地图点存在，那么就用匹配点代替，否则就把匹配点添加到当前关键帧地图点；
6. 同理，将闭环关键帧组的地图点，投影到当前关键帧组地图点进行匹配，融合，新增或者替换；
7. 更新当前关键帧组之间的两级共视相连关系；
8. 进行本质图优化，优化本质图中所有关键帧的位姿和地图点；
9. 新建一个线程用于全局BA优化。

代码流程

文件的调用关系

主函数只调用了Tracking线程 LocalMapping、LoopClosing线程不断查找关键帧队列，如果队列非空，LocalMapping线程开始工作，LoopClosing线程继续检查是否构成回环，如果队列非空且构成回环则LoopClosing开始工作。

重要变量的意义

线程	函数名	变量名	变量类型	意义
Tracking	track()	mState	bool	标志系统状态：未初始化、正常或丢失
		bOK	bool	衡量track()中每一步跟踪的质量
		mbOnlyTracking	bool	标志系统是否处于只跟踪不建图模式
	CheckReplacedInLastFrame()	mLastFrame	Frame	当前帧的上一帧
	TrackWithMotionModel()	vpMapPointMatches	vector<MapPoint*>	上一帧与当前帧的匹配关系
		nmatchesMap	int	记录特征匹配的内点数目
	TrackReferenceKeyFrame()	vpMapPointMatches	vector<MapPoint*>	上一帧与当前帧的匹配关系
		nmatchesMap	int	记录特征匹配的内点数目
	Relocalization()	vpCandidateKFs	vector<KeyFrame*>	与当前帧最相似的关键帧队列
		vvpMapPointMatches	vector<vector<MapPoint*> >	帧队列中所有帧与当前帧的匹配关系
		nCandidates	int	质量好的候选帧数量
		bNoMore	bool	为true表示RANSAC已达到最大迭代次数
		sFound	set<MapPoint*>	投影到当前帧为内点的MapPoint集合
	NeedNewKeyFrame()	mpLocalMapper	LocalMapping*	局部建图线程对象
		mpMap	Map*	全局地图对象
		nKFs	int	全局地图中的关键帧数
		nMinObs	int	规定的最小观测次数
		mpReferenceKF	KeyFrame*	最新的关键帧作为参考关键帧
		nRefMatches	int	参考关键帧中,被观测次数>nMinObs的地图点数量
		thRefRatio	int	值越大，越倾向于接受关键帧
	CreateNewKeyFrame()	pKF	KeyFrame*	新建的关键帧对象
		mpLastKeyFrame	KeyFrame*	最新的关键帧
		mlNewKeyFrames	list<KeyFrame*>	存放Tracking产生的关键帧
LoopMapping	ProcessNewKeyFrame()	vpMapPointMatches	vector<MapPoint*>	当前帧对应的所有地图点
		mlpRecentAddedMapPoints	list<MapPoint*>	还需被MapCulling检验的地图点列表
		mspKeyFrames	set<KeyFrame*>	存储全局map中所有的关键帧
	MapPointCulling()	nThObs	int	根据相机类型设置不同的观测阈值
		mlpRecentAddedMapPoints	list<MapPoint*>	还需被MapCulling检验的地图点列表
		nCurrentKFid	unsigned long	当前帧的id
		mnFirstKFid	int	地图点对象的属性，记录创建此地图点的关键帧id
	CreateNewMapPoints()	nn	int	选取一级共视关键帧的个数
		vpNeighKFs	vector<KeyFrame*>	存放nn个一级共视关键帧
		.......待更新
	SearchInNeighbors()	nn	int	选取一级共视关键帧的个数
		vpNeighKFs	vector<KeyFrame*>	存放nn个一级共视关键帧
		pKFi	KeyFrame*	一级共视关键帧
		vpSecondNeighKFs	vector<KeyFrame*>	存放5个二级关键帧
		vpFuseCandidates	vector<MapPoint*>	一、二级关键帧中，需要与当前帧融合的地图点
	KeyFrameCulling()	vpLocalKeyFrames	vector<KeyFrame*>	当前帧的所有一级共视关键帧
		vpMapPoints	vector<MapPoint*>	每个共视关键帧对应的地图点
		nRedundantObservations	int	记录冗余点的数量
		thObs	int	规定的最小观测次数
		scaleLevel	int	关键帧的金字塔尺度，值越大越靠近顶端
		nObs	int	记录有多少个关键帧高质量地观测到同一个地图点
		nMPs	int	记录当前帧中非坏点的个数
	InsertKeyFrame()	mlpLoopKeyFrameQueue	list<KeyFrame*>	存放LoopClosing产生的关键帧
LoopClosing	DetectLoop()	mlpLoopKeyFrameQueue	list<KeyFrame*>	关键帧缓冲队列
		vpConnectedKeyFrames	vector<KeyFrame *>	与当前帧相连的共视关键帧
		mpORBVocabulary	ORBVocabulary* mpORBVocabulary	词袋模型中的大词典
		vpCandidateKFs	vector<KeyFrame *>	当前帧的候选回环关键帧
		mvConsistentGroups	vector	上一次产生的连续组
		vCurrentConsistentGroups	vector	筛选出来的连续组
		ConsistentGroup	pair<set<KeyFrame*>,int>	连续组结构
		mvpEnoughConsistentCandidates	std::vector<KeyFrame*>	最优质的闭环候选帧
	DetectLoopCandidates()	spConnectedKeyFrames	set<KeyFrame*>	与当前帧相连的关键帧
		lKFsSharingWords	list<KeyFrame*>	可能与当前关键帧形成闭环的候选帧
		mBowVec	map<WordId, WordValue>	WordId 和 WordValue 表示Word在叶子中的id和权重
		mvInvertedFile	vector<list<KeyFrame*> >	vector的下标是WordId，list是包含这个Word的所有关键帧
		lKFsSharingWords	list<KeyFrame *>	初步候选关键帧列表
		lScoreAndMatch	list<pair<float, KeyFrame *>>	筛选单词数后的候选关键帧列表
		vpLoopCandidates	vector<KeyFrame *>	最后返回的关键帧列表
	UpdateConnections()	KFcounter	map<KeyFrame*,int>	关键帧 -- 共视程度(地图点个数)
		vpMP	vector<MapPoint*>	关键帧的地图点
		observations	map<KeyFrame*,size_t>	观察到这个地图点的关键帧，size_t表示这个地图点对应在该关键帧的特征点id
	ComputeSim3()	vvpMapPointMatches	vector<vector<MapPoint *>>	存储每个候选帧的匹配地图点信息
		mvpCurrentMatchedPoints	vector<MapPoint*>	存储的地图点在"当前关键帧"中成功地找到了匹配点的地图点的集合
	CorrectLoop()	CorrectedSim3	map<KeyFrame *, Sim3>	存储的地图点在"当前关键帧"中成功地找到了匹配点的地图点的集合
		NonCorrectedSim3	map<KeyFrame *, Sim3>	存放没有矫正的当前关键帧的共视关键帧的世界坐标系下Sim变换

Tracking流程

1. 位姿初始化（截取部分）：根据系统状况、跟踪效果选择不同的位姿初始化方法

   if(mState==OK){
       CheckReplacedInLastFrame();
       
       if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
           bOK = TrackReferenceKeyFrame();
       else{
           bOK = TrackWithMotionModel();
           if(!bOK)
               //根据恒速模型失败，根据参考关键帧来跟踪
               bOK = TrackReferenceKeyFrame();
       }
   }
   else
       bOK = Relocalization();
   

   CheckReplacedInLastFrame() ==更新上一帧中的地图点==

     - 检查$CurrFrame$中是否有地图点被LocalMapping线程替换或者删除
     					
     	- 被替换则更新，被删除则将地图点赋值为NULL
   

   ---

   TrackWithMotionModel() ==用匀速运动模型，对$CurrFrame$进行位姿初始化==

     - 默认上一帧到 $CurrFrame$ 的变换矩阵等于上上帧到上一帧的变换矩阵。
     					
     - 将上一帧观测到的地图点，投影到$CurrFrame$ ，投影点与特征点的间距作为error，如果匹配点不够，则进行`TrackReferenceKeyFrame()`
     - 如果匹配点足够，进行一次仅位姿的BA优化，优化$CurrFrame$的pose，使error最小
   

   ---

   TrackReferenceKeyFrame() ==用上一帧，对$CurrFrame$进行位姿初始化==

     - 用**词袋模型**，将 $CurrFrame$ 与**参考关键帧**（最新的关键帧）进行特征匹配
     					
     - 将 **参考关键帧**的地图点投影到$CurrFrame$ ，投影点与特征点的间距作为error
     - 如果匹配点足够，进行仅位姿的BA优化，优化$CurrFrame$的pose，使error最小
   

   ---

   Relocalization() ==用词袋模型寻找$RelocFrame$，对$CurrFrame$进行位姿初始化==

     - 在所有关键帧中，先用词袋模型找出一批，与$CurrFrame$共同单词最多的关键帧，作为候选关键帧`vpCandidateKFs`
     					
     - 遍历`vpCandidateKFs`，用词袋模型，找出与$CurrFrame$相似度最高的帧作为$RelocFrame$
     -  用**词袋模型**，将 $CurrFrame$ 与 **$RelocFrame$** 进行特征匹配
     - 将 **$RelocFrame$**的对应地图点投影到$CurrFrame$ 上，投影点与特征点的间距作为error
     - 如果匹配点足够，进行仅位姿的BA优化，优化$CurrFrame$的pose，使error最小
   

   ---

2. 局部位姿优化 、关键帧审核（截取部分）

//更新参考关键帧
mCurrentFrame.mpReferenceKF = mpReferenceKF;
if(!mbOnlyTracking){
if(bOK)
    bOK = TrackLocalMap();
}
else
{
if(bOK && !mbVO)
    bOK = TrackLocalMap();
}

//更新系统状态、更新显示、 等等
.....


if(NeedNewKeyFrame())
    CreateNewKeyFrame();

TrackLocalMap() ==局部地图中，用BA优化 $CurrFrame$ 的位姿==

• 更新局部关键帧、局部地图点，构建局部地图

• 将局部地图中的$MapPoint$投影到$CurrFrame$，建立新的匹配关系

• 仅位姿的BA优化

• 更新$CurrFrame$对应的每个$MapPoint$的被观测信息、删除外点

• 根据匹配点数目判断局部位姿优化的好坏，好的话(成功匹配50个点)返回true，否则flase

---

NeedNewKeyFrame() ==检测系统是否需要插入关键帧==

• 纯Tracking模式，不生产关键帧

• LoopCLosing线程正在使用局部地图时，不生产关键帧

• 关键帧数目较多、且位置离上一个关键帧较近，不生产关键帧

• 当LocalMapping线程不忙，且$\frac{nu{m}_{curr}}{nu{m}_{ref}}$小于一定阈值，则需要生产关键帧，返回true

---

CreateNewKeyFrame() 将$CurrFrame\mathrm{设}\mathrm{置}\mathrm{为}\mathrm{一}\mathrm{个}\mathrm{关}\mathrm{键}\mathrm{帧}\mathrm{对}\mathrm{象}，\mathrm{并}\mathrm{插}\mathrm{入}mlNewKeyFrames$

---

LocalMapping流程

函数主体（截取部分）

if(CheckNewKeyFrames())
{
    ProcessNewKeyFrame();
    
    MapPointCulling();
    
    CreateNewMapPoints();
    
    if(!CheckNewKeyFrames())
        SearchInNeighbors();
                
    mbAbortBA = false;
    
    if(!CheckNewKeyFrames() && !stopRequested())
    {
        if(mpMap->KeyFramesInMap()>2)
            Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
        KeyFrameCulling();
    }
    mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
}

CheckNewKeyFrames() ==检查关键帧队列mlNewKeyFrames是否非空==

---

ProcessNewKeyFrame()
==将此帧对地图点的观测信息补充进（融入）全局图，再将此帧作为节点插入共视图和全局地图==

• 计算此关键帧帧的BOW词袋向量
• 获取全局图中被此关键帧观测到的地图点 vpMapPointMatches
• 对vpMapPointMatches中每一个地图点，更新：地图点的被观测数（+1），平均观测方向、最佳描述子等；不够确定的地图点，记录在 mlpLoopKeyFrameQueue，需进一步检查
• 把帧插入共视图：寻找共视关系，更新共视图链接关系
• 把帧插入全局地图

---

MapPointCulling() ==遍历mlpLoopKeyFrameQueue 中地图点P，删除其中质量不好的点==

• P为坏点，直接删除
• $\frac{\mathrm{实}\mathrm{际}\mathrm{上}\mathrm{观}\mathrm{测}\mathrm{到}P\mathrm{的}\mathrm{帧}\mathrm{数}}{\mathrm{理}\mathrm{论}\mathrm{上}\mathrm{应}\mathrm{该}\mathrm{观}\mathrm{测}\mathrm{到}P\mathrm{的}\mathrm{帧}\mathrm{数}}<25\mathrm{\%}$ ，删除P
• 当前关键帧ID - 创建P的帧ID>2（相当于在时间空间上已经进行了一段时间）但实际观测数仍小于一个阈值cnTHObs，删除P

---

CreateNewMapPoints() ==将关键帧中的特征点三角化，创建新MapPoint==

• 拿到Tracking审核得到的关键帧$CurrFrame$后，获取共视程度最高的帧作为相邻关键帧
• 遍历相邻共视关键帧，分别与$CurrFrame$进行特征匹配和三角化，生成地图点
• 生成的地图点记录在mlpLoopKeyFrameQueue，还需进一步检查

---

SearchInNeighbors() ==将所有新的MapPoint融入全局map中，并建立与其他帧的联系== ==将当前关键帧融入全局map中，建立与其他地图点的联系==

• 获取此关键帧在共视图中的一级、二级相邻关键帧，并存放在队列vpTargetKFs中
• 正向投影融合，将$K_c$观测到的MapPoint投影至vpTargetKFs中的关键帧，与其中的特征点建立匹配关系
• 获取一、二级相邻关键帧观测到的MapPoint集合，并存放在vpFuseCandidates中
• 反向投影融合，将vpFuseCandidates中的MapPoint投影到$K_c$，与$K_c$中的特征点建立匹配关系
• 经过以上步骤，新建了许多共视关系，所以还需更新共视图中$K_c$的邻居

---

KeyFrameCulling() ==删除冗余关键帧==

• 获取此关键帧在共视图中的一级相邻关键帧，并存放在队列vpLocalKeyFrames中
• 遍历vpLocalKeyFrames，获取每一共视关键帧vit所观测到的地图点pMP，存放在队列vpMapPoints中
• 遍历vpMapPoints中每一个地图点pMP，根据pMP->GetObservations()遍历所有能观测到pMP的关键帧mit
• 此时， mit、vit都观测到pMP，如果mit对应的关键帧尺度比较小，说明mit对应的关键帧要比vit对应的关键帧更靠近金字塔底部，则mit对pMP观测的效果更好
• 如果pMP被超过3个mit更好地观测，则将冗余点数目nRedundantObservations+1
• 遍历完每个vit和其对应的每个mit后，统计$\frac{nRedundantObservation}{n}$，如果$\frac{nRedundantObservation}{n}$>90% ，删除关键帧$K_c$

---

MapPoint::GetObservations() ==获得所有能观测到MapPoint 的关键帧集合==

---

InsertKeyFrame(mpCurrentKeyFrame) ==将某一帧放入回环检测处理队列==

LoopClosing流程

函数主题

while (1)
	{
    // 检查缓冲队列中是否有关键帧
    if (CheckNewKeyFrames())
    {
		// 检测闭环
        if (DetectLoop())
        {
            // 计算sim3 相似变换
            if (ComputeSim3())
            {
                // 闭环矫正
                CorrectLoop();
            }
        }
    }

GetVectorCovisibleKeyFrames()

==获得该帧的共视帧(已按权值排序)==

DetectLoopCandidates()

==找出初步闭环候选关键帧==

• 遍历当前关键帧的所有单词，找到这些单词的所有关键帧（不包含与当前帧相连的关键帧），作为候选关键帧，放入集合lKFsSharingWords中，并记录候选关键帧与当前帧的共同单词数mnLoopWords；
• 从lKFsSharingWords取出关键帧，确定最小公共单词数为最大公共单词数目的0.8倍，并且与当前帧的相似度得分要大于阈值minScore，符合要求的候选关键帧放入集合lScoreAndMatch；
• 从lScoreAndMatch中取出关键帧$pKFi$，$pKFi$与其前10的共视帧组成一个组，计算这个组的最高得分和累计得分，只留下最高累计得分的0.75倍的组，将留下的组里最高得分的关键帧放入到vpLoopCandidates返回。

SearchByBoW()

==通过词袋向量进行特征匹配==

• 遍历两个关键帧Bow的所有相同node的特征点，通过描述子距离进行匹配；
• 通过距离、比例阈值和角度直方图投票剔除错误匹配。

这里之后可能会算出一个Sim3变换，地图点发生了变化。

SearchBySim3()

==通过Sim3变换相互投影特征匹配==

• 首先计算一些必要的矩阵，进行位姿变换，投影；
• 将关键帧$KF1$的特征点投影到$KF2$，在一个半径范围内就算特征匹配成功；
• 同理，将关键帧$KF2$的特征点投影到$KF1$；
• 保留两次匹配都出现的匹配。

这里之后会对Sim3变换进行优化。

SearchByProjection()

==投影一次，查找特征匹配，范围比较大==

• 跳过**SearchBySim3()**的匹配，遍历闭环$KF$及其共视$KF$的所有地图点，投影到当前$KF$；
• 搜索半径10，搜索新的匹配点。

UpdateConnections()

==更新关键帧之间的共视关系==

• 获取该关键帧的地图点vpMP，遍历地图点，得到所有观测到该地图点的关键帧observations，并记录关键帧共视的地图点个数，关键帧和共视点个数组成一个pair
• 对pair排序，将共视点个数大于阈值$th=15$的关键帧，建立与当前帧的共视关系。

SearchAndFuse()

==融合和替换地图点==

• 遍历待矫正的当前关键帧的相连关键帧；
• 将闭环帧组所有地图点mvpLoopMapPoints，投影到当前关键帧；
• 替换或融合闭环帧组中的地图点mvpLoopMapPoints。