ORB_SLAM2--回环矫正算法

step1

请求局部地图线程停止,并且中止现有的全局优化进程

    // Send a stop signal to Local Mapping
    // Avoid new keyframes are inserted while correcting the loop
    mpLocalMapper->RequestStop();

    // If a Global Bundle Adjustment is running, abort it  如果正在运行全局BA优化,那么终止它
    if(isRunningGBA())
    {
        unique_lock<mutex> lock(mMutexGBA);
        mbStopGBA = true;

        mnFullBAIdx++;

        if(mpThreadGBA)
        {
            mpThreadGBA->detach();
            delete mpThreadGBA;
        }
    }

    // Wait until Local Mapping has effectively stopped     等待局部地图线程已经完全停止
    while(!mpLocalMapper->isStopped())
    {
        usleep(1000);
    }

step2

根据当前帧求得的相机位姿(相似变换矩阵)来求解矫正前和矫正后的相邻帧位姿变换矩阵(相似变换矩阵)

矫正前和矫正后的位姿

    KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;

遍历所有的相邻关键帧，计算矫正后和矫正前的相邻关键帧的相机位姿(相似变换矩阵)

for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
{
    KeyFrame* pKFi = *vit;

    cv::Mat Tiw = pKFi->GetPose();

    if(pKFi!=mpCurrentKF)  // 将当前帧的相邻关键帧的相机位姿都根据当前帧的相似变换矩阵进行矫正
    {
        cv::Mat Tic = Tiw*Twc;
        cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);
        cv::Mat tic = Tic.rowRange(0,3).col(3);
        g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
        g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;
        //Pose corrected with the Sim3 of the loop closure
        CorrectedSim3[pKFi]=g2oCorrectedSiw;
    }

    cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);
    cv::Mat tiw = Tiw.rowRange(0,3).col(3);
    g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);
    //Pose without correction
    NonCorrectedSim3[pKFi]=g2oSiw;
}

step4

进行地图点融合 将匹配的两地图点融合为同一地图点

  for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
  {
      if(mvpCurrentMatchedPoints[i])
      {
          MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];
          MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i);
          if(pCurMP)
              pCurMP->Replace(pLoopMP);
          else
          {
              mpCurrentKF->AddMapPoint(pLoopMP,i);
              pLoopMP->AddObservation(mpCurrentKF,i);
              pLoopMP->ComputeDistinctiveDescriptors();
          }
      }
  }

step5

根据矫正后的相机相似矩阵位姿重新匹配回环点和当前关键帧,并融合得到的关键帧中匹配点和回环地图点
SearchAndFuse(CorrectedSim3);

step6

在地图点融合之后,更新当前关键帧的共视图中各个关键帧的相连关键帧,更新连接之后,将这些相邻关键帧全部加入LoopConnections容器

    for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
    {
        KeyFrame* pKFi = *vit;
        vector<KeyFrame*> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();

        // Update connections. Detect new links.   更新链接,将当前帧的关联关键帧加入LoopConnections容器,不包括直接相邻的和当前帧的关联关键帧
	// 实际就是一系列回环的集合,保证回环的连续性
        pKFi->UpdateConnections();
        LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames();
        for(vector<KeyFrame*>::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++)
        {
            LoopConnections[pKFi].erase(*vit_prev);
        }
        for(vector<KeyFrame*>::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++)
        {
            LoopConnections[pKFi].erase(*vit2);
        }
    }

step7

进行位姿图优化
这里的优化对象由三个部分组成，扩展树连接关系、闭环产生的连接关系和一些共识关系非常好的边（这里设定的是共识权重超过100的），接着三部分组成的图进行优化。

   Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);

step8

开辟线程进行全局BA进行图优化