认真的虎ORBSLAM2源码解读(十二)：LoopClosing闭环检测图示详解

1.简述

2.头文件

class LoopClosing
{
   
   
public:

    typedef pair<set<KeyFrame*>,int> ConsistentGroup;    
    typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
        Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose;

public:

    LoopClosing(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale);

    void SetTracker(Tracking* pTracker);

    void SetLocalMapper(LocalMapping* pLocalMapper);

    // Main function
    void Run();

    void InsertKeyFrame(KeyFrame *pKF);

    void RequestReset();

    // This function will run in a separate thread
    void RunGlobalBundleAdjustment(unsigned long nLoopKF);

    bool isRunningGBA(){
   
   
        unique_lock<std::mutex> lock(mMutexGBA);
        return mbRunningGBA;
    }
    bool isFinishedGBA(){
   
   
        unique_lock<std::mutex> lock(mMutexGBA);
        return mbFinishedGBA;
    }   

    void RequestFinish();

    bool isFinished();

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

protected:

    //判断mlpLoopKeyFrameQueue是否非空
    bool CheckNewKeyFrames();

    //获取候选闭环关键帧放入mvpEnoughConsistentCandidates
    bool DetectLoop();

    /**
    *
    * 1. 候选帧和当前关键帧通过Bow加速描述子的匹配，剔除特征点匹配数少的闭环候选帧
    * 2. 利用RANSAC粗略地计算出当前帧与闭环帧的Sim3，选出较好的那个sim3，确定闭环帧
    * 2. 根据确定的闭环帧和对应的Sim3，对3D点进行投影找到更多匹配，通过优化的方法计算更精确的Sim3。
    * 3. 将闭环帧以及闭环帧相连的关键帧的MapPoints与当前帧的点进行匹配（当前帧---闭环帧+相连关键帧）
    */
    bool ComputeSim3();
    /**针对CorrectedPosesMap里的关键帧，mvpLoopMapPoints投影到这个关键帧上与其特征点并进行匹配。
     * 如果匹配成功的特征点本身就有mappoint，就用mvpLoopMapPoints里匹配的点替换，替换下来的mappoint则销毁
     * @param CorrectedPosesMap 表示和当前帧在covisibility相连接的keyframe及其修正的位姿
     */
    void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap);

    /**
    * @brief 闭环
    *
    * 1. 通过求解的Sim3以及相对姿态关系，调整与当前帧相连的关键帧位姿以及这些关键帧观测到的MapPoints的位置（相连关键帧---当前帧）
    * 2. 将闭环帧以及闭环帧相连的关键帧的MapPoints和与当前帧相连的关键帧的点进行匹配（相连关键帧+当前帧---闭环帧+相连关键帧）
    * 3. 通过MapPoints的匹配关系更新这些帧之间的连接关系，即更新covisibility graph
    * 4. 对Essential Graph（Pose Graph）进行优化，MapPoints的位置则根据优化后的位姿做相对应的调整
    * 5. 创建线程进行全局Bundle Adjustment
    */
    void CorrectLoop();

    void ResetIfRequested();
    bool mbResetRequested;
    std::mutex mMutexReset;

    bool CheckFinish();
    void SetFinish();
    bool mbFinishRequested;
    bool mbFinished;
    std::mutex mMutexFinish;

    Map* mpMap;
    Tracking* mpTracker;

    KeyFrameDatabase* mpKeyFrameDB;
    ORBVocabulary* mpORBVocabulary;

    LocalMapping *mpLocalMapper;

    std::list<KeyFrame*> mlpLoopKeyFrameQueue;

    std::mutex mMutexLoopQueue;

    // Loop detector parameters
    float mnCovisibilityConsistencyTh;

    // Loop detector variables
    KeyFrame* mpCurrentKF;
    //找到的和mpCurrentKF形成闭环检测的关键帧
    KeyFrame* mpMatchedKF;
    std::vector<ConsistentGroup> mvConsistentGroups;
    //由DetectLoop()得到的候选关键帧
    std::vector<KeyFrame*> mvpEnoughConsistentCandidates;
    
    //将mpMatchedKF闭环关键帧相连的关键帧全部取出来放入vpLoopConnectedKFs
    std::vector<KeyFrame*> mvpCurrentConnectedKFs;
    //将mvpLoopMapPoints投影到当前关键帧mpCurrentKF进行投影得到的匹配
    std::vector<MapPoint*> mvpCurrentMatchedPoints;
    // 将vpLoopConnectedKFs的MapPoints取出来放入mvpLoopMapPoints
    std::vector<MapPoint*> mvpLoopMapPoints;
    //表示通过ComputeSim3()算的当前帧mpCurrentKF到世界坐标系的变换
    cv::Mat mScw;
    //表示通过ComputeSim3()算的当前帧mpCurrentKF到世界坐标系的Sim3变换
    g2o::Sim3 mg2oScw;

    long unsigned int mLastLoopKFid;

    // Variables related to Global Bundle Adjustment
    bool mbRunningGBA;
    bool mbFinishedGBA;
    bool mbStopGBA;
    std::mutex mMutexGBA;
    std::thread* mpThreadGBA;

    // Fix scale in the stereo/RGB-D case
    bool mbFixScale;


    bool mnFullBAIdx;
};

