【ORB-SLAM3】LoopClosing-回环检测与地图融合详述

1. 基础概念

• 共视图 Covisibility Graph：
  共视图是一个加权无向图，图中每个节点是相机的位姿，如果两个位姿的关键帧拍摄到的相同关键点的数量达到一定值（论文设定为至少15个），则认为两个关键帧具有共视关系。此时两个节点之间便生成了一条边，边的权重与共视点的数量有关。
• 生成树 Spanning Tree:
  Spanning Tree用最少的边连接了所有的关键帧节点（即共视图中所有的节点）。当一个关键帧被加入到共视图当中后，这个关键帧与共视图中具有最多观测点的关键帧之间建立一个边，完成Spanning Tree的增长。
• 本质图 Essential Graph:
  根据共视关系得到的共视图是一个连接关系非常稠密的图，即节点之间有较多的边，而这过于稠密而不利于实时的优化。于是构建了Essential Graph，在保证连接关系的前提下尽可能减少节点之间的边。Essential Graph中的节点依旧是全部的关键帧对应的位姿，连接的边包含三种边：Spanning Tree的边、共视图中共视关系强（共视点数量超过100）的边、以及回环时形成的边。
• Atlas
  Atlas翻译为“地图集”，即管理着一系列的子地图（sub-map），这些子地图共用同一个DBoW数据库，使得能够实现重定位回环等操作。
  当相机在正常跟踪状态，所生成关键帧所在的地图称为“活动地图（active map）”。如果跟踪失败，首先将进行重定位操作寻找地图集中对应的关键帧，如果依旧失败，则重新创建一个新的地图。此时旧的地图变成了“非活动地图（non-active map）”，新的地图作为活动地图继续进行跟踪与建图过程。在跟踪过程中，当前相机必然是位于活动地图当中，可能存在零或多个子地图。
  每次插入关键帧时，都与完整地图的DboW数据库进行匹配。如果发现了相同的场景，且两个关键帧同时位于活动地图，则意味着发生了回环，便按照回环的方式进行融合处理；如果匹配上的关键帧位于非活动地图，则需要将两个子地图进行拼接，这便是ORB-Atlas的创新之处。
  地图无缝融合时，当前活跃的地图吞并对应的非活跃地图。通过一系列步骤将非活跃地图的信息补充到当前活跃地图。具体步骤如下：

1. 检测：首先由重识别模块检测出当前关键帧Ka与匹配上的待吞并关键帧Ks，并获取两个子地图当中与匹配上的两个关键帧具有共视关系的关键点和关键帧。
2. 位姿计算：通过Horn+RANSAC方法初步计算两个关键帧之间的变换关系，之后将待吞并地图的地图点通过这个变换投射到当前关键帧Ka上，再利用引导匹配的方法获得更丰富的匹配并进行非线性优化，获得精确的变换。
3. 地图点合并：将被吞并地图的关键点变换到当前关键帧位姿下，融合重复的地图点。之后将两个地图的关键帧融合，重新生成spanning tree和共视图。
4. 衔接区域的局部BA优化：融合后与Ka具有共视关系的关键帧参与局部BA优化，为避免gauge freedom，固定之前活跃地图中的关键帧而移动其他的关键帧。优化完成后再次进行地图点的合并与spanning tree/共视图的更新。
5. 完整地图的位姿图优化：对整个合并后的地图进行位姿图优化。

---

2.算法流程

2.1算法概述

如基础概念的Atlas 所述，每次插入的关键帧都与完整地图的DboW数据库进行匹配。如果发现了相同的场景，且两个关键帧同时位于活动地图，则意味着发生了回环，便按照回环的方式进行融合处理；如果匹配上的关键帧位于非活动地图，则需要将两个子地图进行拼接， LoopClosing就是完成上述的相似场景发现、回环/拼接任务的线程。
线程启动
LoopClosing线程在System构造函数中初始化与创建，其核心函数为LoopClosing::Run.

在LoopClosing::Run中，线程每个5毫秒执行一次回环检测与地图融合检测操作。主体主体逻辑非常简单，即为上述所述的发现相似场景、进行回环/拼接，主要包含：

1. CheckNewKeyFrames：检测线程中待检测的关键帧序列mlpLoopKeyFrameQueue是否为空
2. NewDetectCommonRegions:检测新的关键帧和关键帧数据库中的帧是否存在相似区域，并更新mbMergeDetected和mbLoopDetected两个变量
3. 基于mbMergeDetected和mbLoopDetected变量进行相应的回环与地图融合操作。
4. 检测复位与回环停止请求。
   

