主要内容
- 加载图片和时间戳
- 初始化系统线程和处理帧 ☆
- 计算每帧的跟踪时间
#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<opencv2/core/core.hpp>
#include<System.h>
#include<unistd.h>
using namespace std;
void LoadImages(const string &strFile, vector<string> &vstrImageFilenames,
vector<double> &vTimestamps);
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;
}
/* 加载图片名字和时间戳 */
// Retrieve paths to images
vector<string> vstrImageFilenames;
vector<double> vTimestamps;
string strFile = string(argv[3])+"/rgb.txt";
LoadImages(strFile, 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(string(argv[3])+"/"+vstrImageFilenames[ni],CV_LOAD_IMAGE_UNCHANGED);
double tframe = vTimestamps[ni];
if(im.empty())
{
cerr << endl << "Failed to load image at: "
<< string(argv[3]) << "/" << 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;
}
/**
* @brief 导入图片
*
* @param[in] strFile 读入的文件名称
* @param[in&out] vstrImageFilenames 彩色图片名称
* @param[in&out] vTimestamps 记录时间戳
*/
void LoadImages(const string &strFile, vector<string> &vstrImageFilenames, vector<double> &vTimestamps)
{
ifstream f;
f.open(strFile.c_str());
// skip first three lines
// 前三行是注释,跳过
string s0;
getline(f,s0);
getline(f,s0);
getline(f,s0);
while(!f.eof())
{
string s;
getline(f,s);
if(!s.empty())
{
stringstream ss;
ss << s;
double t;
string sRGB;
ss >> t;
vTimestamps.push_back(t);
ss >> sRGB;
vstrImageFilenames.push_back(sRGB);
}
}
}
重要函数在于这行代码
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);