ORB-SLAM2从理论到代码实现（二）：System.cc程序分析

     上一篇博客给大家了一个很模糊的ORB-SLAM轮廓，从这一篇博客开始，我们开始一起学习具体的理论和代码解读。在我的博客里会参考一些前辈的博客内容。我会把参考的链接放到最后。

二  System.cc

1.代码解读

System.cc是ORB-SLAM2的主程序，也是ORB-SLAM2系统的入口。本篇主要讲System.cc程序，以及其中用到的理论。

还来上一篇博客中的图。

再来一张吴博的图，大家可以作为对照。

我把关键代码提取出来，进行标注。

mpVocabulary = new ORBVocabulary();//读取ORB词袋
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);//创建关键帧数据库
mpMap = new Map();//创建地图对象
mpFrameDrawer = new FrameDrawer(mpMap);//创建显示关键帧的窗口
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);//创建显示地图的窗口
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);//初始化Tracking线程，该线程在主循环中
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);//初始化Local Mapping线程
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);//启动线程
mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);//初始化LoopClosing线程
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);//启动线程
mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);//初始化窗口
mptViewer = new thread(&Viewer::Run, mpViewer);//启动，前面会有一个是否可视化的判断

从上面的代码来看构造函数system()主要对SLAM系统初始化。下面看看System.cc中的函数接口。

cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp)//追踪双目数据，返回mpTracker->GrabImageStereo(imLeft,imRight,timestamp)
cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)//追踪深度相机数据，返回mpTracker->GrabImageRGBD(im,depthmap,timestamp)
cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp)//追踪单目相机数据返回mpTracker->GrabImageMonocular(im,timestamp)
void System::ActivateLocalizationMode()//mbActivateLocalizationMode = true，激活   mbActivateLocalizationMode
void System::DeactivateLocalizationMode()// mbDeactivateLocalizationMode = true，激活  mbDeactivateLocalizationMode
void System::Shutdown()//判断运行是否结束，结束则关闭系统
void System::SaveTrajectoryTUM(const string &filename)//求数据集中每一帧的位姿
void System::SaveKeyFrameTrajectoryTUM(const string &filename)//求数据集中每一关键帧的位姿
void System::SaveTrajectoryKITTI(const string &filename)//求数据集中每一帧的位姿

 

TrackStereo()、TrackRGBD()、TrackMonocular()：

• 判断输入传感器类型是否正确
• 判断模式，如果是mbActivateLocalizationMode则休眠1000ms直到停止局部建图。如果是mbDeactivateLocalizationMode则重新开启局部建图的线程。然后调用GrabImageStereo(imRectLeft,imRectRight)、GrabImageRGBD(im,depthmap)、GrabImageMonocular(im)开启tracking线程

SaveTrajectoryTUM()：

void System::SaveTrajectoryTUM(const string &filename)

{
......//部分代码省略
    vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();//获得所有关键帧vpKFs
    sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);//对关键帧排序，闭环检测后第一关键帧可能就不在起始位置了
    cv::Mat Two = vpKFs[0]->GetPoseInverse();//获得第一帧相对于世界坐标系的位姿

//遍历所有帧
    ofstream f;
    f.open(filename.c_str());
    f << fixed;
       list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();//参考关键帧迭代器
    list<double>::iterator lT = mpTracker->mlFrameTimes.begin();//时间戳迭代器
    list<bool>::iterator lbL = mpTracker->mlbLost.begin();//标志，当追踪失败为true
    for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
        lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)

    {

        if(*lbL)   continue;//如果当前帧追踪失败则丢弃，继续追踪下一帧
        KeyFrame* pKF = *lRit;
        cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
        // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.

        while(pKF->isBad())//追踪成功但是关键帧不好，则获取当前关键帧相对于上一帧的位姿，并将上//一帧设为关键帧，依次不断的判断关键帧的质量，直到选取合适的关键帧
        {
            Trw = Trw*pKF->mTcp;
            pKF = pKF->GetParent();
        }

        Trw = Trw*pKF->GetPose()*Two;//将关键帧的位姿乘第一帧相对于世界坐标的位姿得到关键帧相//对于世界坐标的位姿
        cv::Mat Tcw = (*lit)*Trw;//在将关键帧相对于世界坐标的位姿乘当前帧相对于关键帧的位姿得//到当前帧相对于世界坐标的位姿
        cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();//求旋转矩阵R
        cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);//求t
        vector<float> q = Converter::toQuaternion(Rwc);//求四元数q
        f << setprecision(6) << *lT << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
    }

    f.close();//关闭文件 

}

 

SaveKeyFrameTrajectoryTUM()与SaveTrajectoryTUM()类似。

2. 四元数（参考了高翔SLAM14讲）

四元数（Quaternion）：一种扩展的复数

•四元数有三个虚部，可以表达三维空间中的旋转：

设点  经过一次以表示的旋转后，得到了

旋转之后的关系为：

再看看四元数与旋转矩阵的转化关系。设四元数 q = q0 + q1i + q2j + q3k，对应的旋转矩阵 R 为： 

 反之，由旋转矩阵到四元数的转换如下。假设矩阵为，其对 应的四元数 q 由下式给出：