/**
* 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
#include "ImuTypes.h"
#include "Optimizer.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);
int main(int argc, char **argv)
{
if(argc < 5)
{
cerr << endl << "Usage: ./stereo_inertial_euroc path_to_vocabulary path_to_settings path_to_sequence_folder_1 path_to_times_file_1 (path_to_image_folder_2 path_to_times_file_2 ... path_to_image_folder_N path_to_times_file_N) " << endl;
return 1;
}
const int num_seq = (argc-3)/2;
cout << "num_seq = " << num_seq << endl;
bool bFileName= (((argc-3) % 2) == 1);
string file_name;
if (bFileName)
{
file_name = string(argv[argc-1]);
cout << "file name: " << file_name << endl;
}
// Load all sequences:
int seq;
vector< vector > vstrImageLeft;
vector< vector > vstrImageRight;
vector< vector > vTimestampsCam;
vector< vector > vAcc, vGyro;
vector< vector > vTimestampsImu;
vector nImages;
vector nImu;
vector first_imu(num_seq,0);
vstrImageLeft.resize(num_seq);
vstrImageRight.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> K_l;
fsSettings["RIGHT.K"] >> K_r;
fsSettings["LEFT.P"] >> P_l;
fsSettings["RIGHT.P"] >> P_r;
fsSettings["LEFT.R"] >> R_l;
fsSettings["RIGHT.R"] >> R_r;
fsSettings["LEFT.D"] >> D_l;
fsSettings["RIGHT.D"] >> D_r;
int rows_l = fsSettings["LEFT.height"];
int cols_l = fsSettings["LEFT.width"];
int rows_r = fsSettings["RIGHT.height"];
int cols_r = fsSettings["RIGHT.width"];
if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||
rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0)
{
cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
return -1;
}
cv::Mat M1l,M2l,M1r,M2r;
cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);
// Vector for tracking time statistics
vector 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::IMU_STEREO, true);
cv::Mat imLeft, imRight, imLeftRect, imRightRect;
for (seq = 0; seq vImuMeas;
double t_rect = 0;
int num_rect = 0;
int proccIm = 0;
for(int ni=0; ni >(t_End_Rect - t_Start_Rect).count();
double tframe = vTimestampsCam[seq][ni];
// Load imu measurements from previous frame
vImuMeas.clear();
if(ni>0)
while(vTimestampsImu[seq][first_imu[seq]]<=vTimestampsCam[seq][ni]) // while(vTimestampsImu[first_imu]<=vTimestampsCam[ni])
{
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]]));
first_imu[seq]++;
}
#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(imLeftRect,imRightRect,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();
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;
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]));
}
}
}