/** * 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 . */ #include #include #include #include #include #include #include #include #include "ImuTypes.h" using namespace std; void LoadImages(const string &strPathLeft, const string &strPathRight, const string &strPathTimes, vector &vstrImageLeft, vector &vstrImageRight, vector &vTimeStamps); void LoadIMU(const string &strImuPath, vector &vTimeStamps, vector &vAcc, vector &vGyro); double ttrack_tot = 0; int main(int argc, char **argv) { const int num_seq = (argc-3)/4; cout << "num_seq = " << num_seq << endl; bool bFileName= (((argc-3) % 4) == 1); string file_name; if (bFileName) file_name = string(argv[argc-1]); if(argc < 7) { cerr << endl << "Usage: ./stereo_inertial_tum_vi path_to_vocabulary path_to_settings path_to_image_folder_1 path_to_image_folder_2 path_to_times_file path_to_imu_data (trajectory_file_name)" << endl; return 1; } // Load all sequences: int seq; vector< vector > vstrImageLeftFilenames; vector< vector > vstrImageRightFilenames; vector< vector > vTimestampsCam; vector< vector > vAcc, vGyro; vector< vector > vTimestampsImu; vector nImages; vector nImu; vector first_imu(num_seq,0); vstrImageLeftFilenames.resize(num_seq); vstrImageRightFilenames.resize(num_seq); vTimestampsCam.resize(num_seq); vAcc.resize(num_seq); vGyro.resize(num_seq); vTimestampsImu.resize(num_seq); nImages.resize(num_seq); nImu.resize(num_seq); int tot_images = 0; for (seq = 0; seq vTimesTrack; vTimesTrack.resize(tot_images); cout << endl << "-------" << endl; cout.precision(17); /*cout << "Start processing sequence ..." << endl; cout << "Images in the sequence: " << nImages << endl; cout << "IMU data in the sequence: " << nImu << endl << endl;*/ // 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::IMU_STEREO, true, 0, file_name); int proccIm = 0; for (seq = 0; seq vImuMeas; proccIm = 0; cv::Ptr clahe = cv::createCLAHE(3.0, cv::Size(8, 8)); for(int ni=0; niapply(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; } // Load imu measurements from previous frame vImuMeas.clear(); if(ni>0) { // cout << "t_cam " << tframe << endl; while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni]) { // vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[first_imu],vGyro[first_imu],vTimestampsImu[first_imu])); vImuMeas.push_back(ORB_SLAM3::IMU::Point(vAcc[seq][first_imu[seq]].x,vAcc[seq][first_imu[seq]].y,vAcc[seq][first_imu[seq]].z, vGyro[seq][first_imu[seq]].x,vGyro[seq][first_imu[seq]].y,vGyro[seq][first_imu[seq]].z, vTimestampsImu[seq][first_imu[seq]])); // cout << "t_imu = " << fixed << vImuMeas.back().t << endl; first_imu[seq]++; } } /*cout << "first imu: " << first_imu[seq] << endl; cout << "first imu time: " << fixed << vTimestampsImu[seq][0] << endl; cout << "size vImu: " << vImuMeas.size() << endl;*/ #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,vImuMeas); #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 >(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 LoadIMU(const string &strImuPath, vector &vTimeStamps, vector &vAcc, vector &vGyro) { ifstream fImu; fImu.open(strImuPath.c_str()); vTimeStamps.reserve(5000); vAcc.reserve(5000); vGyro.reserve(5000); while(!fImu.eof()) { string s; getline(fImu,s); if (s[0] == '#') continue; if(!s.empty()) { string item; size_t pos = 0; double data[7]; int count = 0; while ((pos = s.find(',')) != string::npos) { item = s.substr(0, pos); data[count++] = stod(item); s.erase(0, pos + 1); } item = s.substr(0, pos); data[6] = stod(item); vTimeStamps.push_back(data[0]/1e9); vAcc.push_back(cv::Point3f(data[4],data[5],data[6])); vGyro.push_back(cv::Point3f(data[1],data[2],data[3])); } } }