151 lines
4.5 KiB
C++
151 lines
4.5 KiB
C++
/**
|
|
* This file is part of ORB-SLAM3
|
|
*
|
|
* Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
|
|
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
|
|
*
|
|
* ORB-SLAM3 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-SLAM3 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-SLAM3.
|
|
* If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#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_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::MONOCULAR,true);
|
|
|
|
// Vector for tracking time statistics
|
|
vector<float> vTimesTrack;
|
|
vTimesTrack.resize(nImages);
|
|
|
|
// Main loop
|
|
cv::Mat im;
|
|
for(int ni=0; ni<nImages; ni++)
|
|
{
|
|
// Read image from file
|
|
im = cv::imread(vstrImageFilenames[ni],cv::IMREAD_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,vector<ORB_SLAM3::IMU::Point>(), vstrImageFilenames[ni]);
|
|
|
|
#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";
|
|
}
|
|
}
|