ORB-SLAM2的ROS下的RGBD例程&System文件

这是楼主自己的毕设需要看的文件，所以写一下自己的阅读心得，哈哈哈发现有问题提出来大家一起解决
ORB-SLAM2将自己编写的源文件封装成API库文件，使用ORB::SLAM2为命名空间

1. 编译的节点文件ros_rgbd.cc

主要作用：

1. 初始化SLAM系统，声明与定义消息回调函数——回调函数的主要作用：将ROS机制中的消息转换为OPENCV格式下的Mat（ROS::cv_bridge)，在将图片矩阵传递给局部跟踪线程(Track)

// 声明类（图像收集类）
class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}  //构造函数的申明与定义（申明一个指针）——C++函数后面跟：表示赋值

    void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD); //ROS中的消息机制——&设置引用参数，在局部变量的变化会引起实参的变化

    ORB_SLAM2::System* mpSLAM;  //函数成员变量,初始化SLAM系统
};

//消息回调函数
// 定义消息回调函数（输入为两个消息常量）
// 将消息转换为opencv下的矩阵类型
// 传入消息常量
void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD) //引用参数
{
    // Copy the ros image message to cv::Mat.利用CV：bridge连接不同的数据类型
    cv_bridge::CvImageConstPtr cv_ptrRGB;   //将sonsor::message转换为cv::message
    try
    {
        cv_ptrRGB = cv_bridge::toCvShare(msgRGB);   // 函数返回cv状态下的常量值（返回封装后的一个类）
    }
    catch (cv_bridge::Exception& e) //很少以直接变量传送入函数中
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrD;
    try
    {
        cv_ptrD = cv_bridge::toCvShare(msgD);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    
    //开始进入跟踪线程
    mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec()); // 将封装后的图片传递给相应的函数
}

2. 创建ROS节点（subscriber)，订阅由rgbd传感器采集的rgb图像和深度图像信息

    // 接收rgb图像和深度图像的话题
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1);    //模板类（结合拥有相同时间戳的消息）
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth_registered/image_raw", 1);

4. 将收到的图像消息融合（將同一时间来到的rgb图片消息和深度图片消息进行结合），进行统一的消息回调处理

 	// 异源消息融合
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;   //类型定义
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
    // boost是为C++标准库提供拓展的一系列库的统称
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2)); //结合相同时间戳的消息，并执行一个callback（传送callback函数内容，类的实例化对象，_1_2代表传入参数x,y）

2. System.h & System.cc

system类的构造函数中：

1. 初始化系统(确定相机类型、.yaml配置文件路径、ORB词袋文件路径，初始化系统bool型参数）

System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
               const bool bUseViewer):mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false),mbActivateLocalizationMode(false),
        mbDeactivateLocalizationMode(false) //赋值语句后加入程序运行语句；；static_cast完成数据之间的强制类型转换

2. 检验输入的.yaml配置文件&加载词袋文件

    //Check settings
    //FileStorage存储数据和指定标记好的数据（.yaml格式数据）
    cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ); //创建数据读取对象，c_str()返回指向字符串的指针常量
    if(!fsSettings.isOpened())
    {
       cerr << "Failed to open settings file at: " << strSettingsFile << endl;  //标准错误的输出流
       exit(-1);    //程序异常退出
    }


    //Load ORB Vocabulary
    cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;

    mpVocabulary = new ORBVocabulary(); //新建词库
    bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); //加载词库
    if(!bVocLoad)
    {
        cerr << "Wrong path to vocabulary. " << endl;
        cerr << "Falied to open at: " << strVocFile << endl;
        exit(-1);
    }
    cout << "Vocabulary loaded!" << endl << endl;

3. 创建数据库（地图，关键帧）、创建并初始化ORB SLAM2的四大线程——Tracking(主线程）、Local Mapping、Loop Closing、Viewer

//Create KeyFrame Database
    mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

    //Create the Map
    mpMap = new Map();

    //Create Drawers. These are used by the Viewer
    mpFrameDrawer = new FrameDrawer(mpMap);
    mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);//配置文件中含有VIEWER的参数

    //Initialize the Tracking thread
    //(it will live in the main thread of execution, the one that called this constructor)
    mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                             mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);

    //Initialize the Local Mapping thread and launch
    mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper); //创建线程（在类外部调用类内部函数作为线程）

    //Initialize the Loop Closing thread and launch
    mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);

    //Initialize the Viewer thread and launch
    if(bUseViewer)  // 是否可视化画面
    {
        mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
        mptViewer = new thread(&Viewer::Run, mpViewer);
        mpTracker->SetViewer(mpViewer); //mpTacker一直是主线程
    }

由可执行文件ros_rgbd.cc直接调用的跟踪函数：
消息回调函数（注意 互斥锁 操作）

1. 变换模式
2. 重置系统
3. 将图像传入跟踪线程的函数中
4. 更新跟踪状态

// RGB-D相机类型
// main()函数中直接调用的system类中的函数（*消息回调函数*）
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)
        {
            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;
}

其余作用函数：

1. 模式转换函数（对系统中的一些bool型数据进行设置）
   如：

    //三种互斥锁
    // Reset flag 是否重置系统
    std::mutex mMutexReset; //互斥锁。将不同线程对同一资源的操作变成互斥操作
    bool mbReset;

    // Change mode flags 
    std::mutex mMutexMode;
    bool mbActivateLocalizationMode;//定位模式——关闭局部建图
    bool mbDeactivateLocalizationMode;	

2. 以（TUM or KITTI ）的数据集格式保存相机轨迹，关键帧轨迹
3. 从系统中获得一些状态变量

int mTrackingState;
    std::vector<MapPoint*> mTrackedMapPoints;
    std::vector<cv::KeyPoint> mTrackedKeyPointsUn;
    std::mutex mMutexState