orbslam2 流程图_ORB-SLAM代码详解之代码框架

最新推荐文章于 2025-01-18 21:29:26 发布

波西米亚混球

最新推荐文章于 2025-01-18 21:29:26 发布

阅读量283

点赞数

CC 4.0 BY-SA版权

文章标签： orbslam2 流程图

本文链接：https://blog.csdn.net/weixin_35061887/article/details/113022375

本文介绍了一个基于ORB-SLAM2的视觉里程计系统实现案例，详细展示了如何通过命令行工具处理KITTI数据集中的图像序列，并利用C++进行相机位姿追踪。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

/** * This file is part of ORB-SLAM2. * * Copyright (C) 2014-2016 Ra煤l Mur-Artal (University of Zaragoza) * For more information see * * ORB-SLAM2 is free software: you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * ORB-SLAM2 is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with ORB-SLAM2. If not, see . */

#include

#include"System.h"

using namespace std;

// 从文件夹中load图片进来

void LoadImages(const string &strSequence, vector &vstrImageFilenames,

vector &vTimestamps);

int main(int argc, char **argv)

{

// 判断参数是否符合规范

// ./mono_kitti path_to_vocabulary path_to_settings path_to_sequence

// 一共有四个参数：可执行文件argv[0]、字典的目录argv[1]、配置的路径argv[2]、图片序列的路径argv[3]

if(argc != 4)

{

cerr << endl << "Usage: ./mono_kitti path_to_vocabulary path_to_settings path_to_sequence" << endl;

return 1;

}

// Retrieve paths to images

vector vstrImageFilenames;

vector vTimestamps;

// 加载图片，传入图片路径、传回的是图片的文件名、每幅图片的时间戳

LoadImages(string(argv[3]), vstrImageFilenames, vTimestamps);

// 图片的个数

int nImages = vstrImageFilenames.size();

// 对SLAM系统进行初始化，传入字典和配置的路径

// Create SLAM system. It initializes all system threads and gets ready to process frames.

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

// Vector for tracking time statistics

vector vTimesTrack;

// 计算追踪所花的时间

vTimesTrack.resize(nImages);

cout << endl << "-------" << endl;

cout << "Start processing sequence ..." << endl;

cout << "Images in the sequence: " << nImages << endl << endl;

// Main loop

cv::Mat im;

for(int ni=0; ni

{

// Read image from file

im = cv::imread(vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);

double tframe = vTimestamps[ni];

if(im.empty())

{

cerr << endl << "Failed to load image at: " << vstrImageFilenames[ni] << endl;

return 1;

}

#ifdef COMPILEDWITHC11

// 如果编译器可以编译c++11

// 获取当前时间

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

#else

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

#endif

// Pass the image to the SLAM system

// 图片放入SLAM系统中进行追踪

SLAM.TrackMonocular(im,tframe);

#ifdef COMPILEDWITHC11

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

#else

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

#endif

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

// 计算追踪该图片花的时间

vTimesTrack[ni]=ttrack;

// Wait to load the next frame

// 计算下一帧图片时间戳与当前时间戳的差值T，与追踪所需时间进行比较

// 如果有必要就将当前线程暂停sleep

// 主要是为了模拟时间情况，因为追踪结束以后下一帧可能还没来

double T=0;

if(ni

T = vTimestamps[ni+1]-tframe;

else if(ni>0)

T = tframe-vTimestamps[ni-1];

if(ttrack

usleep((T-ttrack)*1e6);

}

// Stop all threads

// 追踪完所有图片序列以后，关掉当前的线程

SLAM.Shutdown();

// Tracking time statistics

// 对追踪的部分进行一个统计

// 计算中位数，总数、平均值

sort(vTimesTrack.begin(),vTimesTrack.end());

float totaltime = 0;

for(int ni=0; 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.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

return 0;

}

void LoadImages(const string &strPathToSequence, vector &vstrImageFilenames, vector &vTimestamps)

{

ifstream fTimes;

// 读4541幅图片的时间戳(kitti的00图片序列)

string strPathTimeFile = strPathToSequence + "/times.txt";

// string类型转化为char*类型

fTimes.open(strPathTimeFile.c_str());

while(!fTimes.eof())

{

string s;

getline(fTimes,s);

if(!s.empty())

{

// string类型转化为double类型

// 使用stringstream类型

stringstream ss;

ss << s;

double t;

ss >> t;

vTimestamps.push_back(t);

}

// 使用image_0目录下的文件，这是双目摄像头的左边的摄像头的序列

string strPrefixLeft = strPathToSequence + "/image_0/";

const int nTimes = vTimestamps.size();

vstrImageFilenames.resize(nTimes);

for(int i=0; i

{

stringstream ss;

// 设置0~nTime-1的图片的路径

ss << setfill('0') << setw(6) << i;

vstrImageFilenames[i] = strPrefixLeft + ss.str() + ".png";

}