211 lines
7.4 KiB
C++
211 lines
7.4 KiB
C++
/*
|
|
* OpenVINS: An Open Platform for Visual-Inertial Research
|
|
* Copyright (C) 2018-2022 Patrick Geneva
|
|
* Copyright (C) 2018-2022 Guoquan Huang
|
|
* Copyright (C) 2018-2022 OpenVINS Contributors
|
|
* Copyright (C) 2018-2019 Kevin Eckenhoff
|
|
*
|
|
* This program 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.
|
|
*
|
|
* This program 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 this program. If not, see <https://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#include <Eigen/Eigen>
|
|
#include <boost/algorithm/string/predicate.hpp>
|
|
#include <boost/filesystem.hpp>
|
|
#include <boost/foreach.hpp>
|
|
#include <fstream>
|
|
#include <iostream>
|
|
#include <string>
|
|
|
|
#include "alignment/AlignTrajectory.h"
|
|
#include "alignment/AlignUtils.h"
|
|
#include "utils/Loader.h"
|
|
#include "utils/colors.h"
|
|
#include "utils/print.h"
|
|
#include "utils/quat_ops.h"
|
|
|
|
#ifdef HAVE_PYTHONLIBS
|
|
|
|
// import the c++ wrapper for matplot lib
|
|
// https://github.com/lava/matplotlib-cpp
|
|
// sudo apt-get install python-matplotlib python-numpy python2.7-dev
|
|
#include "plot/matplotlibcpp.h"
|
|
|
|
// Will plot the xy 3d position of the pose trajectories
|
|
void plot_xy_positions(const std::string &name, const std::string &color, const std::vector<Eigen::Matrix<double, 7, 1>> &poses) {
|
|
|
|
// Paramters for our line
|
|
std::map<std::string, std::string> params;
|
|
params.insert({"label", name});
|
|
params.insert({"linestyle", "-"});
|
|
params.insert({"color", color});
|
|
|
|
// Create vectors of our x and y axis
|
|
std::vector<double> x, y;
|
|
for (size_t i = 0; i < poses.size(); i++) {
|
|
x.push_back(poses.at(i)(0));
|
|
y.push_back(poses.at(i)(1));
|
|
}
|
|
|
|
// Finally plot
|
|
matplotlibcpp::plot(x, y, params);
|
|
}
|
|
|
|
// Will plot the z 3d position of the pose trajectories
|
|
void plot_z_positions(const std::string &name, const std::string &color, const std::vector<double> ×,
|
|
const std::vector<Eigen::Matrix<double, 7, 1>> &poses) {
|
|
|
|
// Paramters for our line
|
|
std::map<std::string, std::string> params;
|
|
params.insert({"label", name});
|
|
params.insert({"linestyle", "-"});
|
|
params.insert({"color", color});
|
|
|
|
// Create vectors of our x and y axis
|
|
std::vector<double> time, z;
|
|
for (size_t i = 0; i < poses.size(); i++) {
|
|
time.push_back(times.at(i));
|
|
z.push_back(poses.at(i)(2));
|
|
}
|
|
|
|
// Finally plot
|
|
matplotlibcpp::plot(time, z, params);
|
|
}
|
|
|
|
#endif
|
|
|
|
int main(int argc, char **argv) {
|
|
|
|
// Verbosity setting
|
|
ov_core::Printer::setPrintLevel("INFO");
|
|
|
|
// Ensure we have a path
|
|
if (argc < 3) {
|
|
PRINT_ERROR(RED "ERROR: Please specify a align mode and trajectory file\n" RESET);
|
|
PRINT_ERROR(RED "ERROR: ./plot_trajectories <align_mode> <file_gt.txt> <file_est1.txt> ... <file_est9.txt>\n" RESET);
|
|
PRINT_ERROR(RED "ERROR: rosrun ov_eval plot_trajectories <align_mode> <file_gt.txt> <file_est1.txt> ... <file_est9.txt>\n" RESET);
|
|
std::exit(EXIT_FAILURE);
|
|
}
|
|
|
|
// Read in all our trajectories from file
|
|
std::vector<std::string> names;
|
|
std::vector<std::vector<double>> times;
|
|
std::vector<std::vector<Eigen::Matrix<double, 7, 1>>> poses;
|
|
for (int i = 2; i < argc; i++) {
|
|
|
|
// Read in trajectory data
|
|
std::vector<double> times_temp;
|
|
std::vector<Eigen::Matrix<double, 7, 1>> poses_temp;
|
|
std::vector<Eigen::Matrix3d> cov_ori_temp, cov_pos_temp;
|
|
ov_eval::Loader::load_data(argv[i], times_temp, poses_temp, cov_ori_temp, cov_pos_temp);
|
|
|
|
// Align all the non-groundtruth trajectories to the base one
|
|
if (i > 2) {
|
|
|
|
// Intersect timestamps
|
|
std::vector<double> gt_times_temp(times.at(0));
|
|
std::vector<Eigen::Matrix<double, 7, 1>> gt_poses_temp(poses.at(0));
|
|
ov_eval::AlignUtils::perform_association(0, 0.02, times_temp, gt_times_temp, poses_temp, gt_poses_temp);
|
|
|
|
// Return failure if we didn't have any common timestamps
|
|
if (poses_temp.size() < 3) {
|
|
PRINT_ERROR(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET);
|
|
PRINT_ERROR(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET);
|
|
std::exit(EXIT_FAILURE);
|
|
}
|
|
|
|
// Perform alignment of the trajectories
|
|
Eigen::Matrix3d R_ESTtoGT;
|
|
Eigen::Vector3d t_ESTinGT;
|
|
double s_ESTtoGT;
|
|
ov_eval::AlignTrajectory::align_trajectory(poses_temp, gt_poses_temp, R_ESTtoGT, t_ESTinGT, s_ESTtoGT, argv[1]);
|
|
|
|
// Debug print to the user
|
|
Eigen::Vector4d q_ESTtoGT = ov_core::rot_2_quat(R_ESTtoGT);
|
|
PRINT_DEBUG("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1),
|
|
q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT);
|
|
|
|
// Finally lets calculate the aligned trajectories
|
|
std::vector<Eigen::Matrix<double, 7, 1>> est_poses_aignedtoGT;
|
|
for (size_t j = 0; j < gt_times_temp.size(); j++) {
|
|
Eigen::Matrix<double, 7, 1> pose_ESTinGT;
|
|
pose_ESTinGT.block(0, 0, 3, 1) = s_ESTtoGT * R_ESTtoGT * poses_temp.at(j).block(0, 0, 3, 1) + t_ESTinGT;
|
|
pose_ESTinGT.block(3, 0, 4, 1) = ov_core::quat_multiply(poses_temp.at(j).block(3, 0, 4, 1), ov_core::Inv(q_ESTtoGT));
|
|
est_poses_aignedtoGT.push_back(pose_ESTinGT);
|
|
}
|
|
|
|
// Overwrite our poses with the corrected ones
|
|
poses_temp = est_poses_aignedtoGT;
|
|
}
|
|
|
|
// Debug print the length stats
|
|
boost::filesystem::path path(argv[i]);
|
|
std::string name = path.stem().string();
|
|
double length = ov_eval::Loader::get_total_length(poses_temp);
|
|
PRINT_INFO("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times_temp.size(), name.c_str(), length);
|
|
|
|
// Save this to our arrays
|
|
names.push_back(name);
|
|
times.push_back(times_temp);
|
|
poses.push_back(poses_temp);
|
|
}
|
|
|
|
#ifdef HAVE_PYTHONLIBS
|
|
|
|
// Colors that we are plotting
|
|
std::vector<std::string> colors = {"black", "blue", "red", "green", "cyan", "magenta"};
|
|
// assert(algo_rpe.size() <= colors.size()*linestyle.size());
|
|
|
|
// Plot this figure
|
|
matplotlibcpp::figure_size(1000, 750);
|
|
|
|
// Plot the position trajectories
|
|
for (size_t i = 0; i < times.size(); i++) {
|
|
plot_xy_positions(names.at(i), colors.at(i), poses.at(i));
|
|
}
|
|
|
|
// Display to the user
|
|
matplotlibcpp::xlabel("x-axis (m)");
|
|
matplotlibcpp::ylabel("y-axis (m)");
|
|
matplotlibcpp::show(false);
|
|
|
|
// Plot this figure
|
|
matplotlibcpp::figure_size(1000, 350);
|
|
|
|
// Zero our time arrays
|
|
double starttime = (times.at(0).empty()) ? 0 : times.at(0).at(0);
|
|
double endtime = (times.at(0).empty()) ? 0 : times.at(0).at(times.at(0).size() - 1);
|
|
for (size_t i = 0; i < times.size(); i++) {
|
|
for (size_t j = 0; j < times.at(i).size(); j++) {
|
|
times.at(i).at(j) -= starttime;
|
|
}
|
|
}
|
|
|
|
// Plot the position trajectories
|
|
for (size_t i = 0; i < times.size(); i++) {
|
|
plot_z_positions(names.at(i), colors.at(i), times.at(i), poses.at(i));
|
|
}
|
|
|
|
// Display to the user
|
|
matplotlibcpp::xlabel("timestamp (sec)");
|
|
matplotlibcpp::ylabel("z-axis (m)");
|
|
matplotlibcpp::xlim(0.0, endtime - starttime);
|
|
matplotlibcpp::legend();
|
|
matplotlibcpp::show(true);
|
|
|
|
#endif
|
|
|
|
// Done!
|
|
return EXIT_SUCCESS;
|
|
}
|