在mono_kitti.cc中通过SLAM.TrackMonocular(im,tframe),进入SLAM系统也就来到了system.cc,
#include "System.h"
#include "Converter.h"
#include <thread>
#include <pangolin/pangolin.h>
#include <iomanip>
namespace ORB_SLAM2
{
//System的构造函数,对一些参数进行设定
System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
const bool bUseViewer):mSensor(sensor), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false),mbActivateLocalizationMode(false),
mbDeactivateLocalizationMode(false)
{
// Output welcome message
cout << endl <<
"ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl <<
"This program comes with ABSOLUTELY NO WARRANTY;" << endl <<
"This is free software, and you are welcome to redistribute it" << endl <<
"under certain conditions. See LICENSE.txt." << endl << endl;
cout << "Input sensor was set to: ";
if(mSensor==MONOCULAR)
cout << "Monocular" << endl;
else if(mSensor==STEREO)
cout << "Stereo" << endl;
else if(mSensor==RGBD)
cout << "RGB-D" << endl;
//Check settings file
//读取对应的参数设定的文件,如果运行过ORB-SLAM2就会知道对应的就是相机内参、帧率、基线(双目)
//深度阈值,对应ORB Extractor的参数设定,还有Viewer线程的参数设定,XXX.yaml这种文件类型
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);
if(!fsSettings.isOpened())
{
cerr << "Failed to open settings file at: " << strSettingsFile << endl;
exit(-1);
}
//Load ORB Vocabulary
//下载对应的词袋模型,对应的是.txt文件类型
cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
mpVocabulary = new ORBVocabulary();
//载入词袋模型
bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile);
if(!bVocLoad)
{
cerr << "Wrong path to vocabulary. " << endl;
cerr << "Falied to open at: " << strVocFile << endl;
exit(-1);
}
cout << "Vocabulary loaded!" << endl << endl;
//Create KeyFrame Database
//创建关键帧数据库
mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
//Create the Map
//建图
mpMap = new Map();
//Create Drawers. These are used by the Viewer
//创建显示窗口供使用者选择
mpFrameDrawer = new FrameDrawer(mpMap);
mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
//Initialize the Tracking thread
//(it will live in the main thread of execution, the one that called this constructor)
//初始化Tracking线程
mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
//Initialize the Local Mapping thread and launch
//初始化Local Mapping 线程
mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
//Initialize the Loop Closing thread and launch
//初始化闭环检测线程
mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
//Initialize the Viewer thread and launch
//初始化可视化线程
if(bUseViewer)
{
mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
mptViewer = new thread(&Viewer::Run, mpViewer);
mpTracker->SetViewer(mpViewer);
}
//Set pointers between threads
mpTracker->SetLocalMapper(mpLocalMapper);
mpTracker->SetLoopClosing(mpLoopCloser);
mpLocalMapper->SetTracker(mpTracker);
mpLocalMapper->SetLoopCloser(mpLoopCloser);
mpLoopCloser->SetTracker(mpTracker);
mpLoopCloser->SetLocalMapper(mpLocalMapper);
}
//运行的系统设定为双目
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<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;
}
}
//进入tracking.cc
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;
}
//运行系统设定为深度相机
cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp)
{
if(mSensor!=RGBD)
{
cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << 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->GrabImageRGBD(im,depthmap,timestamp);
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
return Tcw;
}
//运行系统设定为单目
cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp)
{
if(mSensor!=MONOCULAR)
{
cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << 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->GrabImageMonocular(im,timestamp);
unique_lock<mutex> lock2(mMutexState);
mTrackingState = mpTracker->mState;
mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
return Tcw;
}
//激活定位模块
void System::ActivateLocalizationMode()
{
unique_lock<mutex> lock(mMutexMode);
mbActivateLocalizationMode = true;
}
//失活定位模块
void System::DeactivateLocalizationMode()
{
unique_lock<mutex> lock(mMutexMode);
mbDeactivateLocalizationMode = true;
}
//地图是否进行修改
bool System::MapChanged()
{
static int n=0;
int curn = mpMap->GetLastBigChangeIdx();
if(n<curn)
{
n=curn;
return true;
}
else
return false;
}
//重置系统
void System::Reset()
{
unique_lock<mutex> lock(mMutexReset);
mbReset = true;
}
//关闭整个系统
void System::Shutdown()
{
mpLocalMapper->RequestFinish();
mpLoopCloser->RequestFinish();
if(mpViewer)
{
mpViewer->RequestFinish();
while(!mpViewer->isFinished())
usleep(5000);
}
// Wait until all thread have effectively stopped
while(!mpLocalMapper->isFinished() || !mpLoopCloser->isFinished() || mpLoopCloser->isRunningGBA())
{
usleep(5000);
}
if(mpViewer)
pangolin::BindToContext("ORB-SLAM2: Map Viewer");
}
//如果运行的是TUM数据集,保存其轨迹
void System::SaveTrajectoryTUM(const string &filename)
{
cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
if(mSensor==MONOCULAR)
{
cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl;
return;
}
vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
// Transform all keyframes so that the first keyframe is at the origin.
// After a loop closure the first keyframe might not be at the origin.
cv::Mat Two = vpKFs[0]->GetPoseInverse();
ofstream f;
f.open(filename.c_str());
f << fixed;
// Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
// We need to get first the keyframe pose and then concatenate the relative transformation.
// Frames not localized (tracking failure) are not saved.
// For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
// which is true when tracking failed (lbL).
list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
list<bool>::iterator lbL = mpTracker->mlbLost.begin();
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();
cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
vector<float> q = Converter::toQuaternion(Rwc);
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();
cout << endl << "trajectory saved!" << endl;
}
//如果运行的是TUM数据集,保存其关键帧轨迹
void System::SaveKeyFrameTrajectoryTUM(const string &filename)
{
cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl;
vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
// Transform all keyframes so that the first keyframe is at the origin.
// After a loop closure the first keyframe might not be at the origin.
//cv::Mat Two = vpKFs[0]->GetPoseInverse();
ofstream f;
f.open(filename.c_str());
f << fixed;
for(size_t i=0; i<vpKFs.size(); i++)
{
KeyFrame* pKF = vpKFs[i];
// pKF->SetPose(pKF->GetPose()*Two);
if(pKF->isBad())
continue;
cv::Mat R = pKF->GetRotation().t();
vector<float> q = Converter::toQuaternion(R);
cv::Mat t = pKF->GetCameraCenter();
f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t.at<float>(0) << " " << t.at<float>(1) << " " << t.at<float>(2)
<< " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
}
f.close();
cout << endl << "trajectory saved!" << endl;
}
//如果运行的是KITTI数据集,保存其轨迹
void System::SaveTrajectoryKITTI(const string &filename)
{
cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
if(mSensor==MONOCULAR)
{
cerr << "ERROR: SaveTrajectoryKITTI cannot be used for monocular." << endl;
return;
}
vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
// Transform all keyframes so that the first keyframe is at the origin.
// After a loop closure the first keyframe might not be at the origin.
cv::Mat Two = vpKFs[0]->GetPoseInverse();
ofstream f;
f.open(filename.c_str());
f << fixed;
// Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
// We need to get first the keyframe pose and then concatenate the relative transformation.
// Frames not localized (tracking failure) are not saved.
// For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
// which is true when tracking failed (lbL).
list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(), lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++)
{
ORB_SLAM2::KeyFrame* pKF = *lRit;
cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
while(pKF->isBad())
{
// cout << "bad parent" << endl;
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();
cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
f << setprecision(9) << Rwc.at<float>(0,0) << " " << Rwc.at<float>(0,1) << " " << Rwc.at<float>(0,2) << " " << twc.at<float>(0) << " " <<
Rwc.at<float>(1,0) << " " << Rwc.at<float>(1,1) << " " << Rwc.at<float>(1,2) << " " << twc.at<float>(1) << " " <<
Rwc.at<float>(2,0) << " " << Rwc.at<float>(2,1) << " " << Rwc.at<float>(2,2) << " " << twc.at<float>(2) << endl;
}
f.close();
cout << endl << "trajectory saved!" << endl;
}
int System::GetTrackingState()
{
unique_lock<mutex> lock(mMutexState);
return mTrackingState;
}
vector<MapPoint*> System::GetTrackedMapPoints()
{
unique_lock<mutex> lock(mMutexState);
return mTrackedMapPoints;
}
vector<cv::KeyPoint> System::GetTrackedKeyPointsUn()
{
unique_lock<mutex> lock(mMutexState);
return mTrackedKeyPointsUn;
}
} //namespace ORB_SLAM
这部分代码主要针对三个不同的模型,提供了不同的接口,并初始化了几个线程:
tracking线程:这部分最复杂,主要从图像中提取特征,并根据上一帧进行姿态估计,如果上一帧丢失则通过全局重定位初始化位姿,然后跟踪已经重建的局部地图,优化位姿,再确定新的关键帧。
local mapping线程,这一部分完成局部地图的构建,包含关键帧的插入,验证最近生成的地图点并进行筛选,然后生成新的地图点,使用(local BA)最后再对插入的关键帧进行筛选,去除多余的关键帧。
Loop closing线程 这一部分主要分为两个过程,分别是闭环探测和闭环校正。闭环检测先使用WOB进行探测,然后通过Sim3算法计算相似变换。闭环校正,主要是闭环融合和Essential Graph的图优化。
Viewer线程 对估计的位姿和特征点进行可视化显示
这个部分没有什么好讲的,接下来的ORB、Frame、Track等才是又难又重要的部分。
附上单目系统的运行流程图:
参考链接: