ORB-SLAM2 --- LoopClosing::Run 回环检测线程解析

目录

1.函数作用

2.code

3.函数解析 

3.1 查看是否有待处理的回环关键帧LoopClosing::CheckNewKeyFrames

3.2 检测闭环LoopClosing::DetectLoop 

3.3 计算当前关键帧和上一步闭环候选帧的Sim3变换

3.4  闭环矫正

---

1.函数作用

         回环检测，又称闭环检测，主要解决位置估计随时间漂移的问题。怎么解决呢？假设实际情况下相机经过一段时间的运动后回到了原点，但是由于漂移，它的位置估计值却没有回到原点。怎么办呢？如果有某种手段，让相机知道“回到了原点”这件事，或者把“原点”识别出来，我们再把位置估计值“拉”过去，就可以消除漂移了。这就是所谓的回环检测。

2.code

// 回环线程主函数
void LoopClosing::Run()
{
    mbFinished =false;

    // 线程主循环
    while(1)
    {
        // Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的
        // 在LocalMapping中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue
        // Step 1 查看闭环检测队列mlpLoopKeyFrameQueue中有没有关键帧进来
        if(CheckNewKeyFrames())
        {
            // Detect loop candidates and check covisibility consistency
            if(DetectLoop())
            {
               // Compute similarity transformation [sR|t]
               // In the stereo/RGBD case s=1
               if(ComputeSim3())
               {
                   // Perform loop fusion and pose graph optimization
                   CorrectLoop();
               }
            }
        }

        // 查看是否有外部线程请求复位当前线程
        ResetIfRequested();

        // 查看外部线程是否有终止当前线程的请求,如果有的话就跳出这个线程的主函数的主循环
        if(CheckFinish())
            break;

        //usleep(5000);
		std::this_thread::sleep_for(std::chrono::milliseconds(5));

	}

    // 运行到这里说明有外部线程请求终止当前线程,在这个函数中执行终止当前线程的一些操作
    SetFinish();
}

3.函数解析 

3.1 查看是否有待处理的回环关键帧LoopClosing::CheckNewKeyFrames

        回环检测线程要处理的关键帧是局部建图线程经过判断冗余关键帧判定传来的：

mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);

        关键帧存储在mlpLoopKeyFrameQueue这个队列中。

// 将某个关键帧加入到回环检测的过程中,由局部建图线程调用
void LoopClosing::InsertKeyFrame(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexLoopQueue);
    // 注意：这里第0个关键帧不能够参与到回环检测的过程中,因为第0关键帧定义了整个地图的世界坐标系
    if(pKF->mnId!=0)
        mlpLoopKeyFrameQueue.push_back(pKF);
}

        因此我们进入回环检测的第一件事就是检测队列mlpLoopKeyFrameQueue是否为空，若不为空从中抽取一帧一帧进行闭环检测。

        总结一下：Loopclosing线程中的待处理关键帧是LocalMapping线程发送过来的，LocalMapping线程中的待处理关键帧是Tracking线程中发过来的。在LocalMapping线程中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue中。

/*
 * 查看列表中是否有等待被插入的关键帧
 * @return 如果存在，返回true
 */
bool LoopClosing::CheckNewKeyFrames()
{
    unique_lock<mutex> lock(mMutexLoopQueue);
    return(!mlpLoopKeyFrameQueue.empty());
}

3.2 检测闭环LoopClosing::DetectLoop 

        函数的主要作用是经过一系列筛选操作筛选出当前帧mpCurrentKF的闭环候选关键帧，并将闭环候选关键帧存储在容器mvpEnoughConsistentCandidates中供其他函数使用，更新连续组关系mvConsistentGroups。详解请参阅我的博客：

ORB-SLAM2 --- LoopClosing::DetectLoop 函数解析https://blog.csdn.net/qq_41694024/article/details/128572938

3.3 计算当前关键帧和上一步闭环候选帧的Sim3变换

        本函数分两步，先是对4.2节中的所有候选匹配关键帧mvpEnoughConsistentCandidates进行筛选，主要是对每个闭环候选关键帧与当前帧匹配特征点：

        ①进行BoW匹配初筛：

ORB-SLAM2 ---- ORBmatcher::SearchByBoW函数解析https://blog.csdn.net/qq_41694024/article/details/126322962        ②通过sim3变换剔除一部分不满足要求的闭环候选帧，对于剩下的闭环候选关键帧，我们遍历每一个闭环候选帧进行Sim3Solver求解，得到候选帧pKF到当前帧mpCurrentKF的R（R12），t（t12），变换尺度s（s12），再通过SearchBySim3方法查找更多的匹配。

        剩余详解请参阅我的博客：ORB-SLAM2 --- LoopClosing::ComputeSim3 函数解析https://blog.csdn.net/qq_41694024/article/details/128584438        

3.4  闭环矫正

        主要分五步：

        1. 通过求解的Sim3以及相对姿态关系，调整与当前帧相连的关键帧位姿以及这些关键帧观测到的地图点位置（相连关键帧---当前帧） 
        2. 将闭环帧以及闭环帧相连的关键帧的地图点和与当前帧相连的关键帧的点进行匹配（当前帧+相连关键帧---闭环帧+相连关键帧）     
        3. 通过MapPoints的匹配关系更新这些帧之间的连接关系，即更新covisibility graph                 4. 对Essential Graph（Pose Graph）进行优化，MapPoints的位置则根据优化后的位姿做相对应的调整                        
        5. 创建线程进行全局Bundle Adjustment
        具体详解请参阅我的博客：

ORB-SLAM2 --- LoopClosing::CorrectLoop函数解析https://blog.csdn.net/qq_41694024/article/details/128592728