ORB_SLAM2源码解析-框架
程序员文章站
2022-03-16 17:18:04
...
看了很多SLAM的源码阅读和解析了,有的人已经写了很好了,但是对于一个刚入门的菜鸟来说还是看不太懂,没办法具体到每一个函数的功能,虽然大体的框架知道是怎么回事,但是细细想有搞不明白,所以决定自己写一个博客来详细记录学习的过程,重要的是做笔记,要不然总是忘记。
首先从框架说起,下面的代码来自ORB_SLAM2中的mono_kitti.cc
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<iomanip>
#include<opencv2/core/core.hpp>
#include"System.h"
using namespace std;
void LoadImages(const string &strSequence, vector<string> &vstrImageFilenames,
vector<double> &vTimestamps);
int main(int argc, char **argv)
{
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<string> vstrImageFilenames;
vector<double> vTimestamps;
LoadImages(string(argv[3]), vstrImageFilenames, vTimestamps);
int nImages = vstrImageFilenames.size();
// 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<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 im;
for(int ni=0; ni<nImages; 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
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.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<std::chrono::duration<double> >(t2 - t1).count();
vTimesTrack[ni]=ttrack;
// 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)
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<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.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
return 0;
}
void LoadImages(const string &strPathToSequence, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
ifstream fTimes;
string strPathTimeFile = strPathToSequence + "/times.txt";
fTimes.open(strPathTimeFile.c_str());
while(!fTimes.eof())
{
string s;
getline(fTimes,s);
if(!s.empty())
{
stringstream ss;
ss << s;
double t;
ss >> t;
vTimestamps.push_back(t);
}
}
string strPrefixLeft = strPathToSequence + "/image_0/";
const int nTimes = vTimestamps.size();
vstrImageFilenames.resize(nTimes);
for(int i=0; i<nTimes; i++)
{
stringstream ss;
ss << setfill('0') << setw(6) << i;
vstrImageFilenames[i] = strPrefixLeft + ss.str() + ".png";
}
}
这部分代码最主要的是读取图片并将图片送进SLAM系统,还有一些其他的比如说保存轨迹信息之类的后面再细讲。
以上都是个人理解,肯定有错误之处,也恳请大家帮忙指正,大家共同进步。