ICode9
精准搜索请尝试： 精确搜索
首页
编程语言>
数据库
系统相关
互联网
其他分享
Python
Java
JavaScript
android
PHP
首页 > 编程语言> 文章详细

ORB-SLAM2源码解析

2021-10-22 22:31:51  阅读：262  来源： 互联网

标签：const cc SLAM2 源码 线程 cpp txt ORB cv


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类型

这种将变量类型写进变量名的命名方法叫做匈牙利命名法

理解多线程

为什么要使用多线程

  1. 加快运行速度

    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));
    // ...
}

    开两个线程同时计算两个矩阵,在多核处理器上会加快运算速度

  2. 因为系统的随机性，各个步骤的运行顺序是不确定的

    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));
    }
}

多线程中的锁

为了防止多个线程同时操作同一变量造成混乱，所以引入锁机制

将成员函数本身设置为私有变量(privateorprotected)，并在操作他们的公有函数中设置锁

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 &timestamp)
cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double &timestamp)
cv::Mat TrackMonocular(const cv::Mat &im, const double &timestamp)
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 &timestamp)
STEREO cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timestamp)
RGBD cv::Mat TrackMonocular(const cv::Mat &im, const double &timestamp) 
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;
}

概念解释

特征点响应值和描述子的区别

  • 响应值描述的是该特征点的区分度大小
    • 响应值越大的点越应该被留用做特征点
    • 响应值类似于分数，分数越高的学生能力越强，越应该被录取
  • 描述子是特征点的一个哈希运算
    • 其大小无意义，仅用来在数据库中快速找回某些特征点
    • 描述子相当于学生的学号，系统随机运算出的一串数，用于定位学生

特征点和地图点的区别

  • 维度

    • 特征点是2D的，是相机图像上的点
    • 地图点是3D的，根据同一特征点在多个图片中的不同位置三角化得到的

  • 对应关系

    • 地图点在观测到它的帧上必对应某特征点
    • 特征点不一定能够成功三角化出地图点

双目相机和单目相机的数据

  • 从直接读取的数据来讲
    • 双目相机
      • 知道特征点的左目坐标、右目坐标，不知道特征点深度值
    • RGBD相机
      • 知道特征点的左目数据、特征点深度，不知道右目数据

回环检测的标准

  • 连续3帧场景都在之前出现过
    • 根据BOW判断
  • 当前3帧场景在之前出现的时机也具有一定连续性
    • 根据连续关键帧组判断

标签：const,cc,SLAM2,源码,线程,cpp,txt,ORB,cv
来源： https://www.cnblogs.com/fusheng-rextimmy/p/15440899.html

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

关于我们 | 联系我们 | 留言反馈

专注分享技术，共同学习，共同进步。侵权联系[81616952@qq.com]

Copyright (C)ICode9.com, All Rights Reserved.

ICode9版权所有