ORB_SLAM2的SLAM.TrackRGBD(imRGB,imD,tframe)是如何实现的？

源码解读

SLAM.TrackRGBD(imRGB,imD,tframe); // 这是RGDB数据Examples里面声明的SLAM对象的Shutdown()函数;

cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)
{
    // 判断输入数据类型和摄像头类型是否符合
    if(mSensor!=RGBD)
    {
        cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl;
        exit(-1);
    }    

    // Check mode change 设置运行模式 定位同时建图,指定为不建图，载入之前建立好的地图。
    {
        unique_lock<mutex> lock(mMutexMode);
        // 建图和定位模式
        if(mbActivateLocalizationMode)
        {
            /**
             * Local Mapper. It manages the local map and performs local bundle adjustment.
             * 局部建图和局部BA(bundle adjustment)
             * LocalMapping* mpLocalMapper;
             */
            mpLocalMapper->RequestStop();

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

            /**
             * Tracker. It receives a frame and computes the associated camera pose.
             * It also decides when to insert a new keyframe, create some new MapPoints and
             * performs relocalization if tracking fails.
             * 跟踪，除了进行运动跟踪外还要负责创建关键帧、创建新地图点和进行重定位的工作
             * Tracking* mpTracker;
             */
            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 Tracking::GrabImageRGBD(
    * const cv::Mat &imRGB,           //彩色图像
    * const cv::Mat &imD,             //深度图像
    * const double &timestamp)        //时间戳
    * 输入左目RGB或RGBA图像和深度图
    * 将图像转为mImGray和imDepth初始化mCurrentFrame
    * 进行tracking过程
    * 输出世界坐标系到该帧相机坐标系的变换矩阵
    */
    cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);
    
    /**
     * Linux中提供一把互斥锁mutex（也称之为互斥量）
     * 一个线程在对资源操作前都尝试先加锁，成功加锁才能操作，操作结束解锁。
     * 同一时刻，只能有一个线程持有该锁
     */
    unique_lock<mutex> lock2(mMutexState);
    // 跟踪状态 eTrackingState mState;
    mTrackingState = mpTracker->mState;
    /**
     * MapPoints associated to keypoints, NULL pointer if no association.
     * 每个特征点keypoints对应地图点MapPoint.
     * 如果特征点没有对应的地图点,将存储一个空指针
     * std::vector<MapPoint*> mvpMapPoints;
     * Frame mCurrentFrame; 追踪线程的一个Current Frame(当前帧)
     */
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
    /*
    * std::vector<cv::KeyPoint> mvKeysUn; 校正mvKeys后的特征点
    */
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
    return Tcw;
}