2.2 NewDetectCommonRegions

NewDetectCommonRegions是LoopClosing的关键环节，其任务为确认当前待检测关键帧pKF是否和数据库中的某个关键帧pKFi有相似性，若pKF和pKFi有相似性，且两者位于同一地图，则回环检测标志mbLoopDetected将被置为true，若pKF和pKFi有相似性，且两者位于不同地图，则融合标志位mbMergeDetected将被置为true。
其具体思路为：

1. 调用KeyFrameDatabase::DetectNBestCandidates，在关键数据库里寻找和当前帧相似的候选回环关键帧和候选融合关键帧；
2. 调用LoopClosing::DetectCommonRegionsFromBoW,基于词袋模型判断是否从候选帧中成功检测到融合或者回环。
   在2中，当在候选帧中最相似帧后，会利用最相似帧和当前帧的共视图进行相似度的反校验。
   mnLoopNumCoincidences为回环最相似帧和当前帧共视组相似的帧数；
   mnMergeNumCoincidences为融合最相似帧和当前帧共视组相似的帧数；
3. mnLoopNumCoincidences、 mnMergeNumCoincidences大于3才说明检测到相似/融合。若0<mnLoopNumCoincidences< 3,说明当前帧虽没有满足相似/融合条件，但也和数据库存在相似性，在进行下一关键帧的检测时，会调用DetectAndReffineSim3FromLastKF直接检测下一关键帧和当前帧的最相似帧的相似性，如果也满足相似条件，则NumCoincidences++，但当连续的下两帧相似性均不满足，则清除相关变量，后续待检测关键帧从1.重新开始。

2.2.1 void KeyFrameDatabase::DetectNBestCandidates(KeyFrame *pKF, vector &vpLoopCand, vector &vpMergeCand, int nNumCandidates)

函数名	函数参数	描述
KeyFrameDatabase::DetectNBestCandidates|在数据库中找寻和当前帧最相似的n帧候选帧	*pKF	输入的关键帧，函数将找寻和pKF最为相似的若干候选帧
	修改的变量	描述
	vpLoopCand	若候选帧和pKF在同一地图，则放入vpLoopCand
	vpMergeCand	若候选帧和pKF在不同地图，放入vpMergeCand
	nNumCandidates	候选帧数目

具体思路：

1. 搜索非局部地图共单词关键帧list lKFsSharingWords：在地图中搜索所有和pKF有相同单词的关键帧，并剔除掉其中落在pKF局部地图的关键帧，然后计算剩余每一关键帧和pKF的相同单词个数；
2. 在非局部地图共单词关键帧lKFsSharingWords中进一步筛选满足条件的关键帧（相同单词数>0.8*最大相同单词数），并借助词袋模型计算每帧图像和当前相似度，结果保存在list > lScoreAndMatch中；
3. 遍历 lScoreAndMatch中的每一帧it，并遍历it的共视图中前十权重的关键帧vpNeighs，抽取出vpNeighs落在 lScoreAndMatch中的关键帧并和it组乘一个新的集合，计算该集合的相似度总和以及相似度最高对应的关键帧；将每一个集合的累计得分以及分支最高的关键帧存储在list > lAccScoreAndMatch;
4. 对 lScoreAndMatch按照分支进行排序；
5. 遍历 lAccScoreAndMatch,并返回前n个关键帧，其中若关键帧和pKF在同一地图，则将关键帧放入vpLoopCand，否则放入vpMergeCand；

2.2.2 bool LoopClosing::DetectCommonRegionsFromBoW(std::vector &vpBowCand, KeyFrame* &pMatchedKF2, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw,int &nNumCoincidences, std::vector &vpMPs, std::vector &vpMatchedMPs)

函数名	函数参数	描述
DetectCommonRegionsFromBoW | 基于词袋模型判断是否从候选帧中成功检测到相似帧	vpBowCand	候选帧
	修改的变量	描述
	pMatchedKF2	候选帧中和当前帧最相似的关键帧
	pLastCurrentKF	上一当前帧
	g2oScw	回环或者重定位后计算得到的世界坐标系到当前帧的sim3变换
	nNumCoincidences	用计算的sim3变换检验当前帧的共视帧组，检验通过的帧数
	vpMPs	候选帧及其共视帧的所有地图点
	vpMatchedMPs	候选帧及其共视帧的所有地图点中满足g2oScw投影变换的点

