ORB-SLAM2 小白学习笔记1
小白初次学习ORB-SLAM2，基于自己对代码理解，写出学习笔记以便加深记忆。对于很多相关知识掌握的不是很好，可能也有些错误，望纠正。
单目主函数 mono_tum.cc
int main(int argc, char **argv)
{
if(argc != 4)
{
cerr << endl << "Usage: ./mono_tum path_to_vocabulary path_to_settings path_to_sequence" << endl;
return 1;
}
// Retrieve paths to images
vector<string> vstrImageFilenames; //储存彩色图片
vector<double> vTimestamps; //记录时间戳
string strFile = string(argv[3])+"/rgb.txt"; //rgb图片文件路径
LoadImages(strFile, vstrImageFilenames, vTimestamps); //导入图片和时间戳
int nImages = vstrImageFilenames.size(); //图片的数量
// 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); //SLAM系统初始化
// Vector for tracking time statistics
vector<float> vTimesTrack;
vTimesTrack.resize(nImages);
cout << endl << "-------" << endl;
cout << "Start processing sequence ..." << endl;
cout << "Images in the sequence: " << nImages << endl << endl;
// 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);//读取图片
double tframe = vTimestamps[ni];//第ni张图片对应的时间
if(im.empty())
{
cerr << endl << "Failed to load image at: "
<< string(argv[3]) << "/" << vstrImageFilenames[ni] << endl;
return 1;
}
#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);//将文件输入到SLAM系统
#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)*1e6);
}
// Stop all threads
SLAM.Shutdown();//关闭SLAM系统
// 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");
return 0;
}
需要传入四个参数：可执行文件(./mono_tum)、字典(path_to_vocabulary)、配置文件(path_to_settings)以及数据集(path_to_sequence)所在文件路径。
main() 转到 void LoadImage()
vector<string> vstrImageFilenames; //储存彩色图片
vector<double> vTimestamps; //记录时间戳
string strFile = string(argv[3])+"/rgb.txt"; //rgb图片文件路径
LoadImages(strFile, vstrImageFilenames, vTimestamps); //导入图片和时间戳
函数LoadImages()： 导入数据集中的图片和时间戳。
void LoadImages(const string &strFile, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
ifstream f; //创建读文件流对象
f.open(strFile.c_str()); //打开文件
// 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;
ss << s;
double t;
string sRGB;
ss >> t; //时间戳
vTimestamps.push_back(t); //记录时间戳
ss >> sRGB;//彩色图片
vstrImageFilenames.push_back(sRGB);//储存彩色图片
}
}
}
首先创建读文件流对象来读取文件，文件前三行是注释直接跳过。如果没有读取到文件的最后(!f.eof())进入循环。while中将图片存在vector容器vstrImageFilenames 中，时间戳储存在vector容器vTimestamps中。
int nImages = vstrImageFilenames.size(); //图片的数量
nImages用来储存读取到图片的数量。
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true); //SLAM系统初始化
创建一个System类 SLAM 用来初始化整个SLAM系统，它传入四个参数，分别是ORB字典文件路径、配置文件路径、传感器类型(单目、双面、RBGD)、是否使用可视化界面。
vector<float> vTimesTrack;
vTimesTrack.resize(nImages);
cout << endl << "-------" << endl;
cout << "Start processing sequence ..." << endl;
cout << "Images in the sequence: " << nImages << endl << endl;
这里创建一个vector容器的 vTimesTrack 用来统计跟踪时间，并重新指定大小为图片大小nImages。
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);//读取图片
double tframe = vTimestamps[ni];//第ni张图片对应的时间
if(im.empty())
{
cerr << endl << "Failed to load image at: "
<< string(argv[3]) << "/" << vstrImageFilenames[ni] << endl;
return 1;
}
}
创建Mat类型的im，用来读取的vstrImageFilenames中存储的图片。double tframe用来储存每张图片对应的时间戳。
#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);//将文件输入到SLAM系统
#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();
#ifdef COMPILEDWITHC11 程序1… #else 程序2…#endif 如果 #ifdef 后面的内容被#define定义，则执行程序1，否则执行程序2。 cv::Mat System::TrackMonocular() 为单目的追踪接口。需要传入读取的图像和对应的时间戳。
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)*1e6);
等待下一帧图像输入，如果一帧图像追踪的时间小于T(两帧图像之间的距离)就休眠。
// Stop all threads
SLAM.Shutdown();//关闭SLAM系统
// 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");
==sort(vTimesTrack.begin(),vTimesTrack.end());==对跟踪时间进行排序，计算它的中位数以及平均跟踪时间。
main() 转到ORB_SLAM2::System类
System类中首先定义了一个枚举类型：
// Input sensor
//这个枚举类型用于 表示本系统所使用的传感器类型
enum eSensor{
MONOCULAR=0,
STEREO=1,
RGBD=2
};
System()构造函数
//系统的构造函数，将会启动其他的线程
System::System(const string &strVocFile, //词典文件路径
const string &strSettingsFile, //配置文件路径
const eSensor sensor, //传感器类型
const bool bUseViewer): //是否使用可视化界面
mSensor(sensor), //初始化传感器类型
mpViewer(static_cast<Viewer*>(NULL)), //空。。。对象指针？ TODO
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
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 加载ORB字典
cout << endl << "Loading ORB Vocabulary. This could take a while..." << endl;
//建立一个新的ORB字典
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
//这里的帧绘制器和地图绘制器将会被可视化的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)
mpTracker = new Tracking(this, //现在还不是很明白为什么这里还需要一个this指针 TODO
mpVocabulary, //字典
mpFrameDrawer, //帧绘制器
mpMapDrawer, //地图绘制器
mpMap, //地图
mpKeyFrameDatabase, //关键帧地图
strSettingsFile, //设置文件路径
mSensor); //传感器类型iomanip
//初始化局部建图线程并运行
//Initialize the Local Mapping thread and launch
mpLocalMapper = new LocalMapping(mpMap, //指定使iomanip
mSensor==MONOCULAR); // TODO 为什么这个要设置成为MONOCULAR？？？
//运行这个局部建图线程
mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, //这个线程会调用的函数
mpLocalMapper); //这个调用函数的参数
//Initialize the Loop Closing thread and launchiomanip
mpLoopCloser = new LoopClosing(mpMap, //地图
mpKeyFrameDatabase, //关键帧数据库
mpVocabulary, //ORB字典
mSensor!=MONOCULAR); //当前的传感器是否是单目
//创建回环检测线程
mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, //线程的主函数
mpLoopCloser); //该函数的参数
//Initialize the Viewer thread and launch
if(bUseViewer)
{
//如果指定了，程序的运行过程中需要运行可视化部分
//新建viewer
mpViewer = new Viewer(this, //又是这个
mpFrameDrawer, //帧绘制器
mpMapDrawer, //地图绘制器
mpTracker, //追踪器
strSettingsFile); //配置文件的访问路径
//新建viewer线程
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);
}
System()构造函数转到Tracking::Tracking()构造函数
Tracking::Tracking(
System *pSys, //系统实例
ORBVocabulary* pVoc, //BOW字典
FrameDrawer *pFrameDrawer, //帧绘制器
MapDrawer *pMapDrawer, //地图点绘制器
Map *pMap, //地图句柄
KeyFrameDatabase* pKFDB, //关键帧产生的词袋数据库
const string &strSettingPath, //配置文件路径
const int sensor): //传感器类型
mState(NO_IMAGES_YET), //当前系统还没有准备好
mSensor(sensor),
mbOnlyTracking(false), //处于SLAM模式
mbVO(false), //当处于纯跟踪模式的时候，这个变量表示了当前跟踪状态的好坏
mpORBVocabulary(pVoc),
mpKeyFrameDB(pKFDB),
mpInitializer(static_cast<Initializer*>(NULL)), //暂时给地图初始化器设置为空指针
mpSystem(pSys),
mpViewer(NULL), //注意可视化的查看器是可选的，因为ORB-SLAM2最后是被编译成为一个库，所以对方人拿过来用的时候也应该有权力说我不要可视化界面（何况可视化界面也要占用不少的CPU资源）
mpFrameDrawer(pFrameDrawer),
mpMapDrawer(pMapDrawer),
mpMap(pMap),
mnLastRelocFrameId(0) //恢复为0,没有进行这个过程的时候的默认值
{
// Load camera parameters from settings file
// Step 1 从配置文件中加载相机参数
cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);//只读
float fx = fSettings["Camera.fx"];
float fy = fSettings["Camera.fy"];
float cx = fSettings["Camera.cx"];
float cy = fSettings["Camera.cy"];
// |fx 0 cx|
// K = |0 fy cy|
// |0 0 1 |
//构造相机内参矩阵
cv::Mat K = cv::Mat::eye(3,3,CV_32F);
K.at<float>(0,0) = fx;
K.at<float>(1,1) = fy;
K.at<float>(0,2) = cx;
K.at<float>(1,2) = cy;
K.copyTo(mK); //将K参数拷贝给成员变量mK
// 图像矫正系数
// [k1 k2 p1 p2 k3]
cv::Mat DistCoef(4,1,CV_32F);
DistCoef.at<float>(0) = fSettings["Camera.k1"];
DistCoef.at<float>(1) = fSettings["Camera.k2"];
DistCoef.at<float>(2) = fSettings["Camera.p1"];
DistCoef.at<float>(3) = fSettings["Camera.p2"];
const float k3 = fSettings["Camera.k3"];
//有些相机的畸变系数中会没有k3项
if(k3!=0)
{
DistCoef.resize(5);
DistCoef.at<float>(4) = k3;
}
DistCoef.copyTo(mDistCoef); //将畸变参数拷贝给成员变量mDistCoef
// 双目摄像头baseline * fx 50
mbf = fSettings["Camera.bf"];
float fps = fSettings["Camera.fps"]; //每秒相机的帧数
if(fps==0)
fps=30;
// Max/Min Frames to insert keyframes and to check relocalisation
mMinFrames = 0;
mMaxFrames = fps;
//输出
cout << endl << "Camera Parameters: " << endl;
cout << "- fx: " << fx << endl;
cout << "- fy: " << fy << endl;
cout << "- cx: " << cx << endl;
cout << "- cy: " << cy << endl;
cout << "- k1: " << DistCoef.at<float>(0) << endl;
cout << "- k2: " << DistCoef.at<float>(1) << endl;
if(DistCoef.rows==5)
cout << "- k3: " << DistCoef.at<float>(4) << endl;
cout << "- p1: " << DistCoef.at<float>(2) << endl;
cout << "- p2: " << DistCoef.at<float>(3) << endl;
cout << "- fps: " << fps << endl;
// 1:RGB 0:BGR
int nRGB = fSettings["Camera.RGB"];
mbRGB = nRGB;
if(mbRGB)
cout << "- color order: RGB (ignored if grayscale)" << endl;
else
cout << "- color order: BGR (ignored if grayscale)" << endl;
// Load ORB parameters
// Step 2 加载ORB特征点有关的参数,并新建特征点提取器
// 每一帧提取的特征点数 1000
int nFeatures = fSettings["ORBextractor.nFeatures"];
// 图像建立金字塔时的变化尺度 1.2
float fScaleFactor = fSettings["ORBextractor.scaleFactor"];
// 尺度金字塔的层数 8
int nLevels = fSettings["ORBextractor.nLevels"];
// 提取fast特征点的默认阈值 20
int fIniThFAST = fSettings["ORBextractor.iniThFAST"];
// 如果默认阈值提取不出足够fast特征点，则使用最小阈值 8
int fMinThFAST = fSettings["ORBextractor.minThFAST"];
// tracking过程都会用到mpORBextractorLeft作为特征点提取器
mpORBextractorLeft = new ORBextractor(
nFeatures, //m每一帧提取的特征点数
fScaleFactor, //图像建立金子塔的变化尺度
nLevels, //尺度金字塔的层数
fIniThFAST, //提取fast特征点的默认阈值
fMinThFAST); //如果默认阈值提取不出足够fast特征点，则使用最小阈值 8
// 如果是双目，tracking过程中还会用用到mpORBextractorRight作为右目特征点提取器
if(sensor==System::STEREO)
mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
// 在单目初始化的时候，会用mpIniORBextractor来作为特征点提取器
if(sensor==System::MONOCULAR)
mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
cout << endl << "ORB Extractor Parameters: " << endl;
cout << "- Number of Features: " << nFeatures << endl;
cout << "- Scale Levels: " << nLevels << endl;
cout << "- Scale Factor: " << fScaleFactor << endl;
cout << "- Initial Fast Threshold: " << fIniThFAST << endl;
cout << "- Minimum Fast Threshold: " << fMinThFAST << endl;
if(sensor==System::STEREO || sensor==System::RGBD)
{
// 判断一个3D点远/近的阈值 mbf * 35 / fx
//ThDepth其实就是表示基线长度的多少倍
mThDepth = mbf*(float)fSettings["ThDepth"]/fx;
cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl;
}
if(sensor==System::RGBD)
{
// 深度相机disparity转化为depth时的因子
mDepthMapFactor = fSettings["DepthMapFactor"];
if(fabs(mDepthMapFactor)<1e-5)
mDepthMapFactor=1;
else
mDepthMapFactor = 1.0f/mDepthMapFactor;
}
}
扫一扫
4248
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
