人工智能计算机视觉自动驾驶 ORB-SLAM2源码逐行解析系列（一）：单目模式整体流程

腾讯云原生人工智能 2024-03-05 11 0

单目整体流程

前言：本系列文章以ORB-SLAM2的单目模式为研究对象，按照源码流程走向对源码做出了详细注释，并结合原理进行了详细阐述，旨在细致入微地对ORB-SLAM2做一个全面整体的了解。

1. ORB-SLAM2数据流思维导图

下图为以ORB-SLAM2单目模式为例，梳理了其中的数据流，对里面的函数和功能做了通俗的解释，可以对ORB-SLAM2的整体流程有一个清晰的了解，后面就以该数据流为导向，对其中的重点部分代码和原理进行详细解释。

2. 主函数

（1）主函数大致流程

LoadImages：从图片名文件strFile中读取图片名到vstrImageFilenames中ORB_SLAM2::System：构造SLAM系统，为SLAM运行做准备通过for循环依次读取每帧图像，并将读取到的每帧图像输入到SLAM系统中运行（SLAM.TrackMonocular）for循环结束后，关闭SLAM系统(SLAM.Shutdown)，保存SLAM轨迹(SLAM.SaveKeyFrameTrajectoryTUM)

（2）主函数代码实现

// /Example/Monocular/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;

}

vector vstrImageFilenames;

vector vTimestamps;

string strFile = string(argv[3])+"/rgb.txt";

// 将strFile中的文件(图片名)一次性读取到vstrImageFilenames中，并记录读取的每张图片对应的时间戳vTimestamps

LoadImages(strFile, vstrImageFilenames, vTimestamps);

int nImages = vstrImageFilenames.size(); // 读取的图片个数

// argv[1]: ORB字典文件的地址 argv[2]: 配置文件的地址

// 参数3表示单目模式参数4表示使用可视化界面

ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);

for(int ni=0; ni

{

im = cv::imread(string(argv[3])+"/"+ vstrImageFilenames[ni],

CV_LOAD_IMAGE_UNCHANGED);

if(im.empty())

{

cerr << endl << "Failed to load image at: "

<< string(argv[3]) << "/" << vstrImageFilenames[ni] << endl;

return 1;

}

// 将读取的图片和对应的时间戳输入到SLAM系统中，执行SLAM主程序

SLAM.TrackMonocular(im,tframe);

}

// Stop all threads 关闭SLAM

SLAM.Shutdown();

// Save camera trajectory 保存轨迹

SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

return 0;

}

3. 构造SLAM系统ORB_SLAM2::System

3.1 System类的定义

// /Examples/Monocular/include/System.h

class System

