ORB

/** * 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"; } }