ORB_SLAM3代码学习（1）

本人是SLAM初学者，这是本人学习ORBSLAM3的笔记，论文与代码理解难免会有错误，欢迎交流指正。

一、系统整体

系统整体方面，直接采用ORB3论文中的系统组成图。

ORB_SLAM3与ORB_SLAM2的整体结构类似，将系统分为三个线程，分别是TRACKING（跟踪）、LOCAL MAPPING（局部建图）、LOOP & MAP MERGING（回环和地图合并）。

在论文中，作者也提到了本次ORB_SLAM3的创新点如下：

1. 单目和双目视觉惯性SLAM系统即使在IMU（惯性测量单元）初始化阶段也完全依赖于Post-Posteriori（MAP）估计。在这里，我们添加了它与ORB-SLAM视觉惯性的集成，对立体惯性SLAM的扩展以及对公共数据集的全面评估。 我们的结果表明，即使在无环的序列中，单眼和立体视觉惯性系统也比其他视觉惯性方法具有更高的鲁棒性和准确性。
2. 高召回地点识别。 许多最近的视觉SLAM和VO系统使用DBoW2词库解决了位置识别问题。  DBoW2需要时间一致性，在检查几何一致性之前将三个连续的关键帧匹配到同一区域，以牺牲召回为代价提高精度。 结果，系统在关闭循环和重用以前的映射时速度太慢。 我们提出了一种新颖的位置识别算法，其中首先检查候选关键帧的几何一致性，然后再检查三个常见的关键帧的局部一致性，这在大多数情况下已在地图中。 这种策略增加了查全率，并且使数据关联更紧密，从而提高了地图的准确性，但代价是计算成本略高。
3. ORB-SLAM Atlas，第一个完整的多地图SLAM系统，能够处理单目和立体配置的视觉和视觉惯性系统。 该地图集可以表示一组未连接的地图，并将所有映射操作顺利应用到它们：位置识别，摄像机重新定位，环路闭合和准确的无缝地图合并。 这允许自动使用和组合在不同时间构建的映射，从而执行增量多会话SLAM。 介绍了用于视觉传感器的ORB SLAM Atlas的初步版本。在这里，添加了新的位置识别系统，视觉惯性多地图系统及其对公共数据集的评估。
4. 抽象的相机表示形式，使SLAM代码与所使用的相机模型无关，并且可以通过提供其投影，非投影和Jacobian函数来添加新模型。 提供了针孔模型和鱼眼模型的实现。

二、代码梳理

代码首先从main函数入手，以ros_rgbd.cc为例。

1.主函数main

（1）首先是ros系统的初始化，以及启动相关线程

ros::init(argc,argv,"RGBD")
ros::start();

（2）创建SLAM系统，system会初始化所有的系统进程，并且准备好生成帧，此处会调用system的构造函数System::System()，具体见System.cc

ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::RGBD,true);

（3）准备捕获图像，并用SLAM类进行初始化

ImageGrabber igb(&SLAM);

//ImageGrabber类如下
class ImageGrabber
{
public:
    ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM){}//类的初始化

    void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);//捕获RGBD图像，并进行跟踪
     
    ORB_SLAM3::System* mpSLAM;
};

（4）订阅话题，获取彩色图像和深度图像，并进行时间戳对齐，最后sync.registerCallback()是同步注册回调函数，当接收到图像后便会运行此函数，调用ImageGrabber中的GrabRGBD函数。

如果运行程序时出现没有画面的情形，大概率是因为话题名称不对应，先使用rostopic list或者rviz查看发布的话题，然后更改下面代码中的话题名称。

    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 100);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth_registered/image_raw", 100);
    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);
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));

（5）在回调函数中的GrabRGBD函数作用是，将订阅获取的ros image message转换为矩阵类型，并将rgbd和depth图像以及时间戳参数传递给System中的TrackRGBD函数，进行跟踪。
 

void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{
    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());
    //此处便开始Track线程
}

（6）最后停止所有线程，保存关键帧位姿，结束程序。

    SLAM.shutdown();
    
    SLAM.SaveKeyFramesTrajectoryTUM("KeyFrameTrajectory.txt");

    ros::shutdown();

 

2.System类

（1）首先根据主函数中ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::RGBD,true);第三个参数判断传感器类型，然后加载相机参数。

    cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        cerr << "Failed to open settings file at: " << strSettingsFile << endl;
        exit(-1);
    }

（2）加载ORB词袋向量，并初始化各类系统进程

 

（3）主函数中调用TrackRGBD函数（部分）

cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp, string filename)
{
    //Check reset
    {
        unique_lock<mutex> lock(mMutexReset);
        if(mbReset)
        {
            mpTracker->Reset();
            mbReset = false;
            mbResetActiveMap = false;
        }
        else if(mbResetActiveMap)
        {
            mpTracker->ResetActiveMap();
            mbResetActiveMap = false;
        }
    }

    cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp,filename);


    return Tcw;
}

在检查跟踪器是否重置时，使用到了unique_lock，该对象是对线程进行加锁，在加锁时新建一个对象lock，在这个对象生命周期结束之后自动解锁。如果这个线程在运行时，其他线程这时进来了，但此时发现并没有解锁，只能等待，等解锁后才能执行。

随后，每一帧的旋转矩阵Tcw由Tracking.cc中的GrabImageRGBD函数得到。

 

（4）System类后面便是保存关键帧轨迹函数，以SaveKeyFrameTrajectoryTUM为例

void System::SaveKeyFrameTrajectoryTUM(const string &filename)
{
    cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl;

    vector<KeyFrame*> vpKFs = mpAtlas->GetAllKeyFrames();
    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);

    ofstream f;
    f.open(filename.c_str());
    f << fixed;

    for(size_t i=0; i<vpKFs.size(); i++)
    {
        KeyFrame* pKF = vpKFs[i];

        if(pKF->isBad())
            continue;

        cv::Mat R = pKF->GetRotation().t();
        vector<float> q = Converter::toQuaternion(R);
        cv::Mat t = pKF->GetCameraCenter();
        f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t.at<float>(0) << " " << t.at<float>(1) << " " << t.at<float>(2)
          << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;

    }

    f.close();
}

此处使用了Atlas地图集中的GeyAllKeyFrame函数得到所有的关键帧，将之前计算出的每个关键帧的旋转矩阵R和平移t以及时间戳按顺序保存下来。

Atlas地图集是由一系列不连续的小地图构成，能够无缝连接，实现重定位、回环检测、地点识别等功能。地图集本身也随着时间逐步优化且将新的视觉帧经过挑选作为关键帧。