zoukankan      html  css  js  c++  java
  • 一起学ORBSLAM2(3)system框架搭建

    转载请注明原创地址:https://blog.csdn.net/qq_30356613/article/category/6897125

    接着上一期的一起学ORBSLAM2继续,只不过上一期已经是几个月前了,由于这一段时间忙或者毕业答辩的事情,所以没有时间更新博客,望各位见谅,现在到暑假了,没什么事情,接着之前的工作继续,坚持过好每一天,共勉! 

    ORB-SLAM2是萨拉格萨大学做到一个开源SLAM方案,源代码上传到github上边:https://github.com/raulmur/ORB_SLAM2
    相关效果视频网站为:http://webdiis.unizar.es/~raulmur/orbslam/
    ORB-SLAM2包含了用三种方案进行同步追踪与地图构建,分别为以kinect为视觉传感器的RGBD-SLAM方案、以单目摄像机为视觉传感器的mono-slam方案、以双目摄像机为视觉传感器的stereo-slam方案。三种方案有均已ORB特征点作为图像构建SLAM的基础,因此此工程名ORB-SLAM。
    接下来我们将以RGB-D SLAM作为例子来介绍ORB-SLAM开源方案中所用到的方法
    RGB-D SLAM分为以下三个线程,第一个是图像追踪线程(主线程)作为视觉里程计的Tracking线程,第二个为临时地图构建LocalMapping线程,第三个为回环检测LoopClosing线程,第四个为观测器线程Viewer(根据参数而定,可有可无)在接下来的几个章节中,我们将分别就这些分线程进行介绍ORB-SLAM。
    此讲中我们主要分析ORB-SLAM2如何与Kinect进行数据传输和系统线程构建的方法及其代码分析。
    ORB-SLAM2与Kinect的数据传输有许多中,我们在这里讲述其在ROS操作系统上进行数据传输的实例。该工程中开发者在example中ROS文件夹下举例说明了如何将该工程运行在机器人操作系统中(ROS),我们将从这里的main开始讲起。即ros_rgbd.cc文件
    代码如下:

    int main(int argc, char **argv)
    {
    //初始化ROS系统 
    ros::init(argc, argv, "RGBD");
    //ROS启动函数
    ros::start();
    //检测给定参数数量是否完整
    if(argc != 3)
    {
    cerr << endl << "Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings" << endl; 
    ros::shutdown();
    return 1;
    } 
    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    // 创建SLAM系统,初始化所有系统线程并准备进程帧
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);
    //定义捕获图像的对象
    ImageGrabber igb(&SLAM);
    //建立ROS节点
    ros::NodeHandle nh;
    //Subscriber滤波器作为ROS消息的最顶层,不能够将其他滤波器的输出作为其输入
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/kinect2/hd/image_color_rect", 1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/kinect2/hd/image_depth_rect", 1);
    //The message_filters::sync_policies::ApproximateTime policy uses an adaptive algorithm to match messages based on their timestamp.
    //下边两条指令是为了将rgb_sub和depth_sub进行数据对齐,滤掉不对齐的帧数
    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);
    //采用registerCallback()方法,注册回调函数ImageGrabber::GrabRGBD。
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));
    //此函数用于ROS不断回调节点mageGrabber::GrabRGBD函数
    ros::spin();
    // Stop all threads 停止所有线程
    SLAM.Shutdown();
    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
    ros::shutdown();
    return 0;
    }

    主函数中使用ROS编程的风格将该SLAM系统作为ROS上的一个节点来运行在ROS上面,通过订阅Kinect发布的话题,来获取Kinect上采集的图像信息。
    具体代码含义已经在代码中给出,后面通过不断的回调ImageGrabber类中的成员函数GrabRGBD()来读取Kinect数据,从而完成整个系统的运行。

    class ImageGrabber
    {
    public:
    ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}
    void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);
    ORB_SLAM2::System* mpSLAM;
    };
    void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
    {
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptrRGB;
    try
    {
    cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    }
    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()); //该成员函数的核心代码
    }

    以上类和代码主要就是将采集到的数据检测是否有异常,然后将采集到的数据给system,供整个系统使用。
    system构建及使用在system.cc中实现,如下:

    //strVocFile指向字典的路径,strSettingsFile指向参数配置文件的路径,sensor所使用传感器的种类,bUseViewer是否打开实时观测
    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)
    {
    // Output welcome message
    cout << endl <<
    "ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl <<
    "This program comes with ABSOLUTELY NO WARRANTY;" << endl <<
    "This is free software, and you are welcome to redistribute it" << endl <<
    "under certain conditions. See LICENSE.txt." << endl << endl;
    cout << "Input sensor was set to: ";
    if(mSensor==MONOCULAR)
    cout << "Monocular" << endl;
    else if(mSensor==STEREO)
    cout << "Stereo" << endl;
    else if(mSensor==RGBD)
    cout << "RGB-D" << endl;
    //Check settings file 将配置文件读入到内存中,存储到fsSettings变量中
    cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
    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); //将字典加载到mpVocabulary中
    if(!bVocLoad)
    {
    cerr << "Wrong path to vocabulary. " << endl;
    cerr << "Falied to open at: " << strVocFile << endl;
    exit(-1);
    }
    cout << "Vocabulary loaded!" << endl << endl;
    //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);
    //Initialize the Tracking thread 初始化跟踪线程器
    //(it will live in the main thread of execution, the one that called this constructor) 它运行在主线程中,这个线程被称作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);
    }
    //Set pointers between threads 各个线程之间建立联系
    mpTracker->SetLocalMapper(mpLocalMapper);
    mpTracker->SetLoopClosing(mpLoopCloser);
    mpLocalMapper->SetTracker(mpTracker);
    mpLocalMapper->SetLoopCloser(mpLoopCloser);
    mpLoopCloser->SetTracker(mpTracker);
    mpLoopCloser->SetLocalMapper(mpLocalMapper);
    }

    此函数为system的构造函数,构造函数中主要做了以一下工作
    (1)检测传感器的种类,
    (2)将配置文件和词典读入内存中,
    (3)建立线程,分为四个,一个是主线程(tracking线程),第二个为局部建图线程(mptLocalMapping线程),第三个为回环检测线程(LoopClosing线程),第四个为实时显示线程(根据bUseViewer而定)
    (4)各线程之间建立联系及通信
    在main函数中节点的主要工作是不断回调成员函数ImageGrabber::GrabRGBD,而在该成员函数中除了验证一下传输数据是否正常并将ROS图片转变为cv::Mat格式外主要的工作量在不断调用SLAM->TrackRGBD(parameter)函数,那么该函数的具体工作在什么呢?如下所示函数:

    cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp)
    {
    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); //使用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); //使用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)检测输入传感器
    (2)核查运行模式是否改变(主要就是核查mbActivateLocalizationMode和mbDeactivateLocalizationMode)
    (3)检查是否需要复位
    (4)计算变换矩阵T(GrabImageRGBD() 函数来计算)这一步将是主要的工作量所在
    至于GrabImageRGBD()函数的具体实现就是线程Tracking中的内容了,我们将在下一节中详细介绍线程Tracking,希望大家多多关注。

  • 相关阅读:
    CodeForces 288A Polo the Penguin and Strings (水题)
    CodeForces 289B Polo the Penguin and Matrix (数学,中位数)
    CodeForces 289A Polo the Penguin and Segments (水题)
    CodeForces 540C Ice Cave (BFS)
    网站后台模板
    雅图CAD
    mbps
    WCF学习-协议绑定
    数据库建表经验总结
    资源位置
  • 原文地址:https://www.cnblogs.com/liumantang/p/11830389.html
Copyright © 2011-2022 走看看