ORB_SLAM2--计算每一个回环候选关键帧与当前关键帧之间的相似矩阵

在确认回环存在之后，需要计算出每一个回环候选关键帧与当前关键帧之间的相似矩阵[sR|t]。对于双目和RGBD来说s=1.

step1

初始化当前帧和候选关键帧的相似矩阵求解器,计算匹配地图点,取消回环关键帧标志变量的赋初值。 通过BOW进行匹配，如果改帧的匹配特征点少于20，则直接删除。

 for(int i=0; i<nInitialCandidates; i++)
 {
     KeyFrame* pKF = mvpEnoughConsistentCandidates[i];

     // avoid that local mapping erase it while it is being processed in this thread
     pKF->SetNotErase();

     if(pKF->isBad())
     {
         vbDiscarded[i] = true;
         continue;
     }
// 待回环关键帧与回环候选关键帧进行匹配得到匹配地图点
     int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);

     if(nmatches<20)
     {
         vbDiscarded[i] = true;
         continue;
     }
     else
     {
//相似性矩阵的求解器初始化
         Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
         pSolver->SetRansacParameters(0.99,20,300);
         vpSim3Solvers[i] = pSolver;
     }

     nCandidates++;
 }

step2

int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);

if(nmatches<20)
{
    vbDiscarded[i] = true;
    continue;
}
else
{
//相似性矩阵的求解器初始化
    Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
    pSolver->SetRansacParameters(0.99,20,300);
    vpSim3Solvers[i] = pSolver;
}

step2

进行sim3求解，求解之后进行inliner检测，只要有一次的是合格的就可以返回，最多只能迭代5次。

Sim3Solver* pSolver = vpSim3Solvers[i];
cv::Mat Scm  = pSolver->iterate(5,bNoMore,vbInliers,nInliers);

step3

通过BOW进行的匹配，算出大致的匹配区域再进行Sim3优化

vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));
for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
{
    if(vbInliers[j])
       vpMapPointMatches[j]=vvpMapPointMatches[i][j];
}

cv::Mat R = pSolver->GetEstimatedRotation();
cv::Mat t = pSolver->GetEstimatedTranslation();
const float s = pSolver->GetEstimatedScale();
matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);

剔除误差过大的边（卡方检验，这里设定的阈值是10），

g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);

然后继续对剩下的边进行优化，得到优化结果，只要优化后的nInliers大于等于20就认为通过回环检验。同时停止对其他候选帧的优化。

  if(nInliers>=20)
  {
      bMatch = true;
      mpMatchedKF = pKF;
      g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);
      mg2oScw = gScm*gSmw;
      mScw = Converter::toCvMat(mg2oScw);

      mvpCurrentMatchedPoints = vpMapPointMatches;
      break;
  }

step4

取出闭环匹配上关键帧（也就是刚刚通过回环检验的这一帧）的相邻关键帧,提取出来得到一个集合（同时把匹配上的关键帧也加入到这个集合中），

vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
vpLoopConnectedKFs.push_back(mpMatchedKF);

再把它们所对应的Map Point取出来（这里也包括匹配上的关键帧的Map Point，不过这个需要标记一下，避免重复添加），得到一个集合。

 
mvpLoopMapPoints.clear();
for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
{
    KeyFrame* pKF = *vit;
    vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
    for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
    {
        MapPoint* pMP = vpMapPoints[i];
        if(pMP)
        {
            if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId)
            {
                mvpLoopMapPoints.push_back(pMP);
                pMP->mnLoopPointForKF=mpCurrentKF->mnId;
            }
        }
    }
}

然后把这些Map Point全部投影到当前关键帧上，进行匹配，根据Sim3确定一个大致区域，然后在附近区域搜索，得到匹配。

matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);

如果有足够的匹配点 则说明找到了回环,否则查找回环失败.

    int nTotalMatches = 0;
    for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
    {
        if(mvpCurrentMatchedPoints[i])
            nTotalMatches++;
    }

    if(nTotalMatches>=40)
    {
        for(int i=0; i<nInitialCandidates; i++)
            if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
                mvpEnoughConsistentCandidates[i]->SetErase();
        return true;
    }
    else
    {
        for(int i=0; i<nInitialCandidates; i++)
            mvpEnoughConsistentCandidates[i]->SetErase();
        mpCurrentKF->SetErase();
        return false;
    }