ORB_SLAM2 定位模式

梦里梦外; 2022-04-22 20:10 257阅读 0赞

# ORB\_SLAM2 定位模式 #

上一篇博客记载了ORB\_SLAM2地图保存与加载的过程，本篇博客接着上一篇博客的内容记录在已拥有地图的情况下，ORB\_SLAM2定位模式的如何运行。

## 前提 ##

为了实现纯定位，需要对上一篇博客的内容进行一点小小的修改。我们需要在Map.h文件中重新定义Load()函数。即：

void Load(const string &filename,SystemSetting* mySystemSetting, KeyFrameDatabase* mpKeyFrameDatabase);

别忘了添加头文件和命名空间中的类声明。

同样，需要在Map.cc文件中修改Load()函数。Laod()函数中只有一处需要修改，将从地图文件中读到的关键帧添加到关键帧数据库中。即：

// Then read KeyFrames one after another, and add them into the map
        vector<KeyFrame*>kf_by_order;
        for( unsigned int i = 0; i < nKeyFrames; i ++ )
        {
            KeyFrame* kf = LoadKeyFrame(f, mySystemSetting);
            AddKeyFrame(kf);
            kf_by_order.push_back(kf);
            //将关键帧添加到关键帧数据库中
            mpKeyFrameDatabase->add(kf);
        }

## 定位模式 ##

为了开启定位模式，需要在system.cc文件中系统构造函数中进行相应设置并导入地图。为了方便起见，我加入了一个判断。代码如下：

//设置定位模式（包括地图加载）
         char IsPureLocalization;
         cout << "是否开启纯定位模式？(y/n)"<<endl;
         cin >> IsPureLocalization;
         if(IsPureLocalization == 'Y' || IsPureLocalization == 'y')
         {
             ActivateLocalizationMode();
             //导入地图
             string strPathMap = "/home/my_workspace/github/ORB_SLAM2/Examples/Stereo/map.bin";
             SystemSetting *mySystemSetting = new SystemSetting(mpVocabulary);
             mySystemSetting->LoadSystemSetting(mySettingFile);
             mpMap->Load(strPathMap,mySystemSetting,mpKeyFrameDatabase);
         }

该部分代码放在构造函数中的创建地图部分之后即可。

代码中的ActivateLocalizationMode()函数用来激活定位模式，从其函数体中可以发现，它将mbActivateLocalizationMode变量设置为了真值。

void System::ActivateLocalizationMode()
    {
        unique_lock<mutex> lock(mMutexMode);
        mbActivateLocalizationMode = true;
    }

接下来将system.cc文件中TrackStereo()函数中的内容进行修改。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);
            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;
    }

可以看到，该函数在判断mbActivateLocalizationMode是否为真之后就将其置为False。为了使读入系统的每一帧图像都进行定位模式，我们将该语句注释掉。即为：

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);
            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;
    }

## 测试 ##

为了测试效果，我将构建地图时的数据进行了抽样（抽样间隔为一张图像），并用抽样后的数据进行了定位实验。下过如下图所示。  
![在这里插入图片描述][watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70]

# 如果错误，欢迎指正，谢谢！ #

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3UwMTQ3MDk3NjA_size_16_color_FFFFFF_t_70]: /images/20220327/76069cb678e9416eb24062b49226530b.png