01 整体代码运行流程

2023-12-17 13:25:11

01 整体代码运行流程

1.1 运行官方 Demo

以 stereo_kitti 为例,执行

./stereo_kitti path_to_vocabulary path_to_settings path_to_sequence
  • ./stereo_kitti:可执行文件

  • path_to_vocabulary:字典路径

  • path_to_settings:配置文件路径,包含相机参数和 ORB 特征提取参数

  • path_to_sequence:数据集路径

主函数

int main(int argc, char **argv)
{
    if(argc != 4)
    {
        cerr << endl << "Usage: ./stereo_kitti path_to_vocabulary path_to_settings path_to_sequence" << endl;
        return 1;
    }

    // 载入左右目图片、时间戳
    vector<string> vstrImageLeft;
    vector<string> vstrImageRight;
    vector<double> vTimestamps;
    LoadImages(string(argv[3]), vstrImageLeft, vstrImageRight, vTimestamps);

    const int nImages = vstrImageLeft.size();

    // 创建 SLAM 对象
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true);

    // Vector for tracking time statistics
    vector<float> vTimesTrack;
    vTimesTrack.resize(nImages);

    cout << endl << "-------" << endl;
    cout << "Start processing sequence ..." << endl;
    cout << "Images in the sequence: " << nImages << endl << endl;

    // Main loop
    cv::Mat imLeft, imRight;
    for(int ni=0; ni<nImages; ni++)
    {
        // Read left and right images from file
        imLeft = cv::imread(vstrImageLeft[ni],CV_LOAD_IMAGE_UNCHANGED);
        imRight = cv::imread(vstrImageRight[ni],CV_LOAD_IMAGE_UNCHANGED);
        double tframe = vTimestamps[ni];    // 记录对应的时间

        if(imLeft.empty())
        {
            cerr << endl << "Failed to load image at: "
                 << string(vstrImageLeft[ni]) << endl;
            return 1;
        }

        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();

        // Pass the images to the SLAM system
        SLAM.TrackStereo(imLeft,imRight,tframe);        

        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();

        double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();

        vTimesTrack[ni]=ttrack;     // 记录下每次 track 耗时

        // Wait to load the next frame
        double T=0;
        if(ni<nImages-1)
            T = vTimestamps[ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimestamps[ni-1];

        if(ttrack<T)
			this_thread::sleep_for(std::chrono::microseconds((int)((T-ttrack)*1e6)));
    }

    // Stop all threads
    SLAM.Shutdown();

    // Tracking time statistics
    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;

    // Save camera trajectory
    SLAM.SaveTrajectoryKITTI("CameraTrajectory.txt");

    return 0;
}

1.2 变量命名规则

  • m 开头的变量表示为某类的成员变量;

  • 变量名的第一、二个字母表示其数据类型:

    s 表示 std::set 类型

    v 表示 std::vector 类型

    l 表示 std::list 类型

    p 表示指针类型

    n 表示 int 类型

    b 表示 bool 类型

    KF 表示 KeyFrame 类型

1.3 多线程

在这里插入图片描述

包括 Tracking、LocalMapping、LoopClosing 三个线程。

当 Tracking 不产生关键帧时,LocalMapping、LoopClosing 线程基本处于空转状态;并且,Tracking 线程产生关键帧的频率和时机是不固定的,因此需要三个线程同时进行,LocalMapping 和 LoopClosing 不断循环查询 Tracking 是否产生了关键帧。

1.4 线程锁

unique_lock<mutex> lock(mMutexConnections); 即为加锁,锁的有效性仅限于大括号内,也就是说,到大括号外,锁就会自动释放。所以程序中的一些大括号并非可有可无,需要注意。

1.5 SLAM 主类 System

成员变量/函数访问控制备注
eSensor mSensorprivate传感器类型,可选 MONOCULAR、STEREO、RGBD
ORBVocabulary* mpVocabularyprivate字典,用来保存 ORB 描述子聚类结果
KeyFrameDatabase* mpKeyFrameDatabaseprivate关键帧数据库
Map* mpMapprivate地图
Tracking* mpTrackerprivate追踪器
LocalMapping* mpLocalMapperprivate局部建图、BA
LoopClosing* mpLoopCloserprivate回环检测
Viewer* mpViewerprivate查看器
FrameDrawer* mpFrameDrawerprivate帧绘制器
MapDrawer* mpMapDrawerprivate地图绘制器
std::thread* mptLocalMappingprivate局部建图线程
std::thread* mptLoopClosingprivate回环检测线程
std::thread* mptViewerprivate查看器线程
System(...)public构造函数:初始化 SLAM 系统,启动 建图、回环、查看器线程
cv::Mat TrackStereo(...)
cv::Mat TrackRGBD(...)
cv::Mat TrackMonocular(...)
public
public
public
追踪双目相机,返回相机位姿
追踪 RGBD 相机,返回相机位姿
追踪单目相机,返回相机位姿
void ActivateLocalizationMode()
void DeactivateLocalizationMode()
std::mutex mMutexMode
bool mbActivateLocalizationMode
bool mbDeactivateLocalizationMode
public
public
private
private
private
开启纯定位模式(关闭建图线程)
关闭纯定位模式
void Reset()
std::mutex mMutexReset
bool mbReset
public
private
private
系统复位
void Shutdown()public系统关闭
void SaveTrajectoryTUM
void SaveKeyFrameTrajectoryTUM()
void SaveTrajectoryKITTI()
public
public
public
以 TUM/KITTI 格式保存相机运动轨迹和关键帧位姿

构造函数


// 依次传入数据集、配置文件、传感器类型、bUseViewer
System::System(const string &strVocFile, const string &strSettingsFile, const eSensor sensor,
               const bool bUseViewer):mSensor(sensor),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 创建关键帧数据库
    mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
    
    // Step1.4 创建地图
    mpMap = new Map();
    
    // Step1.5 绘图
    mpFrameDrawer = new FrameDrawer(mpMap);
    mpMapDrawer = new MapDrawer(mpMap, strSettingsFile);
    
    // Step2 三大线程
    // Step2.1 Tracking 线程,只需创建 Tracking 对象即可
    mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                             mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor);
    
    // Step2.2 创建 Local Mapping 线程和 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);
    
    // 线程间通信
    mpTracker->SetLocalMapper(mpLocalMapper);
    mpTracker->SetLoopClosing(mpLoopCloser);

    mpLocalMapper->SetTracker(mpTracker);
    mpLocalMapper->SetLoopCloser(mpLoopCloser);

    mpLoopCloser->SetTracker(mpTracker);
    mpLoopCloser->SetLocalMapper(mpLocalMapper);
}

注意到,创建 Tracking 线程时,并没有 std::thread 成员变量,仅仅初始化了 Tracking 对象。这是因为,在逻辑上三个线程是并发的,互不包含;但实际编程中,我们将 Tracking 线程视为主线程,LocalMapping 和 LoopClosing 为子线程,Tracking 通过持有两个子线程的指针实现对其控制。

文章来源:https://blog.csdn.net/qq_44175983/article/details/135042792
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。