ORB_SLAM2代码阅读(1)——系统入口

1、说明

本文以～/ORB_SLAM2/Examples/Stereo/stereo_kitti.cc 为起点一步步阅读ORB_SLAM2代码。由于本人也是初学，阅读代码过程中难免出现错误，希望各位谅解。

2、简介

ORB-SLAM是由Raul Mur-Artal，J. M. M. Montiel和Juan D. Tardos于2015年发表在IEEE Transactions on Robotics。ORB-SLAM是一个基于特征点的实时单目SLAM系统，在大规模的、小规模的、室内室外的环境都可以运行。该系统对剧烈运动也很鲁棒，支持宽基线的闭环检测和重定位，包括全自动初始化。该系统包含了所有SLAM系统共有的模块：跟踪（Tracking）、建图（Mapping）、重定位（Relocalization）、闭环检测（Loop closing）。由于ORB-SLAM系统是基于特征点的SLAM系统，故其能够实时计算出相机的轨线，并生成场景的稀疏三维重建结果。ORB-SLAM2在ORB-SLAM的基础上，还支持标定后的双目相机和RGB-D相机。

其系统结构图如下图所示：

可以看到ORB-SLAM主要分为三个线程进行，分别是Tracking、LocalMapping和LoopClosing。三个线程分别存放在对应的三个文件中，分别是Tracking.cpp、LocalMapping.cpp和LoopClosing.cpp文件中，很容易找到。

3、stereo_kitti.cc

stereo_kitti.cc 是针对KITTI数据集的双目ORB_SLAM示例。该示例比较简单，主要包括四部分：

1. 载入图像 ：根据命令行输入的参数，载入在相关文件夹下图像和时间戳。LoadImages(string(argv[3]), vstrImageLeft, vstrImageRight, vTimestamps);*注 ：该过程只是将图像的名称保存在相应的vector变量中。
2. 创建SLAM系统：根据命令行输入的参数创建SLAM系统并进行系统初始化。ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true);其中：argv[1]表示词袋文件的路径，argv[2]表示配置文件的路径，ORB_SLAM2::System::STEREO表示创建SLAM系统为双目SLAM系统，true表示进行可视化操作。
3. 传入图像进行追踪 ：通过一个循环过程将图像传入SLAM系统中进行追踪。SLAM.TrackStereo(imLeft,imRight,tframe);
4. 停止SLAM系统并保存轨迹：通过Shutdown()函数停止SLAM系统中的所有线程，利用SaveTrajectoryKITTI()函数来保存轨迹。

该示例主要内容就是这三部分，其他内容主要是相关变量的定义和追踪时间的统计过程。

4、SLAM系统文件（System.cc）

ORB_SLAM2作者将整个系统进行了完整的封装并定义了一个System类作为系统的入口。System类的类图如下图所示：

4.1 构造函数System()

在System类的构造函数中，主要执行以下操作：

1. 判断传感器类型
2. 检验配置文件
3. 载入词袋
4. 实例化相关成员变量：包括局部建图线程、回环检测线程和视图线程的实例化。注:主线程即为追踪线程！
5. 各个线程之间交叉设置指针

4.2 TrackStereo()

System类的构造函数将系统运行的所有条件都已经准备就绪，等待图片序列的传入。在stereo_kitti.cc示例中通过SLAM.TrackStereo(imLeft,imRight,tframe)传入图片序列。因此，我们也选择该函数作为切入点进行阅读（TrackRGBD()、TrackMonocular()与TrackStereo()大同小异，不再赘述）。

cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp)
{
    if(mSensor!=STEREO)
    {
        cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl;
        exit(-1);
    }   

    // Check mode change 
    {
        unique_lock 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 lock(mMutexReset);
    if(mbReset)
    {
        mpTracker->Reset();
        mbReset = false;
    }
    }

    cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);

    unique_lock lock2(mMutexState);
    mTrackingState = mpTracker->mState;//更新追踪状态
    mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;//更新地图点向量
    mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;//更新关键点向量
    return Tcw;//返回相机坐标系到世界坐标系的变换矩阵
}

在TrackStereo()函数中主要执行以下操作：

1. 首先检验传感器类型。若传感器类型错误则直接退出。
2. 检验定位模式的变化。若定位模式被激活，则关闭局部建图功能，系统只进行定位。若定位模式被关闭，则之前建立的局部地图被释放（清除）。
3. 检验系统是否被重启（有UI界面控制），若系统重启，则重启追踪器。
4. 为追踪器输入图像数据，返回世界坐标系到相机坐标系的变换矩阵。
5. 更新变量

