欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

ORB-SLAM2从理论到代码实现(二):System.cc程序分析

程序员文章站 2022-03-13 23:41:40
...

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

二  System.cc

1.代码解读

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

还来上一篇博客中的图。

ORB-SLAM2从理论到代码实现(二):System.cc程序分析

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

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):一种扩展的复数

四元数有三个虚部,可以表达三维空间中的旋转:

ORB-SLAM2从理论到代码实现(二):System.cc程序分析ORB-SLAM2从理论到代码实现(二):System.cc程序分析

设点 ORB-SLAM2从理论到代码实现(二):System.cc程序分析 经过一次以ORB-SLAM2从理论到代码实现(二):System.cc程序分析表示的旋转后,得到了ORB-SLAM2从理论到代码实现(二):System.cc程序分析

旋转之后的关系为:ORB-SLAM2从理论到代码实现(二):System.cc程序分析

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

ORB-SLAM2从理论到代码实现(二):System.cc程序分析

 反之,由旋转矩阵到四元数的转换如下。假设矩阵为ORB-SLAM2从理论到代码实现(二):System.cc程序分析,其对 应的四元数 q 由下式给出:

ORB-SLAM2从理论到代码实现(二):System.cc程序分析

 

相关标签: SLAM ORB-SLAM