ORB-SLAM2源码解析
2021/10/22 22:39:56
本文主要是介绍ORB-SLAM2源码解析,对大家解决编程问题具有一定的参考价值,需要的程序猿们随着小编来一起学习吧!
ORB-SLAM2源码解析
代码文件树 ├── build_ros.sh ├── build.sh ├── CMakeLists.txt ├── cmake_modules │ └── FindEigen3.cmake ├── Dependencies.md ├── #Examples# import!!!! │ ├── Monocular │ │ ├── EuRoC_TimeStamps │ │ │ ├── MH01.txt │ │ │ ├── MH02.txt │ │ │ ├── MH03.txt │ │ │ ├── MH04.txt │ │ │ ├── MH05.txt │ │ │ ├── V101.txt │ │ │ ├── V102.txt │ │ │ ├── V103.txt │ │ │ ├── V201.txt │ │ │ ├── V202.txt │ │ │ └── V203.txt │ │ ├── EuRoC.yaml │ │ ├── KITTI00-02.yaml │ │ ├── KITTI03.yaml │ │ ├── KITTI04-12.yaml │ │ ├── mono_euroc.cc │ │ ├── mono_kitti.cc │ │ ├── mono_tum.cc │ │ ├── TUM1.yaml │ │ ├── TUM2.yaml │ │ └── TUM3.yaml │ ├── RGB-D │ │ ├── associations │ │ │ ├── fr1_desk2.txt │ │ │ ├── fr1_desk.txt │ │ │ ├── fr1_room.txt │ │ │ ├── fr1_xyz.txt │ │ │ ├── fr2_desk.txt │ │ │ ├── fr2_xyz.txt │ │ │ ├── fr3_nstr_tex_near.txt │ │ │ ├── fr3_office.txt │ │ │ ├── fr3_office_val.txt │ │ │ ├── fr3_str_tex_far.txt │ │ │ └── fr3_str_tex_near.txt │ │ ├── rgbd_tum.cc │ │ ├── TUM1.yaml │ │ ├── TUM2.yaml │ │ └── TUM3.yaml │ ├── ROS │ │ └── ORB_SLAM2 │ │ ├── Asus.yaml │ │ ├── CMakeLists.txt │ │ ├── manifest.xml │ │ └── src │ │ ├── AR │ │ │ ├── ros_mono_ar.cc │ │ │ ├── ViewerAR.cc │ │ │ └── ViewerAR.h │ │ ├── ros_mono.cc │ │ ├── ros_rgbd.cc │ │ └── ros_stereo.cc │ └── Stereo │ ├── EuRoC_TimeStamps │ │ ├── MH01.txt │ │ ├── MH02.txt │ │ ├── MH03.txt │ │ ├── MH04.txt │ │ ├── MH05.txt │ │ ├── V101.txt │ │ ├── V102.txt │ │ ├── V103.txt │ │ ├── V201.txt │ │ ├── V202.txt │ │ └── V203.txt │ ├── EuRoC.yaml │ ├── KITTI00-02.yaml │ ├── KITTI03.yaml │ ├── KITTI04-12.yaml │ ├── stereo_euroc.cc │ └── stereo_kitti.cc ├── #include# import!!!! │ ├── Converter.h │ ├── FrameDrawer.h │ ├── Frame.h │ ├── Initializer.h │ ├── KeyFrameDatabase.h │ ├── KeyFrame.h │ ├── LocalMapping.h │ ├── LoopClosing.h │ ├── MapDrawer.h │ ├── Map.h │ ├── MapPoint.h │ ├── Optimizer.h │ ├── ORBextractor.h │ ├── ORBmatcher.h │ ├── ORBVocabulary.h │ ├── PnPsolver.h │ ├── Sim3Solver.h │ ├── System.h │ ├── Tracking.h │ └── Viewer.h ├── License-gpl.txt ├── LICENSE.txt ├── README.md ├── #src# import!!!! │ ├── Converter.cc │ ├── Frame.cc │ ├── FrameDrawer.cc │ ├── Initializer.cc │ ├── KeyFrame.cc │ ├── KeyFrameDatabase.cc │ ├── LocalMapping.cc │ ├── LoopClosing.cc │ ├── Map.cc │ ├── MapDrawer.cc │ ├── MapPoint.cc │ ├── Optimizer.cc │ ├── ORBextractor.cc │ ├── ORBmatcher.cc │ ├── PnPsolver.cc │ ├── Sim3Solver.cc │ ├── System.cc │ ├── Tracking.cc │ └── Viewer.cc ├── Thirdparty │ ├── DBoW2 │ │ ├── CMakeLists.txt │ │ ├── DBoW2 │ │ │ ├── BowVector.cpp │ │ │ ├── BowVector.h │ │ │ ├── FClass.h │ │ │ ├── FeatureVector.cpp │ │ │ ├── FeatureVector.h │ │ │ ├── FORB.cpp │ │ │ ├── FORB.h │ │ │ ├── ScoringObject.cpp │ │ │ ├── ScoringObject.h │ │ │ └── TemplatedVocabulary.h │ │ ├── DUtils │ │ │ ├── Random.cpp │ │ │ ├── Random.h │ │ │ ├── Timestamp.cpp │ │ │ └── Timestamp.h │ │ ├── LICENSE.txt │ │ └── README.txt │ └── g2o │ ├── CMakeLists.txt │ ├── cmake_modules │ │ ├── FindBLAS.cmake │ │ ├── FindEigen3.cmake │ │ └── FindLAPACK.cmake │ ├── config.h.in │ ├── g2o │ │ ├── core │ │ │ ├── base_binary_edge.h │ │ │ ├── base_binary_edge.hpp │ │ │ ├── base_edge.h │ │ │ ├── base_multi_edge.h │ │ │ ├── base_multi_edge.hpp │ │ │ ├── base_unary_edge.h │ │ │ ├── base_unary_edge.hpp │ │ │ ├── base_vertex.h │ │ │ ├── base_vertex.hpp │ │ │ ├── batch_stats.cpp │ │ │ ├── batch_stats.h │ │ │ ├── block_solver.h │ │ │ ├── block_solver.hpp │ │ │ ├── cache.cpp │ │ │ ├── cache.h │ │ │ ├── creators.h │ │ │ ├── eigen_types.h │ │ │ ├── estimate_propagator.cpp │ │ │ ├── estimate_propagator.h │ │ │ ├── factory.cpp │ │ │ ├── factory.h │ │ │ ├── hyper_dijkstra.cpp │ │ │ ├── hyper_dijkstra.h │ │ │ ├── hyper_graph_action.cpp │ │ │ ├── hyper_graph_action.h │ │ │ ├── hyper_graph.cpp │ │ │ ├── hyper_graph.h │ │ │ ├── jacobian_workspace.cpp │ │ │ ├── jacobian_workspace.h │ │ │ ├── linear_solver.h │ │ │ ├── marginal_covariance_cholesky.cpp │ │ │ ├── marginal_covariance_cholesky.h │ │ │ ├── matrix_operations.h │ │ │ ├── matrix_structure.cpp │ │ │ ├── matrix_structure.h │ │ │ ├── openmp_mutex.h │ │ │ ├── optimizable_graph.cpp │ │ │ ├── optimizable_graph.h │ │ │ ├── optimization_algorithm.cpp │ │ │ ├── optimization_algorithm_dogleg.cpp │ │ │ ├── optimization_algorithm_dogleg.h │ │ │ ├── optimization_algorithm_factory.cpp │ │ │ ├── optimization_algorithm_factory.h │ │ │ ├── optimization_algorithm_gauss_newton.cpp │ │ │ ├── optimization_algorithm_gauss_newton.h │ │ │ ├── optimization_algorithm.h │ │ │ ├── optimization_algorithm_levenberg.cpp │ │ │ ├── optimization_algorithm_levenberg.h │ │ │ ├── optimization_algorithm_property.h │ │ │ ├── optimization_algorithm_with_hessian.cpp │ │ │ ├── optimization_algorithm_with_hessian.h │ │ │ ├── parameter_container.cpp │ │ │ ├── parameter_container.h │ │ │ ├── parameter.cpp │ │ │ ├── parameter.h │ │ │ ├── robust_kernel.cpp │ │ │ ├── robust_kernel_factory.cpp │ │ │ ├── robust_kernel_factory.h │ │ │ ├── robust_kernel.h │ │ │ ├── robust_kernel_impl.cpp │ │ │ ├── robust_kernel_impl.h │ │ │ ├── solver.cpp │ │ │ ├── solver.h │ │ │ ├── sparse_block_matrix_ccs.h │ │ │ ├── sparse_block_matrix_diagonal.h │ │ │ ├── sparse_block_matrix.h │ │ │ ├── sparse_block_matrix.hpp │ │ │ ├── sparse_block_matrix_test.cpp │ │ │ ├── sparse_optimizer.cpp │ │ │ └── sparse_optimizer.h │ │ ├── solvers │ │ │ ├── linear_solver_dense.h │ │ │ └── linear_solver_eigen.h │ │ ├── stuff │ │ │ ├── color_macros.h │ │ │ ├── macros.h │ │ │ ├── misc.h │ │ │ ├── os_specific.c │ │ │ ├── os_specific.h │ │ │ ├── property.cpp │ │ │ ├── property.h │ │ │ ├── string_tools.cpp │ │ │ ├── string_tools.h │ │ │ ├── timeutil.cpp │ │ │ └── timeutil.h │ │ └── types │ │ ├── se3_ops.h │ │ ├── se3_ops.hpp │ │ ├── se3quat.h │ │ ├── sim3.h │ │ ├── types_sba.cpp │ │ ├── types_sba.h │ │ ├── types_seven_dof_expmap.cpp │ │ ├── types_seven_dof_expmap.h │ │ ├── types_six_dof_expmap.cpp │ │ └── types_six_dof_expmap.h │ ├── license-bsd.txt │ └── README.txt └── Vocabulary └── ORBvoc.txt.tar.gz
准备活动
解读思路
- 明线——代码里有哪些函数,这些函数做了什么?
- 暗线——这些函数何时被调用,类和类之间如何发生关系?
变量命名规则
ORB-SLAM2中的变量遵循一定的命名规则
- 变量名的第一个字母为
m
表示该变量为某类的成员变量 - 变量名的第一、二个字母表示数据类型
p
表示指针类型n
表示int
类型b
表示bool
类型s
表示std::set
类型v
表示std::vector
类型l
表示std::list
类型KF
表示KeyFrame
类型
这种将变量类型写进变量名的命名方法叫做匈牙利命名法
理解多线程
为什么要使用多线程
-
加快运行速度
bool Initializer::Initialize(const Frame &CurrentFrame) { // ... thread threadH(&Initializer::FindHomography, this, ref(vbMatchesInliersH), ref(SH), ref(H)); thread threadF(&Initializer::FindFundamental, this, ref(vbMatchesInliersF), ref(SF), ref(F)); // ... }
开两个线程同时计算两个矩阵,在多核处理器上会加快运算速度
-
因为系统的随机性,各个步骤的运行顺序是不确定的
Tracking
线程不产生关键帧时,LocalMapping
和LoopClosing
线程基本上处于空转的状态。而
Tracking
线程产生关键帧的频率和时机不是固定的,因此需要3个线程同时运行。LocalMapping
和LoopClosing
线程不断循环查询Tracking
线程是否产生关键帧,产生了的话就处理。// Tracking线程主函数 void Tracking::Track() { // 进行跟踪 // ... // 若跟踪成功,根据条件判定是否产生关键帧 if (NeedNewKeyFrame()) // 产生关键帧并将关键帧传给LocalMapping线程 KeyFrame *pKF = new KeyFrame(mCurrentFrame, mpMap, mpKeyFrameDB); mpLocalMapper->InsertKeyFrame(pKF); } // LocalMapping线程主函数 void LocalMapping::Run() { // 死循环 while (1) { // 判断是否接收到关键帧 if (CheckNewKeyFrames()) { // 处理关键帧 // ... // 将关键帧传给LoopClosing线程 mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame); } // 线程暂停3毫秒,3毫秒结束后再从while(1)循环首部运行 std::this_thread::sleep_for(std::chrono::milliseconds(3)); } } // LoopClosing线程主函数 void LoopClosing::Run() { // 死循环 while (1) { // 判断是否接收到关键帧 if (CheckNewKeyFrames()) { // 处理关键帧 // ... } // 查看是否有外部线程请求复位当前线程 ResetIfRequested(); // 线程暂停5毫秒,5毫秒结束后再从while(1)循环首部运行 std::this_thread::sleep_for(std::chrono::milliseconds(5)); } }
多线程中的锁
为了防止多个线程同时操作同一变量造成混乱,所以引入锁机制
将成员函数本身设置为私有变量(private
orprotected
),并在操作他们的公有函数中设置锁
class KeyFrame { protected: KeyFrame* mpParent; public: void KeyFrame::ChangeParent(KeyFrame *pKF) { unique_lock<mutex> lockCon(mMutexConnections); // 加锁 mpParent = pKF; pKF->AddChild(this); } KeyFrame *KeyFrame::GetParent() { unique_lock<mutex> lockCon(mMutexConnections); // 加锁 return mpParent; } }
一把锁在某个时刻只有一个线程能够拿到,如果程序执行到某个需要锁的位置,但是锁被别的线程拿着不释放的话,当前线程就会暂停下来;直到其它线程释放了这个锁,当前线程才能拿走锁并继续向下执行
什么时候加锁和释放锁?
unique_lock<mutex> lockCon(mMutexConnections);
这句话就是加锁,锁的有效性仅限于大括号{}
之内,也就是说,程序运行出大括号之后就释放锁了.因此可以看到有一些代码中加上了看似莫名其妙的大括号.
void KeyFrame::EraseConnection(KeyFrame *pKF) { // 第一部分加锁 {//here!!! unique_lock<mutex> lock(mMutexConnections); if (mConnectedKeyFrameWeights.count(pKF)) { mConnectedKeyFrameWeights.erase(pKF); bUpdate = true; } }// !!!!!程序运行到这里就释放锁,后面的操作不需要抢到锁就能执行 UpdateBestCovisibles(); }
SLAM主类System
System
类是ORB-SLAM2系统的主类,先分析其主要的成员函数和成员变量:
成员变量/函数 | 访问控制 | 意义 |
---|---|---|
eSensor mSensor |
private |
传感器类型 MONOCULAR , STEREO, RGBD |
ORBVocabulary* mpVocabulary |
private |
ORB字典,保存ORB描述子聚类结果 |
KeyFrameDatabase* mpKeyFrameDatabase |
private |
关键帧数据库,保存ORB描述子倒排索引 |
Map* mpMap |
private |
地图 |
Tracking* mpTracker |
private |
追踪器 |
LocalMapping* mpLocalMapper std::thread* mptLocalMapping |
private private |
局部建图器 局部建图线程 |
LoopClosing* mpLoopCloser std::thread* mptLoopClosing |
private private |
回环检测器 回环检测线程 |
Viewer* mpViewer FrameDrawer* mpFrameDrawer MapDrawer* mpMapDrawer std::thread* mptViewer |
private private private private |
查看器 帧绘制器 地图绘制器 查看器线程 |
System(const string &strVocFile, string &strSettingsFile, const eSensor sensor, const bool bUseViewer=true) |
public |
构造函数 |
cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp) cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp) cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp) int mTrackingState std::mutex mMutexState |
pubilc public public private private |
跟踪双目相机,返回相机位姿 跟踪RGBD相机,返回相机位姿 跟踪单目相机,返回相机位姿 追踪状态 追踪状态锁 |
bool mbActivateLocalizationMode bool mbDeactivateLocalizationMode std::mutex mMutexMode void ActivateLocalizationMode() void DeactivateLocalizationMode() |
private private private pubilc pubilc |
开启/关闭纯定位模式 |
bool mbReset std::mutex mMutexReset void Reset() |
private private public |
系统复位 |
void Shutdown() |
pubilc |
系统关闭 |
void SaveTrajectoryTUM(const string &filename) void SaveKeyFrameTrajectoryTUM(const string &filename) void SaveTrajectoryKITTI(const string &filename) |
public public public |
以TUM/KITTI格式保存相机运动轨迹和关键帧位姿 |
构造函数
System(const string &strVocFile, string &strSettingsFile, const eSensor sensor, const bool bUseViewer=true)
: 构造函数
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线程及mpLocalMapper mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR); mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run, mpLocalMapper); // step2.3. 创建LoopClosing线程及mpLoopCloser mpLoopCloser = 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); }
LocalMapping
和LoopClosing
线程在System
类中有对应的std::thread
线程成员变量,为什么Tracking
线程没有对应的std::thread
成员变量?
因为Tracking
线程就是主线程,而LocalMapping
和LoopClosing
线程是其子线程,主线程通过持有两个子线程的指针(mptLocalMapping
和mptLoopClosing
)控制子线程.
(ps: 虽然在编程实现上三大主要线程构成父子关系,但逻辑上我们认为这三者是并发的,不存在谁控制谁的问题)
跟踪函数
System
对象所在的主线程就是跟踪线程,针对不同的传感器类型有3个用于跟踪的函数,其内部实现就是调用成员变量mpTracker
的GrabImageMonocular
(GrabImageStereo
或GrabImageRGBD
)方法
传感器类型 | 用于跟踪的成员函数 |
---|---|
MONOCULAR |
cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp) |
STEREO |
cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp) |
RGBD |
cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp) |
cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp) { 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; }
概念解释
特征点响应值和描述子的区别
- 响应值描述的是该特征点的区分度大小
- 响应值越大的点越应该被留用做特征点
- 响应值类似于分数,分数越高的学生能力越强,越应该被录取
- 描述子是特征点的一个哈希运算
- 其大小无意义,仅用来在数据库中快速找回某些特征点
- 描述子相当于学生的学号,系统随机运算出的一串数,用于定位学生
特征点和地图点的区别
-
维度
- 特征点是2D的,是相机图像上的点
- 地图点是3D的,根据同一特征点在多个图片中的不同位置三角化得到的
-
对应关系
- 地图点在观测到它的帧上必对应某特征点
- 特征点不一定能够成功三角化出地图点
双目相机和单目相机的数据
- 从直接读取的数据来讲
- 双目相机
- 知道特征点的左目坐标、右目坐标,不知道特征点深度值
- RGBD相机
- 知道特征点的左目数据、特征点深度,不知道右目数据
- 双目相机
回环检测的标准
- 连续3帧场景都在之前出现过
- 根据BOW判断
- 当前3帧场景在之前出现的时机也具有一定连续性
- 根据连续关键帧组判断
这篇关于ORB-SLAM2源码解析的文章就介绍到这儿,希望我们推荐的文章对大家有所帮助,也希望大家多多支持为之网!
- 2024-11-01后台管理开发学习:新手入门指南
- 2024-11-01后台管理系统开发学习:新手入门教程
- 2024-11-01后台开发学习:从入门到实践的简单教程
- 2024-11-01后台综合解决方案学习:从入门到初级实战教程
- 2024-11-01接口模块封装学习入门教程
- 2024-11-01请求动作封装学习:新手入门教程
- 2024-11-01登录鉴权入门:新手必读指南
- 2024-11-01动态面包屑入门:轻松掌握导航设计技巧
- 2024-11-01动态权限入门:新手必读指南
- 2024-11-01动态主题处理入门:新手必读指南