文章目录
- 0 引言
- 1 整体框架
- 1.1 整体流程
- 2 System主类
- 2.1 成员函数
- 2.2 成员变量
- 3 多线程
- 3.1 ORB-SLAM2中的多线程
- 3.2 加锁
0 引言
ORB-SLAM2是一种基于特征的视觉SLAM
(Simultaneous Localization and Mapping
)系统,它能够从单个、双目或RBGD
相机的输入中实时地同时定位相机的位置,并构建环境的三维地图。ORB-SLAM2
是在ORB-SLAM的基础上进行改进和扩展的版本。
本文主要对ORB-SLAM2
的整体框架,System
主类和多线程进行学习和总结,如有理解错误,欢迎指正交流。
1 整体框架
1.1 整体流程
ORB-SLAM2
整体框架如下图,主要流程可以概括为以下几个步骤:
-
特征提取和匹配:
ORB-SLAM2
首先对输入的图像进行特征提取,通常使用Oriented FAST and Rotated BRIEF (ORB)
算法来检测和描述图像中的特征点。然后,它使用特征描述子进行特征匹配,以在连续帧之间建立对应关系。 -
初始化:初始化阶段是在初始帧上建立初始地图并估计相机的初始位姿。
ORB-SLAM2
使用基于单目、双目或RGB-D
输入的不同方法来进行初始化。在单目或双目情况下,可以使用基于运动的方法或基于平面的方法来估计相机的初始位姿。在RGB-D
情况下,可以通过三角测量来估计初始位姿。 -
跟踪:跟踪阶段是
ORB-SLAM2
的核心部分,它通过连续图像帧之间的特征匹配和运动估计来实时定位相机。通过追踪特征点的运动,ORB-SLAM2
可以估计相机的位姿变化,并通过优化方法来减小累积误差。 -
局部地图更新:
ORB-SLAM2
通过局部地图来表示环境的三维结构。在跟踪过程中,它会不断地更新和扩展局部地图,包括添加新的地图点和关键帧。同时,ORB-SLAM2
还会执行一些优化步骤,如相机位姿优化、地图点优化等,以提高地图的一致性和准确性。 -
回环检测:回环检测是为了解决定位漂移和累积误差问题的关键步骤。
ORB-SLAM2
会在跟踪过程中检测可能的回环,并使用回环检测算法来识别和纠正回环。一旦回环被检测到,ORB-SLAM2
会进行全局优化来提高整体的一致性。 -
闭环优化:闭环优化是在回环检测之后执行的步骤,通过全局优化来进一步提高地图的一致性和准确性。
ORB-SLAM2
会使用所有的关键帧和地图点进行非线性优化,以减小累积误差并提高整体的位姿和地图质量。 -
地图管理:
ORB-SLAM2
会维护一个稠密的局部地图和一个稀疏的全局地图,用于表示环境的三维结构。地图管理模块负责管理和更新地图,包括删除冗余地图点、关键帧的选择和插入、地图点的筛选等。
以上是ORB-SLAM2
的主要流程和步骤。通过不断的特征提取、跟踪、地图更新、回环检测和优化,ORB-SLAM2
能够实现实时的定位和地图构建,并在大范围和长时间的场景中表现出较好的性能。
也有大佬绘制了更详细的流程图(以mono_tum.cc
的运行流程为例,建议下载学习):
👉 https://www.jianguoyun.com/p/Dc1MEhMQ-9KLBxjM3uED
此外,还有大佬已经中文注释了ORB_SLAM2可以参考理解代码:
👉 https://github.com/electech6/ORB_SLAM2_detailed_comments/tree/master
但是在学习以上的核心的主要流程之前,需要先熟悉ORB-SLAM2
中的System
主类和多线程…
2 System主类
System
类是ORB-SLAM2系统的主类,主要代码是头文件ORB_SLAM2/include/System.h
和源文件ORB_SLAM2/src/System.cc
,分析其主要的成员函数和成员变量。
2.1 成员函数
vscode
打开System.cc
文件,如下,可以看到成员函数的大纲:
具体成员函数的类型和定义如下:
成员函数 | 类型 | 定义 |
---|---|---|
System(const string &strVocFile, string &strSettingsFile, const eSensor sensor, const bool bUseViewer=true) | public | 构造System 函数 |
cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp) | public | 跟踪双目相机,返回相机位姿 |
cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp) | public | 跟踪RGBD相机,返回相机位姿 |
cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp) | public | 跟踪单目相机,返回相机位姿 |
void ActivateLocalizationMode() | public | 开启纯定位模式 |
void DeactivateLocalizationMode() | public | 关闭纯定位模式 |
bool System::MapChanged() | public | 检测地图是否有较大变化 |
void System::Reset() | public | 系统复位 |
void System::Shutdown() | public | 系统关闭 |
void System::SaveTrajectoryTUM(const string &filename) | public | 以TUM 格式保存相机运动轨迹 |
void System::SaveKeyFrameTrajectoryTUM(const string &filename) | public | 以TUM 格式保存关键帧位姿 |
void System::SaveTrajectoryKITTI(const string &filename) | public | 以KITTI 格式保存相机运动轨迹 |
int System::GetTrackingState() | public | 获取追踪器状态 |
vector<MapPoint*> System::GetTrackedMapPoints() | public | 获取追踪到的地图点 |
vector<cv::KeyPoint> System::GetTrackedKeyPointsUn() | public | 获取追踪到的关键帧的点 |
2.2 成员变量
主要的成员变量及其定义如下:
成员变量 | 类型 | 定义 |
---|---|---|
eSensor mSensor | private | 传感器类型单目相机MONOCULAR ,双目相机STEREO ,彩色深度相机RGBD |
ORBVocabulary* mpVocabulary | private | ORB 字典,保存ORB 描述子聚类结果 |
KeyFrameDatabase* mpKeyFrameDatabase | private | 关键帧数据库,保存ORB 描述子倒排索引 |
Map* mpMap | private | 地图 |
Tracking* mpTracker | private | 追踪器 |
LocalMapping* mpLocalMapper | private | 局部建图器 |
std::thread* mptLocalMapping | private | 局部建图线程 |
LoopClosing* mpLoopCloser | private | 回环检测器 |
std::thread* mptLoopClosing | private | 回环检测线程 |
Viewer* mpViewer | private | 查看器 |
FrameDrawer* mpFrameDrawer | private | 帧绘制器 |
MapDrawer* mpMapDrawer | private | 地图绘制器 |
std::thread* mptViewer | private | 查看器线程 |
int mTrackingState | private | 追踪状态 |
std::mutex mMutexState | private | 追踪状态加锁 |
bool mbActivateLocalizationMode | private | 开启纯定位模式 |
bool mbDeactivateLocalizationMode | private | 关闭纯定位模式 |
std::mutex mMutexMode | private | 纯定位模式加锁 |
bool mbReset | private | 系统复位 |
std::mutex mMutexReset | private | 系统复位加锁 |
都说ORB-SLAM2
有三大线程Tracking
,LocalMapping
和LoopClosing
线程,可从成员变量中只定义了LocalMapping
和LoopClosing
线程,其实Tracking
线程就是Syetem
类的主线程,构成三大线程,虽然Tracking
线程在代码实现上是主线程,但三者的关系其实是并发的。
3 多线程
刚刚学习到ORB-SLAM2
中主要有三大线程,其实SLAM
项目中一般都会使用多线程,由于某个节点可能同时订阅多个消息,或多个线程函数共享数据,为了防止在多个消息被订阅时发生处理时间过长或阻塞,而导致其他回调函数无法正常使用,也为了防止共享数据时在存储或调用时发生错乱,一般都会使用std::mutex(互斥锁)和std::thread(多线程管理)。
3.1 ORB-SLAM2中的多线程
ORB-SLAM2
中三大线程中的Tracking
线程产生关键帧的频率和时机不是固定的,三个线程同时运行,方便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));}
}
3.2 加锁
多线程一般都是和锁一起使用,ORB-SLAM2
中多线程和互斥锁一起使用,而互斥锁是有范围的,锁的有效性仅限于大括号{}
之内,程序运行出大括号之后就释放锁。另外,一把锁一般在某个时刻只有一个线程能够拿到,比如程序执行到某个需要锁的范围,但是锁正在另一个线程,那当前线程就会先停下来,直到其他线程释放这个锁,当前线程才能继续向下运行。
void KeyFrame::EraseConnection(KeyFrame *pKF) {// 以下大括号中的代码部分加锁{unique_lock<mutex> lock(mMutexConnections);if (mConnectedKeyFrameWeights.count(pKF)) {mConnectedKeyFrameWeights.erase(pKF);bUpdate = true;}}// 程序运行到这里就释放锁,比如下行代码未在加锁范围UpdateBestCovisibles();
}
至此,学习了ORB-SLAM2
中的System
主类的实现细节和ORB-SLAM2
中的多线程。后续在此基础上继续学习ORB-SLAM2
中的输入预处理部分的核心—特征点的提取、描述子的生成及特征点匹配等等。
Reference:
- https://github.com/raulmur/ORB_SLAM2
⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