ORB-SLAM2源码笔记（1）—

参考博客

ORB-SLAM2代码详解01: ORB-SLAM2代码运行流程_ncepu_Chen的博客-CSDN博客_orbslam2代码详解

ORB-SLAM2详解（二）代码逻辑_sylvester0510的博客-CSDN博客_orbslam2代码详解

ORB-SLAM2整体框架如图所示

算法流程框架

输入。三种模式：单目、双目、RGBD

跟踪。刚初始化没有速度，所以参考关键帧跟踪。有了速度就进行恒速模型跟踪。都跟丢后进行重定位跟踪。

局部建图。输入跟踪线程里的关键帧。局部地图里的关键帧重新进行特征匹配，生成新的地图点。使用局部BA优化位姿和地图点，随后删除冗余的关键帧。

闭环。通过词袋检测是否闭环，计算当前关键帧和闭环候选关键帧的sim3位姿，然后执行闭环融合和本质图优化（帧与帧之间至少有100个共视地图点）。

全局BA。优化所有关键帧和地图点。

位置识别。导入离线训练好的字典，新输入的图像帧在线转换为词袋向量。应用与特征匹配、重定位、闭环。

地图。地图点+关键帧。关键帧根据共视地图点数目组成共视图（15个地图点），根据父子关系组成生成树。

以RGB-D为例，rgbd_tum.cc的源码：

int main(int argc, char **argv)
{if(argc != 5){cerr << endl << "Usage: ./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association" << endl;return 1;}//按顺序存放需要读取的彩色图像、深度图像的路径，以及对应的时间戳的变量vector<string> vstrImageFilenamesRGB;vector<string> vstrImageFilenamesD;vector<double> vTimestamps;//从命令行输入参数得到关联文件的路径string strAssociationFilename = string(argv[4]);//从关联文件中加载信息LoadImages(strAssociationFilename, vstrImageFilenamesRGB, vstrImageFilenamesD, vTimestamps);//彩色图像和深度图像数据的一致性检查int nImages = vstrImageFilenamesRGB.size();if(vstrImageFilenamesRGB.empty()){cerr << endl << "No images found in provided path." << endl;return 1;}else if(vstrImageFilenamesD.size()!=vstrImageFilenamesRGB.size()){cerr << endl << "Different number of images for rgb and depth." << endl;return 1;}//初始化ORB-SLAM2系统ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::RGBD,true);// 创建一个容器保存跟踪时间vector<float> vTimesTrack;vTimesTrack.resize(nImages);cout << endl << "-------" << endl;cout << "Start processing sequence ..." << endl;cout << "Images in the sequence: " << nImages << endl << endl;cv::Mat imRGB, imD;//遍历图像序列中的每张图像for(int ni=0; ni<nImages; ni++){//读取图像imRGB = cv::imread(string(argv[3])+"/"+vstrImageFilenamesRGB[ni],CV_LOAD_IMAGE_UNCHANGED);imD = cv::imread(string(argv[3])+"/"+vstrImageFilenamesD[ni],CV_LOAD_IMAGE_UNCHANGED);double tframe = vTimestamps[ni];// 判断图像是否为空if(imRGB.empty()){cerr << endl << "Failed to load image at: "<< string(argv[3]) << "/" << vstrImageFilenamesRGB[ni] << endl;return 1;}#ifdef COMPILEDWITHC11std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#elsestd::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif// 追踪SLAM.TrackRGBD(imRGB,imD,tframe);#ifdef COMPILEDWITHC11std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#elsestd::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;//根据时间戳,准备加载下一张图片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);}//终止SLAM过程SLAM.Shutdown();//统计分析追踪耗时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;//保存最终的相机轨迹SLAM.SaveTrajectoryTUM("./CameraTrajectory2.txt");SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");   return 0;
}//从关联文件中提取这些需要加载的图像的路径和时间戳
void LoadImages(const string &strAssociationFilename, vector<string> &vstrImageFilenamesRGB,vector<string> &vstrImageFilenamesD, vector<double> &vTimestamps)
{//输入文件流ifstream fAssociation;//打开关联文件fAssociation.open(strAssociationFilename.c_str());//一直读取,直到文件结束while(!fAssociation.eof()){string s;//读取一行的内容到字符串s中getline(fAssociation,s);//如果不是空行开始分解数据if(!s.empty()){//字符串流stringstream ss;ss << s;//字符串格式:时间戳 rgb图像路径 时间戳 深度图像路径double t;string sRGB, sD;ss >> t;vTimestamps.push_back(t);ss >> sRGB;vstrImageFilenamesRGB.push_back(sRGB);ss >> t;ss >> sD;vstrImageFilenamesD.push_back(sD);}}
}

System类

System类是ORB-SLAM2系统的主类，通过调用其他模块实现跟踪、局部建图、闭环等功能。

