ORB SLAM2 代码分析(1)

简介

ORB-SLAM只支持单目相机，ORB-SLAM2在ORB-SLAM的基础上加入了RGBD和双目相机的支持。相对于ORB-SLAM, ORB-SLAM2的主要贡献如下：

• 这是一个同时支持单目，双目，RGBD的开源SLAM系统
• 通过BA对RGBD进行优化，效果优于ICP
• 通过使用近/远距离的双目观测及单目观测，使得双目slam效果优于目前的state-of-the-art
• 支持localization mode, 直接使用已知地图进行快速定位

三大线程

ORB-SLAM2与ORB-SLAM的代码框架几乎一致。三大线程分别是Tracking, Local Mapping, Loop Closing。下面以RGBD为例：

Tracking

• Tracking是在main线程中被调用的:

     // Pass the image to the SLAM system
        SLAM.TrackRGBD(imRGB,imD, ni);

• TrackRGBD()中主要做以下几件事：
  • 检测mode是否改变
  • 检测系统是否收到reset
  • 调用mpTracker->GrabImageRGBD

cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)
{
	...
    // Check mode change
    {
        unique_lock<mutex> lock(mMutexMode);
        if(mbActivateLocalizationMode)
        {
            mpLocalMapper->RequestStop();

            // Wait until Local Mapping has effectively stopped
            while(!mpLocalMapper->isStopped())
            {
                usleep(1000);
            }

            mpTracker->InformOnlyTracking(true);
            mbActivateLocalizationMode = false;
        }
        if(mbDeactivateLocalizationMode)
        {
            mpTracker->InformOnlyTracking(false);
            mpLocalMapper->Release();
            mbDeactivateLocalizationMode = false;
        }
    }

    // Check reset
    {
        unique_lock<mutex> lock(mMutexReset);
        if(mbReset)
        {
            mpTracker->Reset();
            mbReset = false;
        }
    }

    cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);

    unique_lock<mutex> lock2(mMutexState);
    mTrackingState = mpTracker->mState;
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
    return Tcw;
}

• GrabImageRGBD()中主要完成对创建当前的Frame和Track.

    mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
    Track();

Local Mapping

Local Mapping的run()函数位于LocalMapping.cc下面，主要流程如下：

• 检测是否有新的关键帧
• 对关键帧生成词带模型
• 检测最近的mappoint
• 三角化新的mappoint
• 如果暂时没有新的关键帧，搜索附近的关键帧，对重复的关键点进行融合，然后调用Local BA
• 将新的关键帧插入到Loop closing的队列中

        // Tracking will see that Local Mapping is busy
        SetAcceptKeyFrames(false);

        // Check if there are keyframes in the queue
        if(CheckNewKeyFrames())
        {
            // BoW conversion and insertion in Map
            ProcessNewKeyFrame();

            // Check recent MapPoints
            MapPointCulling();

            // Triangulate new MapPoints
            CreateNewMapPoints();

            if(!CheckNewKeyFrames())
            {
                // Find more matches in neighbor keyframes and fuse point duplications
                SearchInNeighbors();
            }

            mbAbortBA = false;

            if(!CheckNewKeyFrames() && !stopRequested())
            {
                // Local BA
                if(mpMap->KeyFramesInMap()>2)
                    Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);

                // Check redundant local Keyframes
                KeyFrameCulling();
            }

            mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
        }
        else if(Stop())
        {
            // Safe area to stop
            while(isStopped() && !CheckFinish())
            {
                usleep(3000);
            }
            if(CheckFinish())
                break;
        }

        ResetIfRequested();

        // Tracking will see that Local Mapping is busy
        SetAcceptKeyFrames(true);

        if(CheckFinish())
            break;

        usleep(3000);

Loop Closing

Loop Closing的主流程位于LoopClosing.cc中：

• 检测是否有新的关键帧插入
• 检测是否存在Loop
  • 计算Sim3相似变换
  • 将loop约束加入到位姿图进行优化

        // Check if there are keyframes in the queue
        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);