4.3 SaveTrajectoryKITTI (const string &filename)

保存轨迹函数值得详细说明一下。

void System::SaveTrajectoryKITTI(const string &filename)
{
    cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
    if(mSensor==MONOCULAR)
    {
        cerr << "ERROR: SaveTrajectoryKITTI cannot be used for monocular." << endl;
        return;
    }

    vector vpKFs = mpMap->GetAllKeyFrames();//获取所有的关键帧
    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);//对关键帧进行排序

    // Transform all keyframes so that the first keyframe is at the origin.
    // After a loop closure the first keyframe might not be at the origin.
    cv::Mat Two = vpKFs[0]->GetPoseInverse();

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

    // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
    // We need to get first the keyframe pose and then concatenate the relative transformation.
    // Frames not localized (tracking failure) are not saved.

    // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
    // which is true when tracking failed (lbL).
    // 链表mlpReferences保存了所有图像帧的参考关键帧
    // 链表mlRelativeFramePoses保存了所有图像帧的相对于参考帧的姿态变换关系
    list::iterator lRit = mpTracker->mlpReferences.begin();
    list::iterator lT = mpTracker->mlFrameTimes.begin();
    for(list::iterator lit=mpTracker->mlRelativeFramePoses.begin(), lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++)
    {
        ORB_SLAM2::KeyFrame* pKF = *lRit;

        cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);

        while(pKF->isBad())
        {
          //  cout << "bad parent" << endl;
            Trw = Trw*pKF->mTcp;
            pKF = pKF->GetParent();
        }

        Trw = Trw*pKF->GetPose()*Two;//计算全局坐标系到参考关键帧坐标系变换关系

        cv::Mat Tcw = (*lit)*Trw;              //计算全局坐标系到相机坐标系的变换关系，注：Tcw是全局坐标系到相机坐标系的变换
        /************************************************************************
         *                       Tcw是全局坐标系到相机坐标系的变换
         *                      Tcw的逆为相机坐标系到全局坐标系的变换
         *                         变换矩阵的逆可由下式计算
         *                          T^-1 =[ R^T    -R^T*t   
         *                                   0^T      1   ]
         *  
         * **********************************************************************/
        cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();//根据Tcw计算相机坐标系到全局坐标系的旋转矩阵
        cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);         //根据Tcw计算相机坐标系到全局坐标系的平移向量
        
        //将旋转矩阵和平移向量写入文件
        f << setprecision(9) << Rwc.at(0,0) << " " << Rwc.at(0,1)  << " " << Rwc.at(0,2) << " "  << twc.at(0) << " " <<
             Rwc.at(1,0) << " " << Rwc.at(1,1)  << " " << Rwc.at(1,2) << " "  << twc.at(1) << " " <<
             Rwc.at(2,0) << " " << Rwc.at(2,1)  << " " << Rwc.at(2,2) << " "  << twc.at(2) << endl;
    }
    f.close();
    cout << endl << "trajectory saved!" << endl;
}

追踪器中维护了两个重要的链表——mlpReferences 和 mlRelativeFramePoses。其中mlpReferences保存了所有图像帧的参考关键帧；mlRelativeFramePoses保存了图像帧的相对于参考帧的姿态变换关系，即图像与参考关键帧之间的变换矩阵。这正是由于这两个链表的存在，我们才能将整个运动轨迹保存下来。

计算每一帧图像相对于原点处的姿态的流程如下：

1. 从 mpMap 变量处获取所有关键帧并对这些关键帧按ID号进行排序。
2. 获取第一个关键帧的位姿信息。
3. 根据链表mlpReferences和mlRelativeFramePoses的数据，以遍历的方式计算每一帧的姿态。在该过程中：
   1）.首先计算全局坐标系与参考关键帧之间的变换关系Trw。
   2）.计算当前帧相对于全局坐标系的变换关系Tcw。注：Tcw是全局坐标系到相机坐标系的变换，而最终要求的结果是相机坐标系到全局坐标系的变换关系。两者互为逆运算。
   3）.根据Tcw计算相机坐标系到全局坐标系的旋转矩阵Rwc和平移向量twc。

5.总结

作为整个ORB_SLAM2系统的入口,System类并不是很那理解，我对其中的内容解释的并不多，如果逐行阅读代码，实在是太费唇舌。我相信大家都能看明白，所以讲的粗糙一点。下一篇博客一起阅读关于追踪部分的代码，我尽力记录的详细些。如有错误或遗漏，欢迎留言！希望大家支持！