具体思路：

1. 获取和当前帧相连（和当前帧有共视地图点）的关键帧组spConnectedKeyFrames
2. 遍历候选帧组vpBowCand获取每一帧pKFi,提取pKFi的前10共视帧，并和pKFi组成候选帧的共视帧组vpCovKFi;
3. 遍历vpCovKFi，若其中有某一帧出现在spConnectedKeyFrames中，说明检测到的候选帧是距离较近的关键帧，则直接进行下一个pKFi的处理；
4. 否则，将vpCovKFi中的每一关键帧和当前帧对比，进行特征点关联，进而找到特征点关联的地图点，将地图点保存在vvpMatchedMPs[j]变量中，vvpMatchedMPs[j]是一个数组，数组序号是当前帧特征点序号，存储内容是vpCovKFi[j]对应的地图点
5. 汇总上述地图点，将结果存在vpMatchedPoints和vpKeyFrameMatchedMP中，其中，vpMatchedPoints代表候选帧的共视帧组和当前帧对应的地图点，vpKeyFrameMatchedMP代表vpMatchedPoints中地图点对应的关键帧
6. 如果找到的地图点数大于设定阈值，则求解sim3变换，基于变换矩阵进行重投影，计算候选帧的共视帧组中满足投影关系的地图点个数，并将结果保存在vpMatchedMP与vpMatchedKF中，vpMatchedMP保存满足重投影的地图点，vpMatchedKF保存对应的共视帧。
7. 若重投影成功的地图点数大于设定阈值，进行sim3优化
8. 基于优化后的sim3，再一步计算满足重投影的地图点数
9. 若满足重投影的地图点数大于阈值，获取当前帧的前10共视帧
10. 判断sim3变换满足当前帧的共视帧 帧数
11. 如果满足的帧数大于3，说明回环检测成功。

2.2.3 bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,std::vector &vpMPs, std::vector &vpMatchedMPs)

函数名	函数参数	描述
DetectAndReffineSim3FromLastKF | 判断当前帧pCurrentKF和pMatchedKF是否满足相似关系	pCurrentKF	当前帧
	pMatchedKFw	相似的关键帧
	修改的变量	描述
	gScw	世界坐标系到当前帧的sim3粗略估计，该函数会对其进一步优化
	nNumProjMatches	相似帧中和当前帧成功关联上的地图点数目
	vpMPs	相似帧及其共视帧的所有地图点
	vpMatchedMPs	相似帧及其共视帧的所有地图点中和当前帧成功关联的地图点

具体思路：

1. 调用FindMatchesByProjection，找出符合gScw变换关系的地图点集合vpMatchedMPs以及地图点数nNumProjMatches,如果nNumProjMatches大于阈值，则对相似帧到当前帧的变化矩阵gScm进行优化，并返回满足gScm变化的地图点数numOptMatches。
2. 如果numOptMatches大于阈值，再一次调用FindMatchesByProjection检验满足优化后的gScw_estimation的地图点个数，如果nNumProjMatches大于阈值，函数返回true，认为当前帧pCurrentKF和pMatchedKFw具有相似关系。

2.2.4 int LoopClosing::FindMatchesByProjection(KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, g2o::Sim3 &g2oScw,set pMatchedMPinOrigin, vector &vpMapPoints, vector &vpMatchedMapPoints)

函数名	函数参数	描述
FindMatchesByProjection | 基于重投影将相似帧的地图点和当前帧的特征点进行关联	pCurrentKF	当前帧
	pMatchedKFw	相似的关键帧
	修改的变量	描述
	g2oScw	回环或者重定位后计算得到的世界坐标系到当前帧的sim3粗略估计，该函数会对其进一步优化
	spMatchedMPinOrigin	无效变量，没有使用到
	vpMapPoints	相似帧及其共视帧的所有地图点
	vpMatchedMapPoints	相似帧及其共视帧的所有地图点中和当前帧成功关联的地图点

具体思路：

1. 获取相似帧前10最优关键帧，并和相似帧组成相似帧集合vpCovKFm
2. 若集合vpCovKFm中帧数小于10，则添加相似帧的所有二级共视帧。（这部分逻辑有误，但影响不大，可修改为，若集合vpCovKFm中帧数小于10，则添加相似帧的二级共视帧直至vpCovKFm中帧数大于10，要求二级共视帧不落在当前帧的共视帧集合中以及相似帧的一级共视帧集合中）
3. 遍历vpCovKFm,将其中所有的地图点添加入地图点集合spMapPoints中，spMapPoints中的地图点两两互斥；
4. 调用ORBmatcher::SearchByProjection,将spMapPoints中的地图点进行重投影，返回关联成功的地图点集合vpMatchedMapPoints，同时记录关联成功的地图点数num_matches
5. 函数返回num_matches

