ORB-SLAM2流程

最新推荐文章于 2024-04-23 17:14:02 发布

原创最新推荐文章于 2024-04-23 17:14:02 发布 · 1.3k 阅读

4 ·

本内容遵循CC 4.0 BY-SA版权协议

收录于

SLAM

本文详细梳理了ORB-SLAM2的流程，包括数据结构、系统入口文件 ROS_mono.cc 中的初始化、3个核心线程（Tracking、LocalMapping、LoopClosing）的功能。Tracking线程主要负责特征点提取、位姿初始化和跟踪，LocalMapping线程处理关键帧插入、MapPoints优化和局部BA，LoopClosing线程执行闭环检测和校正，优化Essential Graph并进行全局BA。

https://blog.csdn.net/u010128736/article/details/53157605

梳理了一下ORB-SLAM2 的流程：

一、数据结构

 enum eSensor{
        MONOCULAR=0,
        STEREO=1,
        RGBD=2
    };

从数据结构看，根据传感器类型不同，ORB-slam2 框架适合走不同的流程。这里以 MONOCULAR = 0 为例。

可以直接下载使用现成的数据集，直接 LoadImages()，离线map的方式，也可以是通过移动相机抓取特征点，并即时map的方式。这里分析后一种方式。

二、入口文件：/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/ros_mono.cc

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

    ImageGrabber igb(&SLAM);

    ros::NodeHandle nodeHandler;
    ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb);

    ros::spin();

1、用System类生成一个对象SLAM，初始化slam对象，设置三个线程：Tracking ，LocalMapping ，LoopClosing, 如下：

//开启三个线程
    //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);

2、订阅 GrabImage() ——> TrackMonocular()——>GrabImageMonocular()——>Track()

三、3个线程作用：

3.1、Tracking：

（1）Extract ORB，提取关键点和描述子

（2）Initialize pose：在Track() 中初始化位姿 pose ，第一帧：mInitialFrame 当前帧：mCurrentFrame ，从这两帧得到（R，t）。并且，这两帧转为KeyFrame，凡是关键帧KeyFrame都要插入地图 mpMap（mpMap->AddMapPoint(pMP);），根据关键帧与当前帧公共3D点的个数更新关键帧间的连接关系。

MapPoint类表示路标点，Map类用Hash表管理路标点和关键帧。

（3）track local map：

A .更新局部地图，包括局部关键帧和关键点
B. 对局部MapPoints进行投影匹配
C. 根据匹配对估计当前帧的姿态
D. 根据姿态剔除误匹配

（4）keyFrame decision

3.2、Local Mapping: LocalMapping线程处理的关键帧都是Tracking线程发过的

（1）插入关键帧，

（2）去除冗余的MapPoints；

（3）Local BA；

（4）去除冗余的关键帧 : 检测并剔除当前帧相邻的关键帧中和关键帧，剔除的标准是：该关键帧的90%的MapPoints可以被其它关键帧观测到。将当前帧加入到闭环检测队列中。

3.3、Loop Closing

（1) 回环检测Loop decision：计算闭环候选帧，在候选帧中检测具有连续性的候选帧；

（2）计算相似变换阵sim3，求 [ sR | t ]；

（2）Loop Correction：

A. 通过求解的Sim3以及相对姿态关系，调整与当前帧相连的关键帧位姿以及这些关键帧观测到的MapPoints的位置；
B. 将闭环帧以及与闭环帧相连的关键帧的MapPoints和与当前帧相连的关键帧的点进行匹配；
C. 通过MapPoints的匹配关系更新这些帧之间的连接关系，即更新covisibility graph
D. 对Essential Graph（Pose Graph）进行优化，MapPoints的位置则根据优化后的位姿做相对应的调整
E. 创建线程进行全局Bundle Adjustment

四、下载数据集： rgbd_dataset_freiburg1_xyz

进入ORB-SLAM2 目录。执行: ./build.sh
生成可执行文件，在/Examples/目录下；

示例演示：./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml /home/leveon/ORB_SLAM2/rgbd_dataset_freiburg1_xyz