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 mapvector<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 stoppedwhile(!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 stoppedwhile(!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;
}

測試

為了測試效果，我將構建地圖時的數據進行了抽樣（抽樣間隔為一張圖像），并用抽樣后的數據進行了定位實驗。下過如下圖所示。

如果錯誤，歡迎指正，謝謝！

總結

以上是生活随笔為你收集整理的ORB_SLAM2 定位模式的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。