2.2.5 int ORBmatcher::SearchByProjection(KeyFrame* pKF, Sophus::Sim3f &Scw, const vector &vpPoints,vector &vpMatched, int th, float ratioHamming)

函数名	函数参数	描述
SearchByProjection | 基于重投影将地图点和关键帧特征点进行关联	pKF	关键帧
	Scw	世界坐标系到当前帧的转化矩阵（回环、融合后）
	vpPoints	待关联的地图点
	th	搜索半径基值（还需乘以金字塔缩放系数得到真正的搜索半径）
	ratioHamming	汉明距离比例，最优点距离小于ratioHamming*TH_LOW标定关联成功
	修改的变量	描述
	vpMatchedMapPoints	关联成功的地图点集合，其中vpMatchedMapPoints[i]表示和pKF第i个特征点关联的地图点，若无则为空指针

具体思路：

1. 遍历地图点集合vpPoints，若地图点是bad的，进行下一个地图点的关联
2. 将世界坐标系下的地图点通过Scw转换到当前帧相机坐标系下，若相机坐标系下该地图点的深度小于0，则进行下一个地图点的关联
3. 通过pKF的相机内参将地图点投影到图像上获取像素uv，若投影的像素点超出图像边界，则进行下一个地图点的关联
4. 获取地图点的最远最近距离，若地图点与当前帧光心距离超出该范围，则进行下一个地图点的关联
5. 获取地图点的法向量Pn，计算地图点和当前帧的光心构成的向量PO,若两者向量夹角超过60°，则进行下一个地图点的关联
6. 若上述条件都满足，估计地图点在pKF中的金字塔层级nPredictedLevel，利用nPredictedLevel与th确定搜索半径r，以uv为圆心，r为半径，搜索落在其中的图像特征点序号vIndices。
7. 遍历vIndices，计算特征点描述子和地图点描述子的汉明距离，获取最小距离对应的特征点序号，如果最小距离小于阈值，则关联成功，nmatches++,结果保存在vpMatched[bestIdx]中。

2.2.6 void KeyFrameDatabase::DetectBestCandidates(KeyFrame *pKF, vector &vpLoopCand, vector &vpMergeCand, int nMinWords)

函数名	函数参数	描述
KeyFrameDatabase::DetectBestCandidates 获取满足条件的所有候选帧	KeyFrame *pKF	输入的关键帧，函数将找寻和pKF最为相似的若干候选帧
	vector KeyFrame* &vpLoopCand	若候选帧和pKF在同一地图，则放入vpLoopCand
	vectorKeyFrame* &vpMergeCand	若候选帧和pKF在不同地图，放入vpMergeCand
	int nMinWords	候选帧和pKF共视点最低阈值

具体思路：

1. 搜索非局部地图共单词关键帧list lKFsSharingWords：在地图中搜索所有和pKF有相同单词的关键帧，并剔除掉其中落在pKF局部地图的关键帧，然后计算剩余每一关键帧和pKF的相同单词个数；
2. 在非局部地图共单词关键帧lKFsSharingWords中进一步筛选满足条件的关键帧（相同单词数>0.8*最大相同单词数），并借助词袋模型计算每帧图像和当前相似度，结果保存在list > lScoreAndMatch中；
3. 遍历 lScoreAndMatch中的每一帧it，并遍历it的共视图中前十权重的关键帧vpNeighs，抽取出vpNeighs落在 lScoreAndMatch中的关键帧并和it组乘一个新的集合，计算该集合的相似度总和以及相似度最高对应的关键帧；将每一个集合的累计得分以及分支最高的关键帧存储在list > lAccScoreAndMatch;
4. 遍历 lAccScoreAndMatch,并返回满足条件（累计得分>0.75*最大累计得分）的关键帧，其中若关键帧和pKF在同一地图，则将关键帧放入vpLoopCand，否则放入vpMergeCand；

函数名	函数参数	描述
KeyFrame::UpdateConnections | 更新关键帧间的连接关系，包括更新共视图，父子关键帧	bool upParent	若候选帧和pKF在同一地图，则放入vpLoopCand
	修改的变量	描述
	std::map KeyFrame*,int mConnectedKeyFrameWeights	map (KeyFrame*,int) (有共视点的关键帧，共视点数)
	std::vector KeyFrame* mvpOrderedConnectedKeyFrames	将共视图中的关键帧按照共视点个数进行排序得到的关键帧集合
	std::vector int mvOrderedWeights	共视图中的共视点个数排序
	KeyFrame* mpParent;	父关键帧

2.3 地图融合概述

当NewDetectCommonRegions()将融合检测标志mbMergeDetected置为true后，线程开始进入地图融合部分。若传感器中包含IMU，在地图融合前还需满足

• 当前帧所在地图完成IMU初始化
• 表示当前帧世界坐标系到融合帧世界坐标系的相似变换矩阵，其尺度缩放值大于0.9，小于1.1。
• 将当前世界坐标系到融合帧坐标系的旋转矩阵进行修改，使其仅剩沿yaw的变换

满足上述条件后，若系统含有IMU，则执行MergeLocal2将融合帧所在地图融合到当前帧所在地图，若系统不含IMU则执行MergeLocal将当前帧所在地图融合到融合帧所在地图。

2.3.1 MergeLocal2()

主要思路：

1. 停止全局BA线程mpThreadGBA，停止局部建图线程mpLocalMapper->RequestStop()
2. 利用当前世界坐标系到融合帧世界坐标的变换矩阵mSold_new，将当前地图中的关键帧位姿、关键帧速度、地图点位置、IMU到在世界坐标系下的坐标mOwb转换到融合帧的世界坐标系下。
3. 利用mSold_new更新每个帧之间的相互变换矩阵mlRelativeFramePoses，更新IMU预积分的结果，将IMU的预积分帧更新。
4. 如果当前地图还没完成BA2，则进行地图初始化
5. 遍历融合帧地图中的所有关键帧与地图点，将其添加进当前帧所在地图
6. 获取当前帧所在地图中的所有关键帧，将其位姿保存在NonCorrectedSim3
7. 更新生成树的连接关系，融合帧的父关键帧变成融合帧的子关键帧，融合帧新的父关键帧为当前帧。如下图，箭头方向表示父关键帧。
   
8. 维护当前帧及当前帧的共视图数组vpCurrentConnectedKFs、融合帧及融合帧的共视图数组mvpMergeConnectedKFs、mvpMergeConnectedKFs数组中的地图点集合spMapPointMerge，将spMapPointMerge中的点复制进vpCheckFuseMapPoint。
9. 调用SearchAndFuse，搜寻当前帧及当前帧的共视图数组vpCurrentConnectedKFs中可用vpCheckFuseMapPoint中的地图点替代的地图点。
10. 更新vpCurrentConnectedKFs、mvpMergeConnectedKFs中关键帧的连接关系。
11. 执行局部BAOptimizer::MergeInertialBA。

2.3.1.1 void LoopClosing::SearchAndFuse(const vector &vConectedKFs, vector &vpMapPoints)

函数名	函数参数	描述
SearchAndFuse | 搜索关键帧数组中能用vpMapPoints代替的地图点	vConectedKFs	关键帧数组
	vpMapPoints	地图点

主要思路：

1. 遍历vConectedKFs中每一关键帧，获取关键帧的位姿Scw
2. 调用ORBmatcher::Fuse，获取vpReplacePoint，即地图点vpReplacePoint[i]可用vpMapPoints[i]代替。
3. 遍历vpReplacePoints，调用MapPoint::Replace进行地图点替换。

2.3.2 MergeLocal()

主要思路:

1. 停止全局BA线程mpThreadGBA，停止局部建图线程mpLocalMapper->RequestStop()
2. 获取当前帧及其共视帧spLocalWindowKFs、spLocalWindowKFs的地图点spLocalWindowMPs。融合帧及其共视帧spMergeConnectedKFs，spMergeConnectedKFs的地图点spMapPointMerge。将spMapPointMerge中的地图点复制到vpCheckFuseMapPoint
3. 遍历spLocalWindowKFs，spLocalWindowMPs将其位姿更新至融合帧坐标系下，并在融合帧所在地图增加关键帧及地图点。
4. 更新帧连接关系
5. 调用LoopClosing::SearchAndFuse进行地图点融合
6. 调用 Optimizer::LocalBundleAdjustment进行优化
7. 将当前帧中剩余地图点与关键帧加入融合帧中
8. 如果是单目，还要进行本质图优化
9. 调用LoopClosing::RunGlobalBundleAdjustment进行全局优化