DynaSLAM-8 DynaSLAM中双目运行流程（Ⅱ）：初始化SLAM系统部分System.cc

目录

1.回忆

2.System::System 

---

1.回忆

        上篇博客中我们讲述了DynaSLAM中初始化Mask R-CNN网络部分的代码。

        这篇博客我们讲述初始化DynaSLAM除Mask R-CNN网络部分以外的代码。

2.System::System 

        初始化Mask R-CNN网络后，程序执行：

// Create SLAM system. It initializes all system threads and gets ready to process frames.
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true);

        其中参数1为path_to_vocabulary，即ORB字典所在文件。

        参数2为path_to_settings，即相机配置参数所在文件。

System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
               const bool bUseViewer):mSensor(sensor), mpViewer(static_cast(NULL)), mbReset(false),mbActivateLocalizationMode(false),
        mbDeactivateLocalizationMode(false)

        在其构造参数我们可以看出，其初始化了变量：

        @mSensor：设置为stereo，即双目传感器模式。

        @mpViewer：绘图句柄，设置为空。

        @mbReset：复位标志，设置为false

        @mbActivateLocalizationMode：是否仅定位不建图模式，设置为false。

        @bUseViewer：是否使用viewer线程即显示建图，默认设置为true。

        @mbDeactivateLocalizationMode:关闭定位模式的标志，默认设置为false。

        本函数内我们要初始化一系列的东西：

        ①mpVocabulary：ORB字典的句柄

        ②mpKeyFrameDatabase：关键帧数据库的句柄，在这里初始化了倒排索引mvInvertedFile的大小为ORB字典的大小。

        ③mpMap：地图的句柄，在这里初始化了地图点中最大关键帧id mnMaxKFid为0，初始化了与地图中的重大变化相关的索引mnBigChangeIdx（循环闭合，全局BA）。

        ④mpFrameDrawer：帧绘制器的句柄，初始化追踪线程的状态mState为SYSTEM_NOT_READY（系统没有准备好），初始化关键帧画布mIm为480 * 640。

        ⑤mpMapDrawer：地图绘制器的句柄，读取yaml文件的内容初始化下列变量：mKeyFrameSize、mKeyFrameLineWidth、mGraphLineWidth、mPointSize、mCameraSize、mCameraLineWidth。

        ⑥mpTracker：追踪线程的句柄，初始化追踪线程并计算图像金字塔的参数：

Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor):
    mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc),
    mpKeyFrameDB(pKFDB), mpInitializer(static_cast(NULL)), mpSystem(pSys), mpViewer(NULL),
    mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0)
{
    // Load camera parameters from settings file

    cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
    float fx = fSettings["Camera.fx"];
    float fy = fSettings["Camera.fy"];
    float cx = fSettings["Camera.cx"];
    float cy = fSettings["Camera.cy"];

    cv::Mat K = cv::Mat::eye(3,3,CV_32F);
    K.at(0,0) = fx;
    K.at(1,1) = fy;
    K.at(0,2) = cx;
    K.at(1,2) = cy;
    K.copyTo(mK);

    cv::Mat DistCoef(4,1,CV_32F);
    DistCoef.at(0) = fSettings["Camera.k1"];
    DistCoef.at(1) = fSettings["Camera.k2"];
    DistCoef.at(2) = fSettings["Camera.p1"];
    DistCoef.at(3) = fSettings["Camera.p2"];
    const float k3 = fSettings["Camera.k3"];
    if(k3!=0)
    {
        DistCoef.resize(5);
        DistCoef.at(4) = k3;
    }
    DistCoef.copyTo(mDistCoef);

    mbf = fSettings["Camera.bf"];

    float fps = fSettings["Camera.fps"];
    if(fps==0)
        fps=30;

    // Max/Min Frames to insert keyframes and to check relocalisation
    mMinFrames = 0;
    mMaxFrames = fps;

    cout << endl << "Camera Parameters: " << endl;
    cout << "- fx: " << fx << endl;
    cout << "- fy: " << fy << endl;
    cout << "- cx: " << cx << endl;
    cout << "- cy: " << cy << endl;
    cout << "- k1: " << DistCoef.at(0) << endl;
    cout << "- k2: " << DistCoef.at(1) << endl;
    if(DistCoef.rows==5)
        cout << "- k3: " << DistCoef.at(4) << endl;
    cout << "- p1: " << DistCoef.at(2) << endl;
    cout << "- p2: " << DistCoef.at(3) << endl;
    cout << "- fps: " << fps << endl;


    int nRGB = fSettings["Camera.RGB"];
    mbRGB = nRGB;

    if(mbRGB)
        cout << "- color order: RGB (ignored if grayscale)" << endl;
    else
        cout << "- color order: BGR (ignored if grayscale)" << endl;

    // Load ORB parameters

    int nFeatures = fSettings["ORBextractor.nFeatures"];
    float fScaleFactor = fSettings["ORBextractor.scaleFactor"];
    int nLevels = fSettings["ORBextractor.nLevels"];
    int fIniThFAST = fSettings["ORBextractor.iniThFAST"];
    int fMinThFAST = fSettings["ORBextractor.minThFAST"];

    mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

    if(sensor==System::STEREO)
        mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

    if(sensor==System::MONOCULAR)
        mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);

    cout << endl  << "ORB Extractor Parameters: " << endl;
    cout << "- Number of Features: " << nFeatures << endl;
    cout << "- Scale Levels: " << nLevels << endl;
    cout << "- Scale Factor: " << fScaleFactor << endl;
    cout << "- Initial Fast Threshold: " << fIniThFAST << endl;
    cout << "- Minimum Fast Threshold: " << fMinThFAST << endl;

    if(sensor==System::STEREO || sensor==System::RGBD)
    {
        mThDepth = mbf*(float)fSettings["ThDepth"]/fx;
        cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl;
    }

    if(sensor==System::RGBD)
    {
        mDepthMapFactor = fSettings["DepthMapFactor"];
        if(fabs(mDepthMapFactor)<1e-5)
            mDepthMapFactor=1;
        else
            mDepthMapFactor = 1.0f/mDepthMapFactor;
    }

}

        读取了相机的参数fx、fy、cx、cy组成相机内参矩阵K、相机畸变参数、mMinFrames、mMaxFrames。

        初始化ORB提取器的句柄mpORBextractorLeft和mpORBextractorRight：

ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,
         int _iniThFAST, int _minThFAST):
    nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
    iniThFAST(_iniThFAST), minThFAST(_minThFAST)
{
    mvScaleFactor.resize(nlevels);
    mvLevelSigma2.resize(nlevels);
    mvScaleFactor[0]=1.0f;
    mvLevelSigma2[0]=1.0f;
    for(int i=1; i= vmin; --v)
    {
        while (umax[v0] == umax[v0 + 1])
            ++v0;
        umax[v] = v0;
        ++v0;
    }
}

        这里和ORB-SLAM2中相同，不再赘述，详细的解释见我的文章：

ORB-SLAM2 ---- ORBextractor::ORBextractor类解析https://blog.csdn.net/qq_41694024/article/details/126288776       

        初始化深度阈值mThDepth，用于判定离群点。

        ⑦初始化局部建图线程句柄：初始了有关于一些局部建图进程的变量。

        ⑧初始化回环检测线程句柄：初始了有关于一些回环检测进程的变量。

        @mpMatchedKF：最终检测出来的，和当前关键帧形成闭环的闭环关键帧。

        @mLastLoopKFid：上一次闭环帧的id

        @mbFixScale：是否固定尺度的标志，单目固定其他不固定。

        ⑨执行线程mptLocalMapping、mptLoopClosing

        ⑩我们默认使用绘图器，因为传入SLAM系统的时候设置bUseViewer = true了

        因为要绘图，因此设置mbFinishRequested、mbFinished、mbStopped、mbStopRequested设置为true。设置fps、图片的宽高...。并设置线程mptViewer执行它，赋予跟踪线程，现在就有画布了。

        ⑪最后设置三大线程间的指针。