ORB_SLAM2 定位模式
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
vectorkf_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)"<> 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 lock(mMutexMode);
mbActivateLocalizationMode = true;
}
接下来将system.cc文件中TrackStereo()函数中的内容进行修改。TrackStereo()函数的源码为：
cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp)
{
if(mSensor!=STEREO)
{
cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl;
exit(-1);
}
// Check mode change
{
unique_lock 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 lock(mMutexReset);
if(mbReset)
{
mpTracker->Reset();
mbReset = false;
}
}
cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);
unique_lock 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 ×tamp)
{
if(mSensor!=STEREO)
{
cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl;
exit(-1);
}
// Check mode change
{
unique_lock 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 lock(mMutexReset);
if(mbReset)
{
mpTracker->Reset();
mbReset = false;
}
}
cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);
unique_lock lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
return Tcw;
}
测试
为了测试效果，我将构建地图时的数据进行了抽样（抽样间隔为一张图像），并用抽样后的数据进行了定位实验。下过如下图所示。