构造函数

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) {// step1. 初始化各成员变量// step1.1. 读取配置文件信息cv::FileStorage fsSettings(strSettingsFile.c_str(), cv::FileStorage::READ);// step1.2. 创建ORB词袋mpVocabulary = new ORBVocabulary();// step1.3. 创建关键帧数据库,主要保存ORB描述子倒排索引(即根据描述子查找拥有该描述子的关键帧)mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);// step1.4. 创建地图mpMap = new Map();// step2. 创建3大线程: Tracking、LocalMapping和LoopClosing// step2.1. 主线程就是Tracking线程,只需创建Tracking对象即可mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer, mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);// step2.2. 创建LocalMapping线程及mpLocalMappermpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper);// step2.3. 创建LoopClosing线程及mpLoopClosermpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);// step3. 设置线程间通信mpTracker->SetLocalMapper(mpLocalMapper);mpTracker->SetLoopClosing(mpLoopCloser);mpLocalMapper->SetTracker(mpTracker);mpLocalMapper->SetLoopCloser(mpLoopCloser);mpLoopCloser->SetTracker(mpTracker);mpLoopCloser->SetLocalMapper(mpLocalMapper);
}

这里需要注意，在创建Tracking对象时就已经产生了跟踪线程（构造函数内完成），所以Tracking线程也是主线程。主线程通过持有两个子线程的指针(mptLocalMapping和mptLoopClosing)控制子线程。

跟踪函数

针对3种不同的传感器（单目/双目/RGBD），System类中有3种追踪函数，分别为TrackRGBD、TrackStereo、TrackMonocular。它们通过调用本类中成员变量mpTracker下的GrabImageMonocular、GrabImageStereo、GrabImageRGBD计算相机位姿并保存跟踪到的地图点。

cv::Mat System::TrackMonocular(const cv::Mat &im, const double &timestamp) {cv::Mat Tcw = mpTracker->GrabImageMonocular(im, timestamp);unique_lock<mutex> lock(mMutexState);mTrackingState = mpTracker->mState;mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;return Tcw;
}

这里有点绕，梳理一下

mono_tum.cc调用System类里的TrackMonocular方法，TrackMonocular方法的实现用到了GrabImageMonocular方法，GrabImageMonocular方法是mpTracker中的一个成员函数，而mpTracker是在System初始化时就产生的Tracking类的对象。

ORB-SLAM2源码笔记（1）——框架结构相关推荐

ORB SLAM2源码解读(三)：Frame类
文章目录前言构造函数双目相机 RGBD相机单目相机 ExtractORB:提取特征点 ComputeBoW:计算词袋数据 SetPose:设置相机外参 isInFrustum:判断一个MapP ...
angularjs源码笔记(3)--injector
2019独角兽企业重金招聘Python工程师标准>>> 简介 injector是用来做参数自动注入的,例如 function fn ($http, $scope, aService) ...
spring aop原理_Spring知识点总结！已整理成142页离线文档（源码笔记+思维导图）...
写在前面由于Spring家族的东西很多,一次性写完也不太现实.所以这一次先更新Spring[最核心]的知识点:AOP和IOC 无论是入门还是面试,理解AOP和IOC都是非常重要的.在面试的时候,我没 ...
数据结构源码笔记（C语言描述）汇总
数据结构源码笔记(C语言):英文单词按字典序排序的基数排序数据结构源码笔记(C语言):直接插入排序数据结构源码笔记(C语言):直接选择排序数据结构源码笔记(C语言):置换-选择算法数据结构源码 ...
数据结构源码笔记（C语言）：英文单词按字典序排序的基数排序
//实现英文单词按字典序排序的基数排序算法#include<stdio.h> #include<malloc.h> #include<string.h>#defin ...
数据结构源码笔记（C语言）：索引文件建立和查找
//实现索引文件建立和查找算法#include<stdio.h> #include<malloc.h> #include<string.h> #include< ...
数据结构源码笔记（C语言）：快速排序
//实现快速排序算法 #include<stdio.h> #include<malloc.h> #define MAXE 20typedef int KeyType; type ...
数据结构源码笔记（C语言）：冒泡排序
//冒泡排序算法实现 #include<stdio.h> #include<malloc.h> #define MAXE 20typedef int KeyType; type ...
数据结构源码笔记（C语言）：希尔插入排序
//实现希尔插入排序算法 #include<stdio.h> #include<malloc.h> #define MAXE 20typedef int KeyType; ty ...
数据结构源码笔记（C语言）：直接插入排序
//实现直接插入排序算法#include<stdio.h> #include<malloc.h> #define MAXE 20typedef int KeyType; typ ...

ORB-SLAM2源码笔记（1）——框架结构

ORB-SLAM2源码笔记（1）——框架结构相关推荐

最新文章

热门文章