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

àì夳堔傛蜴生んèń 2022-03-16 00:28 176阅读 0赞

### ORB\_SLAM2代码阅读(1）——系统简介 ###

*  1、说明
 *  2、简介
 *  3、stereo\_kitti.cc
 *  4、SLAM系统文件（System.cc）
 *   *   *  4.1 构造函数System()
         *  4.2 TrackStereo()
         *  4.3 SaveTrajectoryKITTI (const string &filename)
 *  5.总结

# 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相机。

其系统结构图如下图所示：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70_pic_center]  
可以看到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类的类图如下图所示：  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70_pic_center 1]

### 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 &timestamp)
    {
        if(mSensor!=STEREO)
        {
            cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << 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);
        if(mbReset)
        {
            mpTracker->Reset();
            mbReset = false;
        }
        }
    
        cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);
    
        unique_lock<mutex> 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<KeyFrame*> 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<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
        list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
        for(list<cv::Mat>::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<float>(0,0) << " " << Rwc.at<float>(0,1)  << " " << Rwc.at<float>(0,2) << " "  << twc.at<float>(0) << " " <<
                 Rwc.at<float>(1,0) << " " << Rwc.at<float>(1,1)  << " " << Rwc.at<float>(1,2) << " "  << twc.at<float>(1) << " " <<
                 Rwc.at<float>(2,0) << " " << Rwc.at<float>(2,1)  << " " << Rwc.at<float>(2,2) << " "  << twc.at<float>(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类并不是很那理解，我对其中的内容解释的并不多，如果逐行阅读代码，实在是太费唇舌。我相信大家都能看明白，所以讲的粗糙一点。下一篇博客一起阅读关于追踪部分的代码，我尽力记录的详细些。如有错误或遗漏，欢迎留言！希望大家支持！

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70_pic_center]: /images/20220316/bd692d75395c4e8993b77a348d1906ae.png
[System.cc]: http://System.cc
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70_pic_center 1]: /images/20220316/d42f33a98edc4faabd37953ca720190c.png