3、ORBSLAM闭环检测终局之战之sim3计算流程，确定是否检测到闭环

前两篇博客讲了闭环候选关键帧是怎么层层选拔出来的，详情请看链接1和链接2.等选拔出足够的闭环候选关键帧以后，就要在里面挑选真正能构成闭环的闭环关键帧了。这就是通过计算sim3变换得到的。

1、为什么要计算sim3？

sim3是三维空间的相似变换，计算sim3实际就是计算三个参数$R,t,s$。具体怎么计算等以后再写一篇博客，今天只看通过计算sim3确定闭环关键帧的流程。
我们知道闭环检测通常发生在距离很远的两个关键帧之间的，而他们的位姿都是由临近信息得到的，这说明期间有了很大的累计误差（单目尺度漂移有尺度误差，双目RGBD认为没有尺度误差），所以闭环时强行将两帧的位姿接上，很可能发生明显的错位。

sim3就是考虑两关键帧的共视关键帧这样的两个关键帧组的信息，将他们重新连接，做一个软过渡，尽可能将累计误差分摊到两个关键帧组上去，也就是闭环调整。这个在看代码实际运行时可以明显地感受到，闭环时，很平滑的调整了两帧周围的关键帧将两帧连接在一起了，看起来特别舒服。

2、sim3计算流程

整个计算sim3，确定闭环帧的流程，如下图。（可对照代码看）

Sim3 计算流程说明：
1. 通过Bow加速描述子的匹配，利用RANSAC粗略地计算出当前关键帧与闭环候选关键帧的Sim3（当前关键帧—闭环候选关键帧）
2. 根据估计的Sim3，对3D点进行投影找到更多匹配，通过优化的方法计算更精确的Sim3（当前关键帧—闭环候选关键帧）
3. 将闭环候选关键帧以及闭环候选关键帧共视的关键帧的MapPoints与当前关键帧的点进行匹配（当前关键帧—闭环候选关键帧+相连关键帧）–看代码后面的图。
对于双目或者是RGBD输入的情况,计算得到的尺度=1

bool LoopClosing::ComputeSim3()
{
   
    //  准备工作
    // 对每个（上一步层层选拔出来的）闭环候选关键帧都准备算一个Sim3，上图中以2个画的流程。
    const int nInitialCandidates = mvpEnoughConsistentCandidates.size();//闭环候选关键帧的数量给到初始化候选nInitialCandidates，用作后面初始化

	//定义了一个匹配类的对象 最佳与次佳阈值0.75  检查旋转
    ORBmatcher matcher(0.75,true);

    // 用 vector 存储每一个闭环候选关键帧的Sim3Solver求解器
    vector<Sim3Solver*> vpSim3Solvers;
    vpSim3Solvers.resize(nInitialCandidates);//用闭环候选关键帧的数量（初始化候选）初始化闭环候选关键帧的Sim3Solver求解器的个数

    // 用 vector 存储每个候选关键帧的匹配地图点信息
    vector<vector<MapPoint*> > vvpMapPointMatches;//定义闭环候选关键帧匹配地图点，二层vector，外层是每一个闭环候选关键帧，内层是每一个闭环候选关键帧的匹配地图点
    vvpMapPointMatches.resize(nInitialCandidates);//用闭环候选关键帧的数量（初始化候选）初始化闭环候选关键帧的匹配地图点信息，初始化外层vector个数

    // 用 vector 存储每个候选帧应该被放弃(True）或者 保留(False)
    vector<bool> vbDiscarded;
    vbDiscarded.resize(nInitialCandidates);//用闭环候选关键帧的数量（初始化候选）初始化是否要丢弃的闭环候选关键帧

    // 完成 Step 1 的匹配后，被保留的候选关键帧数量
    int nCandidates=0;

    // Step 1. 遍历闭环候选关键帧集，初步筛选出与当前关键帧的匹配特征点数大于20的闭环候选关键帧集合，并为每一个闭环候选关键帧构造一个Sim3Solver
    for(int i=0; i<nInitialCandidates; i++)//循环闭环候选关键帧
    {
   
        // Step 1.1 从筛选的闭环候选帧中取出一帧有效关键帧pKF
        KeyFrame* pKF = mvpEnoughConsistentCandidates[i];//取出当前循环的闭环候选关键帧

        // 避免在LocalMapping中KeyFrameCulling函数将此关键帧作为冗余帧剔除
        pKF->SetNotErase();

        // 如果候选帧质量不高，直接PASS
        if(pKF->isBad())
        {
   
            vbDiscarded[i] = true;//标记丢弃
            continue;
        }

        // Step 1.2 将当前关键帧 mpCurrentKF 与闭环候选关键帧 pKF 匹配
        // 通过bow加速得到 mpCurrentKF 与 pKF 之间的匹配特征点
        // vvpMapPointMatches 是匹配特征点对应的地图点,本质上来自于候选闭环关键帧
        int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i])