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進行圖優化