ORB-SLAM2源码学习（五）——回环检测LoopClosing.cc

最新推荐文章于 2025-08-05 18:25:55 发布

原创最新推荐文章于 2025-08-05 18:25:55 发布 · 1.3k 阅读

4 ·

本内容遵循CC 4.0 BY-SA版权协议

slam 专栏收录该内容

20 篇文章

订阅专栏

本文深入解析ORB-SLAM2的回环检测模块，包括DetectLoop()中寻找闭环帧的策略，ComputeSim3()中计算相似变换的过程，以及CorrectLoop()中闭环纠正的步骤，涉及Sim3求解、3D点重投影匹配和全局优化。

同局部构图模块一样回环检测模块同样是存在一个线程函数run；

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

    while(1)
    {
        // Check if there are keyframes in the queue
        // Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的
        // 在LocalMapping中通过InsertKeyFrame将关键帧插入闭环检测队列mlpLoopKeyFrameQueue
        // 闭环检测队列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();
}