{

public:

// 这个枚举类型用于表示本系统所使用的传感器类型(SLAM的运行模式)

enum eSensor{

MONOCULAR=0,

STEREO=1,

RGBD=2

};

public:

// 构造函数，用来初始化整个系统。

System(const string &strVocFile, //指定ORB字典文件的路径

const string &strSettingsFile, //指定配置文件的路径

const eSensor sensor, //指定所使用的传感器类型

const bool bUseViewer = true); //指定是否使用可视化界面

/* 下面是针对三种不同类型的传感器所设计的三种运动追踪接口

彩色图像为CV_8UC3类型，并且都将会被转换成为灰度图像。

追踪接口返回估计的相机位姿，如果追踪失败则返回NULL

// 双目追踪

cv::Mat TrackStereo(const cv::Mat &imLeft, // 左目图像

const cv::Mat &imRight, // 右目图像

const double ×tamp); // 时间戳

// RGBD追踪

cv::Mat TrackRGBD(const cv::Mat &im, // 彩色图像

const cv::Mat &depthmap, // 深度图像

const double ×tamp); // 时间戳

// 单目追踪

cv::Mat TrackMonocular(const cv::Mat &im, // 图像

const double ×tamp); // 时间戳

// 使能定位模式，此时仅有运动追踪部分在工作，局部建图功能则不工作

void ActivateLocalizationMode();

// 反之同上

void DeactivateLocalizationMode();

// 获取从上次调用本函数后是否发生了比较大的地图变化

bool MapChanged();

// 复位系统

void Reset();

// 关闭系统，这将会关闭所有线程并且丢失曾经的各种数据

void Shutdown();

// 以TUM格式保存关键帧位姿到文件名filename中

void SaveKeyFrameTrajectoryTUM(const string &filename);

// See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php

// 以KITTI格式保存相机的运行轨迹到文件名filename中

void SaveTrajectoryKITTI(const string &filename);

// 获取最近的运动追踪状态、地图点追踪状态、特征点追踪状态（）

int GetTrackingState();

std::vector GetTrackedMapPoints();

std::vector GetTrackedKeyPointsUn();

};

3.2 System构造函数

（1）大致流程

读取yaml配置文件加载ORB字典创建关键帧、地图、帧绘制器和地图绘制器新建追踪线程、局部建图线程、回环线程以及可视化线程（可选）设置进程间的指针

（2）代码实现

// /Examples/Monocular/src/System.cc

System::System(const string &strVocFile, // ORB词典文件路径

const string &strSettingsFile, // 配置文件路径

const eSensor sensor, // 传感器类型

const bool bUseViewer): // 是否使用可视化界面

mSensor(sensor), // 初始化传感器类型

mpViewer(static_cast(NULL)),

mbReset(false), // 无复位标志

mbActivateLocalizationMode(false), // 没有这个模式转换标志

mbDeactivateLocalizationMode(false) // 没有这个模式转换标志

{

if(mSensor==MONOCULAR)

cout << "Monocular" << endl;

else if(mSensor==STEREO)

cout << "Stereo" << endl;

else if(mSensor==RGBD)

cout << "RGB-D" << endl;

// 使用opencv的cv::FileStorage读取配置文件

cv::FileStoragecv::FileStorage fsSettings(strSettingsFile.c_str(),

cv::FileStorage::READ);

// 建立一个新的ORB字典

mpVocabulary = new ORBVocabulary(); // 第三方库实现的

// 加载ORB字典

bool bVocLoad = mpVocabulary->loadFromTextFile(strVocFile); // 第三方库实现的

// 创建关键帧数据

mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);

// 创建地图

mpMap = new Map();

// 新建帧绘制器和地图绘制器，用于可视化

mpFrameDrawer = new FrameDrawer(mpMap);

mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);

// 新建(初始化)追踪线程

mpTracker = new Tracking(this,

mpVocabulary, // 字典

mpFrameDrawer, // 帧绘制器

mpMapDrawer, // 地图绘制器

mpMap, // 地图

mpKeyFrameDatabase, // 关键帧地图

strSettingsFile, // 设置文件路径

mSensor); // 传感器类型iomanip

// 新建(初始化)局部建图线程

mpLocalMapper = new LocalMapping(mpMap, // 单目

mSensor==MONOCULAR);

mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,

mpLocalMapper);

// 新建(初始化)回环线程

mpLoopCloser = new LoopClosing(mpMap, // 地图

mpKeyFrameDatabase, // 关键帧数据库

mpVocabulary, // ORB字典

mSensor!=MONOCULAR); // 当前的传感器是否是单目

mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run,

mpLoopCloser);

// 可视化设置

if(bUseViewer)

{

// 新建(初始化)viewer

mpViewer = new Viewer(this,

mpFrameDrawer, // 帧绘制器

mpMapDrawer, // 地图绘制器

mpTracker, // 追踪器

strSettingsFile); // 配置文件的访问路径

mptViewer = new thread(&Viewer::Run, mpViewer);

//给运动追踪器设置其查看器

mpTracker->SetViewer(mpViewer);

}

// 设置进程间的指针

mpTracker->SetLocalMapper(mpLocalMapper);

mpTracker->SetLoopClosing(mpLoopCloser);

mpLocalMapper->SetTracker(mpTracker);

mpLocalMapper->SetLoopCloser(mpLoopCloser);

mpLoopCloser->SetTracker(mpTracker);

mpLoopCloser->SetLocalMapper(mpLocalMapper);

}

4. 单目追踪TrackMonocular

（1）大致流程

传感器判断：是否为单目模式判断：是否关闭局部建图线程复位判断单目追踪核心函数GrabImageMonocular获取追踪结果

（2）代码实现

/src/System.cc

cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp)

{

// 传感器判断，单目才可以执行

if(mSensor!=MONOCULAR)

{

cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;

exit(-1);

}

// 模式判断

{

// 独占锁，主要是为了mbActivateLocalizationMode和mbDeactivateLocalizationMode不会发生混乱

unique_lock lock(mMutexMode);

// mbActivateLocalizationMode为true会关闭局部地图线程

if(mbActivateLocalizationMode)

{

mpLocalMapper->RequestStop();

// Wait until Local Mapping has effectively stopped

while(!mpLocalMapper->isStopped())

{

usleep(1000);

}

// 局部地图关闭以后，只进行追踪的线程，只计算相机的位姿，没有对局部地图进行更新

// 设置mbOnlyTracking为真

mpTracker->InformOnlyTracking(true);

// 关闭线程可以使得别的线程得到更多的资源

mbActivateLocalizationMode = false;

}

// 如果mbDeactivateLocalizationMode是true，局部地图线程就被释放, 关键帧从局部地图中删除.

if(mbDeactivateLocalizationMode)

{

mpTracker->InformOnlyTracking(false);

mpLocalMapper->Release();

mbDeactivateLocalizationMode = false;

}

// 复位判断

{

unique_lock lock(mMutexReset);

if(mbReset)

{

mpTracker->Reset();

mbReset = false;

}

// 根据输入的图片im和时间戳timestamp，获取相机位姿的估计结果

cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);

unique_lock lock2(mMutexState);

// 获取追踪状态

mTrackingState = mpTracker->mState;

// 获取追踪结果

mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;

mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;

return Tcw;

}

好文链接

评论可见，请评论后查看内容，谢谢！！！

您阅读本篇文章共花了：

人工智能计算机视觉自动驾驶

本文由用户于 2024-03-05 发布在金钥匙，如有疑问，请联系我们。
本文链接：https://www.51969.com/post/18599750.html

金钥匙

人工智能计算机视觉自动驾驶 ORB-SLAM2源码逐行解析系列（一）：单目模式整体流程

人工智能图像处理计算机视觉基础（13）——深度估计

计算机视觉（相机标定；内参；外参；畸变系数）

发表评论取消回复

金钥匙

人工智能 计算机视觉 自动驾驶 ORB-SLAM2源码逐行解析系列（一）：单目模式整体流程

人工智能 图像处理 计算机视觉基础（13）——深度估计

计算机视觉（相机标定；内参；外参；畸变系数）

相关文章

发表评论取消回复

人工智能计算机视觉自动驾驶 ORB-SLAM2源码逐行解析系列（一）：单目模式整体流程

人工智能图像处理计算机视觉基础（13）——深度估计