ORB-SLAM代码详解之SLAM.TrackMonocular

转载请注明出处：http://blog.csdn.net/c602273091/article/details/54955663

总述

在mono_kitti.cc中运行kitti数据集的时候，初始化完了system之后，就出现了并行的多个线程：追踪、局部地图构建、闭环检测、地图显示等等。这个时候我们就需要喂入数据给整个系统，所以就是：

SLAM.TrackMonocular(im,tframe);

把传感器采集的图片以及时间戳传入，我们就可以更新系统的状态，获取新的数据，更新地图。

这个时候TrackMonocular的流程图为：

TrackMonocular

对TrackMonocular进行解释，具体如下：

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)
{
    // 传感器不是单目摄像头、退出
    if(mSensor!=MONOCULAR)
    {
        cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;
        exit(-1);
    }

    // Check mode change
    // 这一部分主要是对局部地图线程进行操作.
    // mbActivateLocalizationMode是是否停止局部地图线程
    // mbDeactivateLocalizationMode是是否清空局部地图.
    {
        // 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode
        // 不会发生混乱，没有死锁或者在临界区
        unique_lock<mutex> lock(mMutexMode);
        // mbActivateLocalizationMode为true会关闭局部地图线程
        if(mbActivateLocalizationMode)
        {
            mpLocalMapper->RequestStop();
            // 设置local map的mbStopRequested，mbAbortBA为true.
            // 当这两个为true的时候，那么进行就会去关闭局部地图的线程

            // Wait until Local Mapping has effectively stopped
            // mbStopped为true，说明局部地图线程已经关闭了
            while(!mpLocalMapper->isStopped())
            {
                usleep(1000);
            }
            // 局部地图关闭以后，只进行追踪的线程
            // 只计算相机的位姿，没有对局部地图进行更新
            // 设置mbOnlyTracking为真
            mpTracker->InformOnlyTracking(true);
            // 执行完当前的部分之和把mbActivateLocalizationMode再置回false.
            // 当然这里设置mbActivateLocalizationMode为true的部分应该是没有新的关键帧和点云的时候
            // 关闭线程可以使得别的线程得到更多的资源
            mbActivateLocalizationMode = false;
        }
        // 如果mbDeactivateLocalizationMode是true
        // 设置mbActivateLocalizationMode为false
        // 局部地图线程就被释放, 关键帧从局部地图中删除.
        // mbStopped和mbStopRequested被置为false.
        if(mbDeactivateLocalizationMode)
        {
            mpTracker->InformOnlyTracking(false);
            mpLocalMapper->Release();
            mbDeactivateLocalizationMode = false;
        }
    }

    // Check reset
    // 检查是否需要进行复位重置.
    {
        // 给mbReset加锁，防止被别的线程修改
        unique_lock<mutex> lock(mMutexReset);
        if(mbReset)
        {
            // mpViwer暂停，视图停止更新
            // 局部地图：mpLocalMapper和闭环检测：mpLoopClosing被停止.
            // Bow:mpKeyFrameDB和mpMap被清空
            // 就是把所有资源释放
            mpTracker->Reset();
            mbReset = false;
        }
    }
    // 可以看出上面这两部分都是对于各个线程状态的反馈.
    // 其实可以看做是对上一个状态的反馈.
    // 接下来的部分才是最重要的部分,获取数据，对各个线程的数据进行更新, 对地图重新进行计算.
    return mpTracker->GrabImageMonocular(im,timestamp);
}

mpTracker->GrabImageMonocular

这部分是上一部分的核心。对GrabImageMonocular的解释如下：

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double &timestamp)
{
    // 把输入的图片设为当前帧.
    mImGray = im;

    // 若图片是三\四通道的，还需要转化为灰度图.
    if(mImGray.channels()==3)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,CV_RGB2GRAY);
        else
            cvtColor(mImGray,mImGray,CV_BGR2GRAY);
    }
    else if(mImGray.channels()==4)
    {
        if(mbRGB)
            cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
        else
            cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
    }

    // 若状态是未初始化或者当前获取的图片是第一帧的时候，那么就需要进行初始化.
    // 传入的参数就是当前帧（灰度图）、时间戳、ORBextractor（ORB特征）、DBoW2的字典、标定矩阵、畸变参数、立体视觉的（baseline）x（fx）、可靠点的距离的阈值
    // mCurrentFrame就是对当前帧进行处理. 获取ORB特征、获取尺度信息.
    // 这里需要进行更加深入的研究.
    if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
        mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
    else
        mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

    // 追踪,初始化以及特征匹配.
    // 这里需要更加深入的研究.
    Track();

    // mTcw是位姿  
    // clone是cv::Mat特有的方法.
    // 实现的就是完全拷贝, 把数据完全拷贝而不共享数据.
    return mCurrentFrame.mTcw.clone();
}

Frame

对于这一部分，需要特别研究一下：

// 若状态是未初始化或者当前获取的图片是第一帧的时候，那么就需要进行初始化.
// 传入的参数就是当前帧（灰度图）、时间戳、ORBextractor（ORB特征）、DBoW2的字典、标定矩阵、畸变参数、立体视觉的（baseline）x（fx）、可靠点的距离的阈值
// mCurrentFrame就是对当前帧进行处理. 获取ORB特征、获取尺度信息.
// 这里需要进行更加深入的研究.
if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
        mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
    else
        mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);

整个Frame的流程图如上所示，比较清晰。

对Frame部分进行注释得到：

Frame::Frame(