/**
* 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<iomanip>
#include<chrono>
#include<opencv2/core/core.hpp>
#include<System.h>
using namespace std;
void LoadImages(const string &strPathToSequence, vector<string> &vstrImageLeft,
vector<string> &vstrImageRight, vector<double> &vTimestamps);
int main(int argc, char **argv)
{
if(argc != 4)
{
cerr << endl << "Usage: ./stereo_test path_to_vocabulary path_to_settings path_to_sequence" << endl;
return 1;
}
// Retrieve paths to images
vector<string> vstrImageLeft;
vector<string> vstrImageRight;
vector<double> vTimestamps;
LoadImages(string(argv[3]), vstrImageLeft, vstrImageRight, vTimestamps);
const int nImages = vstrImageLeft.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::STEREO,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 imLeft, imRight;
for(int ni=0; ni<nImages; ni++)
{
// Read left and right images from file
imLeft = cv::imread(vstrImageLeft[ni],cv::IMREAD_UNCHANGED);
imRight = cv::imread(vstrImageRight[ni],cv::IMREAD_UNCHANGED);
double tframe = vTimestamps[ni];
if(imLeft.empty())
{
cerr << endl << "Failed to load image at: "
<< string(vstrImageLeft[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 images to the SLAM system
SLAM.TrackStereo(imLeft,imRight,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.SaveTrajectoryKITTI("CameraTrajectory.txt");
return 0;
}
void LoadImages(const string &strPathToSequence, vector<string> &vstrImageLeft,
vector<string> &vstrImageRight, 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/";
string strPrefixRight = strPathToSequence + "/image_1/";
const int nTimes = vTimestamps.size();
vstrImageLeft.resize(nTimes);
vstrImageRight.resize(nTimes);
for(int i=0; i<nTimes; i++)
{
stringstream ss;
ss << setfill('0') << setw(6) << i;
vstrImageLeft[i] = strPrefixLeft + ss.str() + ".png";
vstrImageRight[i] = strPrefixRight + ss.str() + ".png";
}
}
ORB_SLAM3双目测试 stereo_test.cc
最新推荐文章于 2024-06-29 17:12:21 发布