代码：
#include<iostream> #include<algorithm> #include<fstream> #include<chrono> #include<opencv2/core/core.hpp> #include <unistd.h> #include<System.h> using namespace std; 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;//每行格式长这样：1305033527.670034 rgb/1305033527.670034.png string sRGB; ss >> t;//字符串变成double vTimestamps.push_back(t);//把t加到 vTimestamps后面 ss >> sRGB; vstrImageFilenames.push_back(sRGB);//把sRGB 加到vstrImageFilenames 后面 } } } 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";//strFile 指向rgb.txt的路径 LoadImages(strFile, vstrImageFilenames, vTimestamps);//获取每一幅图片路径 int nImages = vstrImageFilenames.size();//获取 vstrImageFilenames的元素个数 // 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);// vTimesTrack和vstrImageFilenames的元素个数一样 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); //进入RGB文件夹读取某幅图片的数据 double tframe = vTimestamps[ni]; //如果某幅图片为空 if(im.empty()) { cerr << endl << "Failed to load image at: " << string(argv[3]) << "/" << vstrImageFilenames[ni] << endl; return 1; } #ifdef COMPILEDWITHC11//如果 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; }
------------------------------------------------------------
总的流程是：
1.加载图片路径
2.启动slam系统
3.循环{对每张图片：
->开始记时
->将图片载入入slam
->结束记时
->计算并保存用时时长
}
4.关闭slam线程
5. 保存关键帧运动轨迹
------------------------------------------------------------
上面代码用到的c++基础知识：
1.文件读写： #include <fstream> ofstream f //文件写操作 内存写入存储设备 ifstream f //文件读操作，存储设备读区到内存中 fstream f //读写操作，对打开的文件可进行读写操作 参考：https://blog.csdn.net/kingstar158/article/details/6859379 2.getline(f,s0); //此函数可读取整行，包括前导和嵌入的空格，并将其存储在字符串对象中。f是数据流，s0是要写入的字符串名 3.f.eof //C++ eof()函数返回true时是读到文件结束符0xFF，而文件结束符是最后一个字符的下一个字符。(f是文件名) 4.stringstream进行流输入输出操作 例如： #include <string> #include <sstream> #include <iostream> #include <stdio.h> using namespace std; int main() { stringstream sstream; string strResult; int nValue = 1000; // 将int类型的值放入输入流中 sstream << nValue; // 从sstream中抽取前面插入的int类型的值，赋给string类型 sstream >> strResult; cout << "[cout]strResult is: " << strResult << endl; printf("[printf]strResult is: %s\n", strResult.c_str()); return 0; } 5.push_back()函数的用法 函数将一个新的元素加到vector的最后面，位置为当前最后一个元素的下一个元素 string str; str.push_back('d');//在str字符串后面加上一个元素‘d’ 6.int main(int argc, char* argv[]) int argc 表示你在命令行下输入命令的时候，一共有多少个参数。 比方说你的程序编译后，可执行文件是test.exe D:\tc2>test , argc=1 D:\tc2>test.exe myarg1 myarg2 argc=3 char *argv[] 用来取得你所输入的参数 D:\tc2>test 这个时候，argc的值是1，argv[0]的值是 “test” D:\tc2>test myarg1 myarg2 这个时候，argc的值是3，argc[0]的值是”test”，argc[1]的值是”myarg1”，argc[2]的值是”myarg2”。 这个东东一般用来为程序提供非常重要的信息，如：数据文件名，等等。 如：copy a.c b.txt 这个时候，a.c和b.txt就是所谓的“非常重要的信息”。 7.main函数的return return 0 代表程序正常退出 return 1代表程序异常退出 8.#ifdef如果标识符被定义过，就会运行后面内容下面为例 #include <iostream> using namespace std; #define DEBUG int main( int argc, char * argv[] ) { #ifdef DEBUG cout << "Beginning execution of main()" << endl; #endif return 0; } 运行结果为： Beginning execution of main() Press any key to continue 9.steady_clock steady_clock 是单调的时钟，相当于教练手中的秒表；只会增长，适合用于记录程序耗时； system_clock 是系统的时钟；因为系统的时钟可以修改；甚至可以网络对时； 所以用系统时间计算时间差可能不准。 CLOCK_MONOTONIC：以绝对时间为准，获取的时间为系统重启到现在的时间，更改系统时间对它没有影响。