今天我们来一起读ORB SLAM2的代码，其实前几个月的时候，在博客写过一些自己的理解，但是不是很详细。然后最近再用ORB SLAM2跑一些室内的数据集，所以会更加详细一些。
我们按照github上面的说明搭建好环境，然后下载对应的代码，发现也是一个CMAKE工程，然后我们进去对应的CMakeLists.txt，发现是把对应几个.cc文件编译成可执行文件。分别是：把rgbd_tum.cc编译成rgbd_tum可执行文件；把stereo_kitti.cc编译成stereo_kitti可执行文件；把stereo_euroc.cc编译成stereo_euroc可执行文件；把mono_tum.cc编译成mono_tum可执行文件；把mono_kitti.cc编译成mono_kitti可执行文件；把mono_euroc.cc编译成mono_euroc可执行文件；
-
add_executable(rgbd_tum
-
Examples/
RGB-D/rgbd_tum.cc)
-
-
add_executable(stereo_kitti
-
Examples/Stereo/stereo_kitti.cc)
-
-
add_executable(stereo_euroc
-
Examples/Stereo/stereo_euroc.cc)
-
-
add_executable(mono_tum
-
Examples/Monocular/mono_tum.cc)
-
-
add_executable(mono_kitti
-
Examples/Monocular/mono_kitti.cc)
-
-
add_executable(mono_euroc
-
Examples/Monocular/mono_euroc.cc)
下面我们重点看一下单目的例子，即mono_tum.cc程序。
首先判断输入是否是4个参数，按照正确的输入应该是：./mono_tum path_to_vocabulary path_to_settings path_to_sequence。这里需要注意的是传入的参数分别对应argv[0],argv[1],argv[2],argv[3]。并且argv[0]对应的是程序本身。这里总有一些小伙伴搞不清楚。
-
if(argc !=
4)
-
{
-
cerr <<
endl <<
"Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence" <<
endl;
-
return
1;
-
}
举个例子：
我调用程序的输入为：
./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt /home/w/Documents/shLeft/shleft.yaml /home/w/Documents/shLeft
cd ~/ORB_SLAM2/
./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUM1.yaml Examples/rgbd_dataset_freiburg1_xyz
mono_tum 程序
ORBvoc.txt 词典
TUM1.yaml 内参
rgbd_dataset_freiburg1_xyz 图片序列路径与时间戳
接下来，定义一个类型为vector<string> 的变量vstrImageFilenames和类型为vector<double>的变量vTimestamps，一个字符串类型变量strFile，用来存储/home/w/Documents/shLeft/rgb.txt这个文件。
-
vector<
string> vstrImageFilenames;
-
vector<
double> vTimestamps;
-
string strFile =
string(argv[
3])+
"/rgb.txt";
接下来进入一个子函数：
LoadImages(strFile, vstrImageFilenames, vTimestamps)
进入这个子函数，我们发现这个函数的功能就是将strFile文件中的时间戳信息存到vTimestamps中去，将png图片路径存在vstrImageFilenames中去。
-
void LoadImages(const string &strFile, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
-
{
-
ifstream f;
-
f.open(strFile.c_str());
-
cout <<
"already open the file:" << strFile <<
endl;
-
-
// skip first three lines
-
string s0;
-
getline(f,s0);
-
getline(f,s0);
-
getline(f,s0);
-
-
while(!f.eof())
-
{
-
string s;
-
getline(f,s);
//读取每一行的内容到s里面去
-
if(!s.empty())
-
{
-
stringstream ss;
double t;
string sRGB;
-
-
ss << s;
-
ss >> t;
-
//将strFile文件中的时间戳信息存到vTimestamps中去
-
vTimestamps.push_back(t);
-
ss >> sRGB;
-
//将png图片路径存在vstrImageFilenames中去。
-
vstrImageFilenames.push_back(sRGB);
-
}
-
}
-
}
然后把控制权返回到主函数，继续往下执行。获取图片的个数，这里是在对应的文件中得到的。如果我们需要跑自己采集的数据集，那么就需要在数据集文件中预处理对应的信息。
int nImages = vstrImageFilenames.size();//图片的个数
下面进入函数的重要部分，初始化SLAM系统，(这里是进入了system.cc文件中)这里会初始化三个线程：Tracking,local Mapping和Loop Closing。
-
//
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::MONOCULAR,
true);
以我的输入为例子，这里的参数是：词典;相机参数文件；单目相机；true;
Vocabulary/ORBvoc.txt /home/w/Documents/shLeft/shleft.yaml
我们先暂停进去system.cc里面看看程序是如何进行初始化这三个线程的吧。先看一下传入的参数：
-
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)
首先是词典文件，以我的例子这里是Vocabulary/ORBvoc.txt 第二个参数是相机参数文件，以我的例子是/home/w/Documents/shLeft/shleft.yaml。第三个参数是用ORB_SLAM2::System::MONOCULAR初始化mSensor。其余的参数不用太在意，已经用默认的参数进行初始化了。
输出ORB SLAM2的欢迎信息，然后判断传感器的类型是单目、立体相机或者RGB-D相机。
-
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
cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);//strSettingsFile:相机参数文件
if(!fsSettings.isOpened())
{
cerr << “Failed to open settings file at: ” << strSettingsFile << endl;
exit(-1);
}
然后加载ORB词典文件。
-
//加载 ORB Vocabulary
-
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);
创建地图
mpMap = new Map();
创建两个显示窗口FrameDrawer，MapDrawer
-
//Create Drawers. These are used by the Viewer 创建两个显示窗口FrameDrawer，MapDrawer
-
mpFrameDrawer =
new FrameDrawer(mpMap);
-
mpMapDrawer =
new MapDrawer(mpMap, strSettingsFile);
初始化Tracking线程。
-
//Initialize the Tracking thread 初始化Tracking线程。
-
//(it will live in the main thread of execution, the one that called this constructor)
-
mpTracker =
new Tracking(
this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
-
mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
初始化Local Mapping线程并开启线程运行。
-
//Initialize the Local Mapping thread and launch. 初始化Local Mapping线程并开启线程运行。
-
mpLocalMapper =
new LocalMapping(mpMap, mSensor==MONOCULAR);
-
mptLocalMapping =
new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
初始化loop closing对象，并开启线程运行
-
//Initialize the Loop Closing thread and launch 初始化loop closing对象，并开启线程运行
-
mpLoopCloser =
new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
-
mptLoopClosing =
new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
初始化显示线程Viewer()，开启线程显示图像和地图点
-
//Initialize the Viewer thread and launch. 初始化显示线程Viewer()，开启线程显示图像和地图点
-
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);
下面我们把注意力收回来主函数，接着下面定义一个vector<float>类型的容器vTimesTrack来存放追踪的时间，并把容器的大小设置为图片数量的大小。
-
// Vector for tracking time statistics
-
vector<
float> vTimesTrack;
//记录每张图片的跟踪时间的容器，下面循环中使用。
-
vTimesTrack.resize(nImages);
下面进入主循环。主循环主要完成下面几件事情，循环读取每一张图片，对图片中的特征点进行跟踪，记录跟踪的时间。代码的注释已经写在对应的代码附近啦。
-
// Main loop
-
cv::Mat im;
-
for(
int ni=
0; ni<nImages; ni++)
-
{
-
// Read image from file
-
im = cv::imread(
string(argv[
3])+
"/"+vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
// 路径：/home/w/Documents/shLeft/rgb/1305031102.175304.png
-
double tframe = vTimestamps[ni];
//时间戳信息---->tframe变量中。
-
-
if(im.empty())
-
{
-
cerr <<
endl <<
"Failed to load image at: "
-
<<
string(argv[
3]) <<
"/" << vstrImageFilenames[ni] <<
endl;
-
return
1;
-
}
-
cout<<
" Pass the image to the SLAM system-----ni = "<<ni<<
endl;
-
#ifdef COMPILEDWITHC11
-
std::chrono::steady_clock::time_point t1 =
std::chrono::steady_clock::now();
-
#else
-
std::chrono::monotonic_clock::time_point t1 =
std::chrono::monotonic_clock::now();
-
#endif
-
-
// Pass the image to the SLAM system
-
SLAM.TrackMonocular(im,tframe);
//图片和时间戳
-
-
#ifdef COMPILEDWITHC11
-
std::chrono::steady_clock::time_point t2 =
std::chrono::steady_clock::now();
-
#else
-
std::chrono::monotonic_clock::time_point t2 =
std::chrono::monotonic_clock::now();
-
#endif
-
-
double ttrack=
std::chrono::duration_cast<
std::chrono::duration<
double> >(t2 - t1).count();
-
-
vTimesTrack[ni]=ttrack;
-
-
// Wait to load the next frame
-
double T=
0;
//上一帧、当前帧、下一帧
-
if(ni<nImages
-1)
-
T = vTimestamps[ni+
1]-tframe;
//（下一帧-当前帧）的时间戳的差
-
else
if(ni>
0)
-
T = tframe-vTimestamps[ni
-1];
//（当前帧-上一帧）的时间戳的差
-
-
if(ttrack<T)
// 跟踪的时间 < 帧之间的时间差，则等待。
-
usleep((T-ttrack)*
1e4);
-
}
循环处理完每一张图片后，就需要关闭这个SLAM系统了。关闭的时候会判断每个线程都结束了才会关闭，不然就处于等待状态。
-
//
Stop all threads
-
SLAM.Shutdown();
然后程序在结束后会对跟踪的时间进行排序，然后计算跟踪时间的中位数和跟踪时间的平均值。之后就把程序中的关键帧的信息保存在文件
KeyFrameTrajectory.txt中。保存的格式是：timestamp,tx,ty,tz,qx,q,qz,qw。
-
// Tracking time statistics
-
sort(vTimesTrack.begin(),vTimesTrack.end());
-
float totaltime =
0;
-
for(
int ni=
0; ni<nImages; ni++)
-
{
-
totaltime+=vTimesTrack[ni];
-
}
-
cout <<
"-------" <<
endl <<
endl;
-
cout <<
"median tracking time: " << vTimesTrack[nImages/
2] <<
endl;
-
cout <<
"mean tracking time: " << totaltime/nImages <<
endl;
-
-
// Save camera trajectory
-
SLAM.SaveKeyFrameTrajectoryTUM(
"KeyFrameTrajectory.txt");