转载请注明原创地址：https://blog.csdn.net/qq_30356613/article/category/6897125
ORBSLAM中的回环检测和重定位具有类似的方式，所以我们这里将两个部分放到同一篇文章中讲解。首先看tracking线程中的重定位。
一．Tracking重定位
重定位模式是在系统处于LOST（追踪线程失败）的情况下进行的系统挽救工作，它的基本流程是：
（1）计算当前帧的BoW映射
（2）找到与当前帧相似的候选关键帧
（3）匹配当前帧与找到的候选关键帧，计算相机位姿Tcw（RANSAC+EPNP算法）
（4）优化相机位姿Tcw
首先进行优化，将优化的结果存到nGood中，
如果优化结果不理想进行映射匹配，然后再进行优化
如果优化结果还不理想缩小映射窗口后在进行匹配并优化
此时若还不理想就判定该候选关键帧不能与本帧形成匹配，继续进行下一关键帧的匹配;如果可以，则证明已经进行重定位或者找到回环，退出循环
经过上述重定位之后如果失败，那么进行下一帧
如果成功则，追踪局部地图并添加当前关键帧等，系统进入OK状态。
这里有几点说明：
1 首先是如何寻找当前帧的候选关键帧
Tracking线程的重定位使用词袋模型进行查找。
首先我们在建立关键帧时，我们对于每一个关键帧都提取BOW向量和Feature向量，并将他们加入到KeyFrameDatabase中（回环检测时），关于KeyFrameDatabase的维护见文章（ORBSLAM的tracking）补充说明（4），我们在选取当前帧的重定位候选关键帧时遍历当前帧的所有BOW向量，并从KeyFrameDatabase中寻找这些BOW向量节点id下的所有其他关键帧作为初步的候选关键帧。
然后计算这些候选关键帧与当前待重定位关键帧之间的共视节点数（特征向量在相同的BOW节点下），从而找出其中最大的共视节点数以此计算最小共视节点数阈值，以此来筛选掉那些共视节点数小的候选关键帧
通过这些候选帧的共视关键帧从而计算累计重定位得分（重定位得分是指候选关键帧和待重定位帧之间的BOW向量距离，累计重定位得分是指将候选帧和共视关键帧的重定位得分加在一起得到的分数），通过累计重定位得分来进一步筛选得分少的候选关键帧。这其中的候选关键帧发生了改变，用共视关键帧中重定位得分最高的关键帧来代替候选关键帧作为新的候选关键帧加入。
通过上述三步得到当前待重定位帧的重定位候选关键帧。然后根据这些候选关键帧作进一步的操作
2 RANSAC+PNP算法的使用
对于1中得到的候选关键帧我们进一步将其与待重定位帧进行匹配，得到匹配点，匹配的方式见另一篇文章（ORBSLAM中的特征匹配）通过这些匹配点进一步计算相机位姿，这里的相机位姿计算与tracking线程OK状态下的位姿跟踪不同，tracking线程中的位姿跟踪是给定当前相机位姿的初始位姿（为上一帧相机位姿或者根据上一帧相机位姿和相机位姿的变化速度作为初始值）和匹配地图点进行位姿的优化得到当前位姿，而由于重定位无法获得上一帧的位姿和当下的位姿速度给定位姿初始值，因此我们使用PNP的方式根据待重定位帧和候选关键帧的几何关系来计算待重定位帧的初始相机位姿，然后根据初始位姿和匹配点优化位姿从而确定最终的位姿。其中位姿优化我们在另一篇文章（ORBSLAM中的优化问题）中讲解。RANSAC+PNP算法（实际使用的是EPNP算法）（见另一篇文章（ORBSLAM中的PNP解决方案））。
二．Localmapping回环检测
回环检测作为系统的一部分，对于维护系统的精度和鲁棒性是十分重要的，他的工作模式和Tracking线程的重定位有相似之处，但也大不相同。主要流程如下：
1．检测是否有新关键帧加入回环检测线程
2．如果有新的关键帧则检测该关键帧与之前关键帧是否发生回环，选定连续候选回环关键帧
3．计算回环候选关键帧和当前待检测回环关键帧之间的相似变换矩阵
4. 如果可以得到一个合适的相似变换矩阵证明回环候选关键帧和当前待检测回环关键帧之间发生了回环，根据回环开始进行全局的位姿地图点全优化。
5. 根据优化结果更新全局地图中的所有关键帧位姿和地图点三维坐标。
那么存在几个问题：
1 如何检测回环
（1）取出关键帧之后提取待检测关键帧的候选回环关键帧
回环检测中的候选回环关键帧使用词袋模型进行查找。
首先我们在建立关键帧时，我们对于每一个关键帧都提取BOW向量和Feature向量，并将他们加入到KeyFrameDatabase中（回环检测时），关于KeyFrameDatabase的维护见文章（ORBSLAM的tracking）补充说明（4），我们在选取当前帧的重定位候选关键帧时遍历当前帧的所有BOW向量，并从KeyFrameDatabase中寻找这些BOW向量节点id下的所有其他关键帧作为初步的关键帧，然后应注意为了避免相邻帧会出现回环检测的现象（如果相邻帧也发生回环检测的话对于系统精度的提高作用不大，而且还会大幅降低系统的实时性），我们将这些关键帧中与当前待回环关键帧是共视关键帧的关键帧去掉，去除共视关键帧之后构成了初步的候选回环关键帧。
然后计算这些候选关键帧与当前待回环关键帧之间的共视节点数（特征向量在相同的BOW节点下），从而找出其中最大的共视节点数以此计算最小共视节点数阈值，以此来筛选掉那些共视节点数小的候选关键帧
通过这些候选帧的共视关键帧从而计算累计回环检测得分（重定位得分是指候选关键帧和待重定位帧之间的BOW向量距离，累计回环检测得分是指将候选帧和共视关键帧的回环检测得分加在一起得到的分数），通过累计回环检测得分来进一步筛选得分少的候选关键帧。这其中的候选关键帧发生了改变，用共视关键帧中回环检测得分最高的关键帧来代替候选关键帧作为新的候选关键帧加入。
通过上述三步得到当前待重定位帧的重定位候选关键帧。然后根据这些候选关键帧作进一步的操作
注意这里的回环检测候选关键帧和重定位候选关键帧的筛选方式基本一致，除了回环检测候选关键帧不包括当前关键帧的共视关键帧。
（2）在候选关键帧中检测具有连续性的候选关键帧
首先，每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”，vpCandidateKFs-->spCandidateGroup
然后，检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在nCurrentConsistency++，则将该“子候选组”放入“当前连续组vCurrentConsistentGroups”
最后，如果nCurrentConsistency大于等于3，那么该“子候选组”代表的候选帧过关，进入mvpEnoughConsistentCandidates
筛选后得到的具有连续性的候选帧，存储在mvpEnoughConsistentCandidates中
选取连续性的候选关键帧作为回环候选关键帧的原因：我们通过聚类相连候选关键帧，可以将一些得分很高但却相对独立的帧给去掉，这些帧与其他帧相对没有关联，而我们知道事实上回环处会有一定时间和空间上的连续性，因此对于正确的回环来讲，这些独立的相似性评分较高的帧时错误的候选回环关键帧
2 如何计算相似矩阵
这里的关键算法在于如何计算两关键帧之间的相似变换矩阵，这里与重定位有所不同，重定位计算的是两帧之间的欧式变换矩阵，相似变换矩阵与欧式变换矩阵相比较多了尺度因子，计算方法当然也会有所不同。这里主要参考论文:Closed-form solution of absolute orientation using unit quaternions.论文主要内容参考另一篇文章ORBSLAM中的关键帧相似变换矩阵的计算。
这里主要介绍在ORBSLAM中相似矩阵的计算流程
1> 首先通过BOW匹配计算当前关键帧和候选关键帧之间的匹配地图点（函数SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector<MapPoint*> &vpMatches12)详情见另一篇文章ORBSLAM中的特征匹配），根据匹配地图点通过sim矩阵求解器求解两关键帧之间的相似变换矩阵，之后根据计算得到的相似变换矩阵重新匹配当前关键帧和候选关键帧之间的匹配地图点（函数SearchBySim3详情见另一篇文章ORBSLAM中的特征匹配），根据计算的匹配地图点对相似变换矩阵进行优化（OptimizeSim3函数详情见另一篇文章ORBSLAM中的优化问题），并根据优化得到内点数量，根据内点数量判断当前计算的sim矩阵的准确性。
2> 通过回环候选关键帧的共视关键帧并匹配共视关键帧的地图点和当前关键帧，相当于是匹配covisibility graph和当前关键帧（SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector<MapPoint*> &vpPoints, vector<MapPoint*> &vpMatched, int th)函数详情见另一篇文章ORBSLAM中的特征匹配）,通过此次匹配来计算内点数量并确定该候选回环关键帧是否真的是回环，计算得到的sim矩阵是否适合大部分匹配地图点。
3 在真的检测到回环之后如何对整个SLAM系统进行校正
回环之后对系统怎样进行校正关键在于两次优化，一次是根据四种边（1新检测到的回环边 2 父关键帧的边 3 历史回环边 4 共视图边）对全局地图中的所有关键帧的位姿（相似变换矩阵）进行校正；另一次是根据地图点和关键帧位姿计算重投影误差对全局地图进行优化。两种优化详见另一篇文章ORBSLAM中的优化问题。
校正流程：
1> 请求局部地图线程停止,并且中止现有的全局优化进程
2> 根据当前帧求得的相机位姿(相似变换矩阵)来求解矫正前和矫正后的相邻帧位姿变换矩阵(相似变换矩阵)
3> 将相邻关键帧的所有地图点都根据更新后的相机位姿(相似变换矩阵)重新计算地图点世界坐标
4> 进行地图点融合 将之前匹配的(在ComputeSim3()函数中计算局部地图点和当前帧的匹配)两地图点融合为同一地图点
5> 根据第3步中计算的地图点重新进行匹配,并融合匹配点和当前关键帧中的地图点
6> 在地图点融合之后,更新当前关键帧的共视图中各个关键帧的相连关键帧,更新连接之后,将这些相邻关键帧全部加入LoopConnections容器
7> 根据四种边(1 新检测到的回环边 2 父关键帧与子关键帧的边 3 历史回环关键帧 4 共视图边)对全局地图中的所有关键帧的位姿进行矫正
8> 根据地图点和关键帧位姿计算重投影误差对全局地图进行优化