3.源文件

3.1. 类函数Run()

3.1.1. 代码

void LoopClosing::Run()
{
   
   
    mbFinished =false;

    while(1)
    {
   
   
        // Check if there are keyframes in the queue
	//如果有新的keyframe插入到闭环检测序列（在localmapping::run()结尾处插入）
        if(CheckNewKeyFrames())
        {
   
   
            // Detect loop candidates and check covisibility consistency
	    //检测是否有闭环候选关键帧
            if(DetectLoop())
            {
   
   
               // Compute similarity transformation [sR|t]
               // In the stereo/RGBD case s=1
		//计算候选关键帧的与当前帧的sim3并且返回是否形成闭环的判断
		//并在候选帧中找出闭环帧
		//并计算出当前帧和闭环帧的sim3
               if(ComputeSim3())
               {
   
   
                   // Perform loop fusion and pose graph optimization
                   CorrectLoop();
               }
            }
        }       

        ResetIfRequested();

        if(CheckFinish())
            break;

        usleep(5000);
    }

    SetFinish();
}

3.2. 类函数DetectLoop()

3.2.1.代码

bool LoopClosing::DetectLoop()
{
   
   
    //先将要处理的闭环检测队列的关键帧弹出来一个
    {
   
   
        unique_lock<mutex> lock(mMutexLoopQueue);
        mpCurrentKF = mlpLoopKeyFrameQueue.front();
        mlpLoopKeyFrameQueue.pop_front();
        // Avoid that a keyframe can be erased while it is being process by this thread
        mpCurrentKF->SetNotErase();
    }

    //If the map contains less than 10 KF or less than 10 KF have passed from last loop detection
    // 如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
    if(mpCurrentKF->mnId<mLastLoopKFid+10)
    {
   
   
        mpKeyFrameDB->add(mpCurrentKF);
        mpCurrentKF->SetErase();
        return false;
    }

    // Compute reference BoW similarity score
    // This is the lowest score to a connected keyframe in the covisibility graph
    // We will impose loop candidates to have a higher similarity than this
    //遍历所有共视关键帧，计算当前关键帧与每个共视关键的bow相似度得分，计算minScore
    
    //返回Covisibility graph中与此节点连接的节点（即关键帧），总的来说这一步是为了计算阈值 minScore
    const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
    const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
    float minScore = 1;
    for(size_t i=0; i<vpConnectedKeyFrames.size(); i++)
    {
   
   
        KeyFrame* pKF = vpConnectedKeyFrames[i];
        if(pKF->isBad())
            continue;
        const DBoW2::BowVector &BowVec = pKF->mBowVec;

        float score = mpORBVocabulary->score(CurrentBowVec, BowVec);

        if(score<minScore)
            minScore = score;
    }

    // Query the database imposing the minimum score