diff --git a/Examples/Stereo/stereo_uzh-fpv.cc b/Examples/Stereo/stereo_uzh-fpv.cc new file mode 100644 index 0000000..3de76dc --- /dev/null +++ b/Examples/Stereo/stereo_uzh-fpv.cc @@ -0,0 +1,308 @@ +/** +* This file is part of ORB-SLAM3 +* +* Copyright (C) 2017-2021 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 . +*/ + +#include +#include +#include +#include +#include +#include + +#include + +#include + +using namespace std; + +void LoadImages(const string &strPath, const string &strPathTimesLeft, const string &strPathTimesRight, + vector &vstrImageLeft, vector &vstrImageRight, vector &vTimeStamps); + +double ttrack_tot = 0; +int main(int argc, char **argv) +{ + const int num_seq = (argc-3)/3; + cout << "num_seq = " << num_seq << endl; + bool bFileName= (((argc-3) % 3) == 1); + string file_name; + if (bFileName) + file_name = string(argv[argc-1]); + + if(argc < 6) + { + cerr << endl << "Usage: ./stereo_uzh-fpv path_to_vocabulary path_to_settings path_to_image_folder_1 path_to_times_file_left_1 path_to_times_file_right_1 (path_to_image_folder1_2 path_to_image_folder2_2 path_to_times_file_2 ... path_to_image_folder1_N path_to_image_folder2_N path_to_times_file_N) (trajectory_file_name)" << endl; + return 1; + } + + // Load all sequences: + int seq; + vector< vector > vstrImageLeftFilenames; + vector< vector > vstrImageRightFilenames; + vector< vector > vTimestampsCam; + vector nImages; + + vstrImageLeftFilenames.resize(num_seq); + vstrImageRightFilenames.resize(num_seq); + vTimestampsCam.resize(num_seq); + nImages.resize(num_seq); + + int tot_images = 0; + for (seq = 0; seq vTimesTrack; + vTimesTrack.resize(tot_images); + + cout << endl << "-------" << endl; + cout.precision(17); + + // 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); + float imageScale = SLAM.GetImageScale(); + + cout << endl << "-------" << endl; + cout.precision(17); + + cv::Mat imLeft, imRight; + cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); + + double t_resize = 0.f; + double t_track = 0.f; + + int proccIm = 0; + for (seq = 0; seq >(t_End_Resize - t_Start_Resize).count(); + SLAM.InsertResizeTime(t_resize); +#endif + } + + // clahe + clahe->apply(imLeft,imLeft); + clahe->apply(imRight,imRight); + + double tframe = vTimestampsCam[seq][ni]; + + if(imLeft.empty() || imRight.empty()) + { + cerr << endl << "Failed to load image at: " + << vstrImageLeftFilenames[seq][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.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 + +#ifdef REGISTER_TIMES + t_track = t_resize + std::chrono::duration_cast >(t2 - t1).count(); + SLAM.InsertTrackTime(t_track); +#endif + + double ttrack= std::chrono::duration_cast >(t2 - t1).count(); + ttrack_tot += ttrack; + // std::cout << "ttrack: " << ttrack << std::endl; + + vTimesTrack[ni]=ttrack; + + // Wait to load the next frame + double T=0; + if(ni0) + T = tframe-vTimestampsCam[seq][ni-1]; + + if(ttrack &vstrImageLeft, vector &vstrImageRight, vector &vTimeStamps) +{ + ifstream fTimes; + cout << strPathLeft << endl; + cout << strPathRight << endl; + cout << strPathTimes << endl; + fTimes.open(strPathTimes.c_str()); + vTimeStamps.reserve(5000); + vstrImageLeft.reserve(5000); + vstrImageRight.reserve(5000); + while(!fTimes.eof()) + { + string s; + getline(fTimes,s); + if(!s.empty()) + { + stringstream ss; + ss << s; + vstrImageLeft.push_back(strPathLeft + "/" + ss.str() + ".png"); + vstrImageRight.push_back(strPathRight + "/" + ss.str() + ".png"); + double t; + ss >> t; + vTimeStamps.push_back(t/1e9); + } + } +}*/ + +void LoadImages(const string &strPath, const string &strPathTimesLeft, const string &strPathTimesRight, + vector &vstrImageLeft, vector &vstrImageRight, vector &vTimeStamps) +{ + ifstream fTimesLeft, fTimesRight; + cout << strPath << endl; + cout << strPathTimesRight << endl; + cout << strPathTimesLeft << endl; + fTimesLeft.open(strPathTimesLeft.c_str()); + fTimesRight.open(strPathTimesRight.c_str()); + vTimeStamps.reserve(5000); + vstrImageLeft.reserve(5000); + vstrImageRight.reserve(5000); + while(!fTimesLeft.eof()) + { + string sLeft, sRight; + getline(fTimesLeft,sLeft); + getline(fTimesRight, sRight); + + if(!sLeft.empty()) + { + if (sLeft[0] == '#') + continue; + + int pos = sLeft.find(' '); + sLeft = sLeft.substr(pos+1, string::npos); + sRight = sRight.substr(pos+1, string::npos); + pos = sLeft.find(' '); + string item = sLeft.substr(0, pos); + string strImgLeft = sLeft.substr(pos+1, string::npos); +// std::cout << "The resulting left filename string is: " << strImgLeft << std::endl; + string strImgRight = sRight.substr(pos+1, string::npos); + std::remove(strImgLeft.begin(), strImgLeft.end(), ' '); + std::remove(strImgRight.begin(), strImgRight.end(), ' '); + + vstrImageLeft.push_back(strPath + "/" + strImgLeft); + vstrImageRight.push_back(strPath + "/" + strImgRight); + + double t = stod(item); + vTimeStamps.push_back(t); + } + } +}