initial commit

ov_eval/src/alignment/AlignTrajectory.cpp

@@ -0,0 +1,166 @@
+/*
+ * 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 "AlignTrajectory.h"
+
+using namespace ov_eval;
+
+void AlignTrajectory::align_trajectory(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
+                                       const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t,
+                                       double &s, std::string method, int n_aligned) {
+
+  // Use the correct method
+  if (method == "posyaw") {
+    s = 1;
+    align_posyaw(traj_es, traj_gt, R, t, n_aligned);
+  } else if (method == "posyawsingle") {
+    s = 1;
+    align_posyaw_single(traj_es, traj_gt, R, t);
+  } else if (method == "se3") {
+    s = 1;
+    align_se3(traj_es, traj_gt, R, t, n_aligned);
+  } else if (method == "se3single") {
+    s = 1;
+    align_se3_single(traj_es, traj_gt, R, t);
+  } else if (method == "sim3") {
+    assert(n_aligned >= 2 || n_aligned == -1);
+    align_sim3(traj_es, traj_gt, R, t, s, n_aligned);
+  } else if (method == "none") {
+    s = 1;
+    R.setIdentity();
+    t.setZero();
+  } else {
+    PRINT_ERROR(RED "[ALIGN]: Invalid alignment method!\n" RESET);
+    PRINT_ERROR(RED "[ALIGN]: Possible options: posyaw, posyawsingle, se3, se3single, sim3, none\n" RESET);
+    std::exit(EXIT_FAILURE);
+  }
+}
+
+void AlignTrajectory::align_posyaw_single(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
+                                          const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t) {
+
+  // Get first ever poses
+  Eigen::Vector4d q_es_0 = traj_es.at(0).block(3, 0, 4, 1);
+  Eigen::Vector3d p_es_0 = traj_es.at(0).block(0, 0, 3, 1);
+
+  Eigen::Vector4d q_gt_0 = traj_gt.at(0).block(3, 0, 4, 1);
+  Eigen::Vector3d p_gt_0 = traj_gt.at(0).block(0, 0, 3, 1);
+
+  // Get rotations from IMU frame to World (note JPL!)
+  Eigen::Matrix3d g_rot = ov_core::quat_2_Rot(q_gt_0).transpose();
+  Eigen::Matrix3d est_rot = ov_core::quat_2_Rot(q_es_0).transpose();
+
+  // Data matrix for the Frobenius problem
+  Eigen::Matrix3d C_R = est_rot * g_rot.transpose();
+
+  // Recover yaw
+  double theta = AlignUtils::get_best_yaw(C_R);
+
+  // Compute rotation
+  R = ov_core::rot_z(theta);
+
+  // Compute translation
+  t.noalias() = p_gt_0 - R * p_es_0;
+}
+
+void AlignTrajectory::align_posyaw(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
+                                   const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t,
+                                   int n_aligned) {
+
+  // If we only have one, just use the single alignment
+  if (n_aligned == 1) {
+    align_posyaw_single(traj_es, traj_gt, R, t);
+  } else {
+
+    // Get just position vectors
+    assert(!traj_es.empty());
+    std::vector<Eigen::Vector3d> pos_est, pos_gt;
+    for (size_t i = 0; i < traj_es.size() && i < traj_gt.size(); i++) {
+      pos_est.push_back(traj_es.at(i).block(0, 0, 3, 1));
+      pos_gt.push_back(traj_gt.at(i).block(0, 0, 3, 1));
+    }
+
+    // Align using the method of Umeyama
+    double s;
+    AlignUtils::align_umeyama(pos_est, pos_gt, R, t, s, true, true);
+    assert(s == 1);
+  }
+}
+
+void AlignTrajectory::align_se3_single(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
+                                       const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t) {
+
+  // Get first ever poses
+  Eigen::Vector4d q_es_0 = traj_es.at(0).block(3, 0, 4, 1);
+  Eigen::Vector3d p_es_0 = traj_es.at(0).block(0, 0, 3, 1);
+
+  Eigen::Vector4d q_gt_0 = traj_gt.at(0).block(3, 0, 4, 1);
+  Eigen::Vector3d p_gt_0 = traj_gt.at(0).block(0, 0, 3, 1);
+
+  // Get rotations from IMU frame to World (note JPL!)
+  Eigen::Matrix3d g_rot = ov_core::quat_2_Rot(q_gt_0).transpose();
+  Eigen::Matrix3d est_rot = ov_core::quat_2_Rot(q_es_0).transpose();
+
+  R.noalias() = g_rot * est_rot.transpose();
+  t.noalias() = p_gt_0 - R * p_es_0;
+}
+
+void AlignTrajectory::align_se3(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
+                                const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t,
+                                int n_aligned) {
+
+  // If we only have one, just use the single alignment
+  if (n_aligned == 1) {
+    align_se3_single(traj_es, traj_gt, R, t);
+  } else {
+
+    // Get just position vectors
+    assert(!traj_es.empty());
+    std::vector<Eigen::Vector3d> pos_est, pos_gt;
+    for (size_t i = 0; i < traj_es.size() && i < traj_gt.size(); i++) {
+      pos_est.push_back(traj_es.at(i).block(0, 0, 3, 1));
+      pos_gt.push_back(traj_gt.at(i).block(0, 0, 3, 1));
+    }
+
+    // Align using the method of Umeyama
+    double s;
+    AlignUtils::align_umeyama(pos_est, pos_gt, R, t, s, true, false);
+  }
+}
+
+void AlignTrajectory::align_sim3(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
+                                 const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t, double &s,
+                                 int n_aligned) {
+
+  // Need to have more than two to get
+  assert(n_aligned >= 2 || n_aligned == -1);
+
+  // Get just position vectors
+  assert(!traj_es.empty());
+  std::vector<Eigen::Vector3d> pos_est, pos_gt;
+  for (size_t i = 0; i < traj_es.size() && i < traj_gt.size(); i++) {
+    pos_est.push_back(traj_es.at(i).block(0, 0, 3, 1));
+    pos_gt.push_back(traj_gt.at(i).block(0, 0, 3, 1));
+  }
+
+  // Align using the method of Umeyama
+  AlignUtils::align_umeyama(pos_est, pos_gt, R, t, s, false, false);
+}

ov_eval/src/alignment/AlignTrajectory.h

@@ -0,0 +1,121 @@
+/*
+ * 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/>.
+ */
+
+#ifndef OV_EVAL_ALIGNTRAJECTORY_H
+#define OV_EVAL_ALIGNTRAJECTORY_H
+
+#include <Eigen/Eigen>
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "AlignUtils.h"
+
+#include "utils/colors.h"
+#include "utils/print.h"
+
+namespace ov_eval {
+
+/**
+ * @brief Class that given two trajectories, will align the two.
+ *
+ * Given two trajectories that have already been time synchronized we will compute the alignment transform between the two.
+ * We can do this using different alignment methods such as full SE(3) transform, just postiion and yaw, or SIM(3).
+ * These scripts are based on the [rpg_trajectory_evaluation](https://github.com/uzh-rpg/rpg_trajectory_evaluation) toolkit by Zhang and
+ * Scaramuzza. Please take a look at their [2018 IROS](http://rpg.ifi.uzh.ch/docs/IROS18_Zhang.pdf) paper on these methods.
+ */
+class AlignTrajectory {
+
+public:
+  /**
+   * @brief Align estimate to GT using a desired method using a set of initial poses
+   * @param traj_es Estimated trajectory values in estimate frame [pos,quat]
+   * @param traj_gt Groundtruth trjaectory in groundtruth frame [pos,quat]
+   * @param R Rotation from estimate to GT frame that will be computed
+   * @param t translation from estimate to GT frame that will be computed
+   * @param s scale from estimate to GT frame that will be computed
+   * @param method Method used for alignment
+   * @param n_aligned Number of poses to use for alignment (-1 will use all)
+   */
+  static void align_trajectory(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
+                               const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t, double &s,
+                               std::string method, int n_aligned = -1);
+
+protected:
+  /**
+   * @brief Align estimate to GT using only position and yaw (for gravity aligned trajectories) using only the first poses
+   * @param traj_es Estimated trajectory values in estimate frame [pos,quat]
+   * @param traj_gt Groundtruth trjaectory in groundtruth frame [pos,quat]
+   * @param R Rotation from estimate to GT frame that will be computed
+   * @param t translation from estimate to GT frame that will be computed
+   */
+  static void align_posyaw_single(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
+                                  const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t);
+
+  /**
+   * @brief Align estimate to GT using only position and yaw (for gravity aligned trajectories) using a set of initial poses
+   * @param traj_es Estimated trajectory values in estimate frame [pos,quat]
+   * @param traj_gt Groundtruth trjaectory in groundtruth frame [pos,quat]
+   * @param R Rotation from estimate to GT frame that will be computed
+   * @param t translation from estimate to GT frame that will be computed
+   * @param n_aligned Number of poses to use for alignment (-1 will use all)
+   */
+  static void align_posyaw(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es, const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt,
+                           Eigen::Matrix3d &R, Eigen::Vector3d &t, int n_aligned = -1);
+
+  /**
+   * @brief Align estimate to GT using a full SE(3) transform using only the first poses
+   * @param traj_es Estimated trajectory values in estimate frame [pos,quat]
+   * @param traj_gt Groundtruth trjaectory in groundtruth frame [pos,quat]
+   * @param R Rotation from estimate to GT frame that will be computed
+   * @param t translation from estimate to GT frame that will be computed
+   */
+  static void align_se3_single(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
+                               const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t);
+
+  /**
+   * @brief Align estimate to GT using a full SE(3) transform using a set of initial poses
+   * @param traj_es Estimated trajectory values in estimate frame [pos,quat]
+   * @param traj_gt Groundtruth trjaectory in groundtruth frame [pos,quat]
+   * @param R Rotation from estimate to GT frame that will be computed
+   * @param t translation from estimate to GT frame that will be computed
+   * @param n_aligned Number of poses to use for alignment (-1 will use all)
+   */
+  static void align_se3(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es, const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt,
+                        Eigen::Matrix3d &R, Eigen::Vector3d &t, int n_aligned = -1);
+
+  /**
+   * @brief Align estimate to GT using a full SIM(3) transform using a set of initial poses
+   * @param traj_es Estimated trajectory values in estimate frame [pos,quat]
+   * @param traj_gt Groundtruth trjaectory in groundtruth frame [pos,quat]
+   * @param R Rotation from estimate to GT frame that will be computed
+   * @param t translation from estimate to GT frame that will be computed
+   * @param s scale from estimate to GT frame that will be computed
+   * @param n_aligned Number of poses to use for alignment (-1 will use all)
+   */
+  static void align_sim3(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es, const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt,
+                         Eigen::Matrix3d &R, Eigen::Vector3d &t, double &s, int n_aligned = -1);
+};
+
+} // namespace ov_eval
+
+#endif /* OV_EVAL_ALIGNTRAJECTORY_H */

ov_eval/src/alignment/AlignUtils.cpp

@@ -0,0 +1,189 @@
+/*
+ * 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 "AlignUtils.h"
+
+using namespace ov_eval;
+
+void AlignUtils::align_umeyama(const std::vector<Eigen::Matrix<double, 3, 1>> &data, const std::vector<Eigen::Matrix<double, 3, 1>> &model,
+                               Eigen::Matrix<double, 3, 3> &R, Eigen::Matrix<double, 3, 1> &t, double &s, bool known_scale, bool yaw_only) {
+
+  assert(model.size() == data.size());
+
+  // Substract mean of each trajectory
+  Eigen::Matrix<double, 3, 1> mu_M = get_mean(model);
+  Eigen::Matrix<double, 3, 1> mu_D = get_mean(data);
+  std::vector<Eigen::Matrix<double, 3, 1>> model_zerocentered, data_zerocentered;
+  for (size_t i = 0; i < model.size(); i++) {
+    model_zerocentered.push_back(model[i] - mu_M);
+    data_zerocentered.push_back(data[i] - mu_D);
+  }
+
+  // Get correlation matrix
+  double n = model.size();
+  Eigen::Matrix<double, 3, 3> C = Eigen::Matrix<double, 3, 3>::Zero();
+  for (size_t i = 0; i < model_zerocentered.size(); i++) {
+    C.noalias() += model_zerocentered[i] * data_zerocentered[i].transpose();
+  }
+  C *= 1.0 / n;
+
+  // Get data sigma
+  double sigma2 = 0;
+  for (size_t i = 0; i < data_zerocentered.size(); i++) {
+    sigma2 += data_zerocentered[i].dot(data_zerocentered[i]);
+  }
+  sigma2 *= 1.0 / n;
+
+  // SVD decomposition
+  Eigen::JacobiSVD<Eigen::Matrix<double, 3, 3>> svd(C, Eigen::ComputeFullV | Eigen::ComputeFullU);
+
+  Eigen::Matrix<double, 3, 3> U_svd = svd.matrixU();
+  Eigen::Matrix<double, 3, 1> D_svd = svd.singularValues();
+  Eigen::Matrix<double, 3, 3> V_svd = svd.matrixV();
+
+  Eigen::Matrix<double, 3, 3> S = Eigen::Matrix<double, 3, 3>::Identity();
+  if (U_svd.determinant() * V_svd.determinant() < 0) {
+    S(2, 2) = -1;
+  }
+
+  // If only yaw, use that specific solver (optimizes over yaw angle)
+  // Else get best full 3 dof rotation
+  if (yaw_only) {
+    Eigen::Matrix<double, 3, 3> rot_C = n * C.transpose();
+    double theta = AlignUtils::get_best_yaw(rot_C);
+    R = ov_core::rot_z(theta);
+  } else {
+    R.noalias() = U_svd * S * V_svd.transpose();
+  }
+
+  // If known scale, fix it
+  if (known_scale) {
+    s = 1;
+  } else {
+    // Get best scale
+    s = 1.0 / sigma2 * (D_svd.asDiagonal() * S).trace();
+  }
+
+  // Get best translation
+  t.noalias() = mu_M - s * R * mu_D;
+
+  // Debug printing
+  // std::stringstream ss;
+  // ss << "R- " << std::endl << R << std::endl;
+  // ss << "t- " << std::endl << t << std::endl;
+  // PRINT_DEBUG(ss.str().c_str());
+}
+
+void AlignUtils::perform_association(double offset, double max_difference, std::vector<double> &est_times, std::vector<double> &gt_times,
+                                     std::vector<Eigen::Matrix<double, 7, 1>> &est_poses,
+                                     std::vector<Eigen::Matrix<double, 7, 1>> &gt_poses) {
+  std::vector<Eigen::Matrix3d> est_covori, est_covpos, gt_covori, gt_covpos;
+  AlignUtils::perform_association(offset, max_difference, est_times, gt_times, est_poses, gt_poses, est_covori, est_covpos, gt_covori,
+                                  gt_covpos);
+}
+
+void AlignUtils::perform_association(double offset, double max_difference, std::vector<double> &est_times, std::vector<double> &gt_times,
+                                     std::vector<Eigen::Matrix<double, 7, 1>> &est_poses,
+                                     std::vector<Eigen::Matrix<double, 7, 1>> &gt_poses, std::vector<Eigen::Matrix3d> &est_covori,
+                                     std::vector<Eigen::Matrix3d> &est_covpos, std::vector<Eigen::Matrix3d> &gt_covori,
+                                     std::vector<Eigen::Matrix3d> &gt_covpos) {
+
+  // Temp results which keeps only the matches
+  std::vector<double> est_times_temp, gt_times_temp;
+  std::vector<Eigen::Matrix<double, 7, 1>> est_poses_temp, gt_poses_temp;
+  std::vector<Eigen::Matrix3d> est_covori_temp, est_covpos_temp, gt_covori_temp, gt_covpos_temp;
+
+  // Iterator over gt (only ever increases to enforce injectivity of matches)
+  size_t gt_pointer = 0;
+
+  // Try to find closest GT pose for each estimate
+  for (size_t i = 0; i < est_times.size(); i++) {
+
+    // Default params
+    double best_diff = max_difference;
+    int best_gt_idx = -1;
+
+    // Increment while too small and is not within our difference threshold
+    while (gt_pointer < gt_times.size() && gt_times.at(gt_pointer) < (est_times.at(i) + offset) &&
+           std::abs(gt_times.at(gt_pointer) - (est_times.at(i) + offset)) > max_difference) {
+      gt_pointer++;
+    }
+
+    // If we are closer than max difference, see if we can do any better
+    while (gt_pointer < gt_times.size() && std::abs(gt_times.at(gt_pointer) - (est_times.at(i) + offset)) <= max_difference) {
+      // Break if we found a good match but are getting worse, we are done
+      if (std::abs(gt_times.at(gt_pointer) - (est_times.at(i) + offset)) >= best_diff) {
+        break;
+      }
+      // We have a closer match, save it and move on
+      best_diff = std::abs(gt_times.at(gt_pointer) - (est_times.at(i) + offset));
+      best_gt_idx = gt_pointer;
+      gt_pointer++;
+    }
+
+    // Did we get a valid match
+    if (best_gt_idx != -1) {
+
+      // Save estimate and gt states for the match
+      est_times_temp.push_back(gt_times.at(best_gt_idx));
+      est_poses_temp.push_back(est_poses.at(i));
+      gt_times_temp.push_back(gt_times.at(best_gt_idx));
+      gt_poses_temp.push_back(gt_poses.at(best_gt_idx));
+
+      // If we have covariance then also append it
+      // If the groundtruth doesn't have covariance say it is 100% certain
+      if (!est_covori.empty()) {
+        assert(est_covori.size() == est_covpos.size());
+        est_covori_temp.push_back(est_covori.at(i));
+        est_covpos_temp.push_back(est_covpos.at(i));
+        if (gt_covori.empty()) {
+          gt_covori_temp.push_back(Eigen::Matrix3d::Zero());
+          gt_covpos_temp.push_back(Eigen::Matrix3d::Zero());
+        } else {
+          assert(gt_covori.size() == gt_covpos.size());
+          gt_covori_temp.push_back(gt_covori.at(best_gt_idx));
+          gt_covpos_temp.push_back(gt_covpos.at(best_gt_idx));
+        }
+      }
+    }
+  }
+
+  // Ensure that we have enough associations
+  if (est_times.size() < 3) {
+    PRINT_ERROR(RED "[ALIGN]: Was unable to associate groundtruth and estimate trajectories\n" RESET);
+    PRINT_ERROR(RED "[ALIGN]: %d total matches....\n" RESET, (int)est_times.size());
+    PRINT_ERROR(RED "[ALIGN]: Do the time of the files match??\n" RESET);
+    return;
+  }
+  assert(est_times_temp.size() == gt_times_temp.size());
+  // PRINT_DEBUG("[TRAJ]: %d est poses and %d gt poses => %d
+  // matches\n",(int)est_times.size(),(int)gt_times.size(),(int)est_times_temp.size());
+
+  // Overwrite with intersected values
+  est_times = est_times_temp;
+  est_poses = est_poses_temp;
+  est_covori = est_covori_temp;
+  est_covpos = est_covpos_temp;
+  gt_times = gt_times_temp;
+  gt_poses = gt_poses_temp;
+  gt_covori = gt_covori_temp;
+  gt_covpos = gt_covpos_temp;
+}

ov_eval/src/alignment/AlignUtils.h

@@ -0,0 +1,110 @@
+/*
+ * 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/>.
+ */
+
+#ifndef OV_EVAL_ALIGNUTILS_H
+#define OV_EVAL_ALIGNUTILS_H
+
+#include <Eigen/Eigen>
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <vector>
+
+#include "utils/colors.h"
+#include "utils/print.h"
+#include "utils/quat_ops.h"
+
+namespace ov_eval {
+
+/**
+ * @brief Helper functions for the trajectory alignment class.
+ *
+ * The key function is an implementation of Umeyama's
+ * [Least-squares estimation of transformation parameters between two point patterns](https://ieeexplore.ieee.org/document/88573).
+ * This is what allows us to find the transform between the two
+ * trajectories without worrying about singularities for the absolute trajectory error.
+ */
+class AlignUtils {
+
+public:
+  /**
+   * @brief Gets best yaw from Frobenius problem.
+   * Equation (17)-(18) in [Zhang et al. 2018 IROS](http://rpg.ifi.uzh.ch/docs/IROS18_Zhang.pdf) paper.
+   * @param C Data matrix
+   */
+  static inline double get_best_yaw(const Eigen::Matrix<double, 3, 3> &C) {
+    double A = C(0, 1) - C(1, 0);
+    double B = C(0, 0) + C(1, 1);
+    // return M_PI_2 - atan2(B, A);
+    return atan2(A, B);
+  }
+
+  /**
+   * @brief Gets mean of the vector of data
+   * @param data Vector of data
+   * @return Mean value
+   */
+  static inline Eigen::Matrix<double, 3, 1> get_mean(const std::vector<Eigen::Matrix<double, 3, 1>> &data) {
+    Eigen::Matrix<double, 3, 1> mean = Eigen::Matrix<double, 3, 1>::Zero();
+    for (size_t i = 0; i < data.size(); i++) {
+      mean.noalias() += data[i];
+    }
+    mean /= data.size();
+    return mean;
+  }
+
+  /**
+   * @brief Given a set of points in a model frame and a set of points in a data frame,
+   * finds best transform between frames (subject to constraints).
+   *
+   * @param data Vector of points in data frame (i.e., estimates)
+   * @param model Vector of points in model frame (i.e., gt)
+   * @param R Output rotation from data frame to model frame
+   * @param t Output translation from data frame to model frame
+   * @param s Output scale from data frame to model frame
+   * @param known_scale Whether to fix scale
+   * @param yaw_only Whether to only use yaw orientation (such as when frames are already gravity aligned)
+   */
+  static void align_umeyama(const std::vector<Eigen::Matrix<double, 3, 1>> &data, const std::vector<Eigen::Matrix<double, 3, 1>> &model,
+                            Eigen::Matrix<double, 3, 3> &R, Eigen::Matrix<double, 3, 1> &t, double &s, bool known_scale, bool yaw_only);
+
+  /**
+   * @brief Will intersect our loaded data so that we have common timestamps.
+   * @param offset Time offset to append to our estimate
+   * @param max_difference Biggest allowed difference between matched timesteps
+   */
+  static void perform_association(double offset, double max_difference, std::vector<double> &est_times, std::vector<double> &gt_times,
+                                  std::vector<Eigen::Matrix<double, 7, 1>> &est_poses, std::vector<Eigen::Matrix<double, 7, 1>> &gt_poses);
+
+  /**
+   * @brief Will intersect our loaded data so that we have common timestamps.
+   * @param offset Time offset to append to our estimate
+   * @param max_difference Biggest allowed difference between matched timesteps
+   */
+  static void perform_association(double offset, double max_difference, std::vector<double> &est_times, std::vector<double> &gt_times,
+                                  std::vector<Eigen::Matrix<double, 7, 1>> &est_poses, std::vector<Eigen::Matrix<double, 7, 1>> &gt_poses,
+                                  std::vector<Eigen::Matrix3d> &est_covori, std::vector<Eigen::Matrix3d> &est_covpos,
+                                  std::vector<Eigen::Matrix3d> &gt_covori, std::vector<Eigen::Matrix3d> &gt_covpos);
+};
+
+} // namespace ov_eval
+
+#endif // OV_EVAL_ALIGNUTILS_H

ov_eval/src/calc/ResultSimulation.cpp

@@ -0,0 +1,500 @@
+/*
+ * 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 "ResultSimulation.h"
+
+using namespace ov_eval;
+
+ResultSimulation::ResultSimulation(std::string path_est, std::string path_std, std::string path_gt) {
+
+  // Load from disk
+  Loader::load_simulation(path_est, est_state);
+  Loader::load_simulation(path_std, state_cov);
+  Loader::load_simulation(path_gt, gt_state);
+
+  /// Assert they are of equal length
+  assert(est_state.size() == state_cov.size());
+  assert(est_state.size() == gt_state.size());
+
+  // Debug print
+  PRINT_DEBUG("[SIM]: loaded %d timestamps from file!!\n", (int)est_state.size());
+  PRINT_DEBUG("[SIM]: we have %d cameras in total!!\n", (int)est_state.at(0)(18));
+}
+
+void ResultSimulation::plot_state(bool doplotting, double max_time) {
+
+  // Errors for each xyz direction
+  Statistics error_ori[3], error_pos[3], error_vel[3], error_bg[3], error_ba[3];
+
+  // Calculate the position and orientation error at every timestep
+  double start_time = est_state.at(0)(0);
+  for (size_t i = 0; i < est_state.size(); i++) {
+
+    // Exit if we have reached our max time
+    if ((est_state.at(i)(0) - start_time) > max_time)
+      break;
+
+    // Assert our times are the same
+    assert(est_state.at(i)(0) == gt_state.at(i)(0));
+
+    // Calculate orientation error
+    // NOTE: we define our error as e_R = -Log(R*Rhat^T)
+    Eigen::Matrix3d e_R =
+        ov_core::quat_2_Rot(gt_state.at(i).block(1, 0, 4, 1)) * ov_core::quat_2_Rot(est_state.at(i).block(1, 0, 4, 1)).transpose();
+    Eigen::Vector3d ori_err = -180.0 / M_PI * ov_core::log_so3(e_R);
+    for (int j = 0; j < 3; j++) {
+      error_ori[j].timestamps.push_back(est_state.at(i)(0));
+      error_ori[j].values.push_back(ori_err(j));
+      error_ori[j].values_bound.push_back(3 * 180.0 / M_PI * state_cov.at(i)(1 + j));
+      error_ori[j].calculate();
+    }
+
+    // Calculate position error
+    Eigen::Vector3d pos_err = gt_state.at(i).block(5, 0, 3, 1) - est_state.at(i).block(5, 0, 3, 1);
+    for (int j = 0; j < 3; j++) {
+      error_pos[j].timestamps.push_back(est_state.at(i)(0));
+      error_pos[j].values.push_back(pos_err(j));
+      error_pos[j].values_bound.push_back(3 * state_cov.at(i)(4 + j));
+      error_pos[j].calculate();
+    }
+
+    // Calculate velocity error
+    Eigen::Vector3d vel_err = gt_state.at(i).block(8, 0, 3, 1) - est_state.at(i).block(8, 0, 3, 1);
+    for (int j = 0; j < 3; j++) {
+      error_vel[j].timestamps.push_back(est_state.at(i)(0));
+      error_vel[j].values.push_back(vel_err(j));
+      error_vel[j].values_bound.push_back(3 * state_cov.at(i)(7 + j));
+      error_vel[j].calculate();
+    }
+
+    // Calculate gyro bias error
+    Eigen::Vector3d bg_err = gt_state.at(i).block(11, 0, 3, 1) - est_state.at(i).block(11, 0, 3, 1);
+    for (int j = 0; j < 3; j++) {
+      error_bg[j].timestamps.push_back(est_state.at(i)(0));
+      error_bg[j].values.push_back(bg_err(j));
+      error_bg[j].values_bound.push_back(3 * state_cov.at(i)(10 + j));
+      error_bg[j].calculate();
+    }
+
+    // Calculate accel bias error
+    Eigen::Vector3d ba_err = gt_state.at(i).block(14, 0, 3, 1) - est_state.at(i).block(14, 0, 3, 1);
+    for (int j = 0; j < 3; j++) {
+      error_ba[j].timestamps.push_back(est_state.at(i)(0));
+      error_ba[j].values.push_back(ba_err(j));
+      error_ba[j].values_bound.push_back(3 * state_cov.at(i)(13 + j));
+      error_ba[j].calculate();
+    }
+  }
+
+  // return if we don't want to plot
+  if (!doplotting)
+    return;
+
+#ifndef HAVE_PYTHONLIBS
+  PRINT_ERROR(RED "Unable to plot the state error, just returning..\n" RESET);
+  return;
+#else
+
+  //=====================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(1000, 500);
+  plot_3errors(error_ori[0], error_ori[1], error_ori[2], "blue", "red");
+  // Update the title and axis labels
+  matplotlibcpp::subplot(3, 1, 1);
+  matplotlibcpp::title("IMU Orientation Error");
+  matplotlibcpp::ylabel("x-error (deg)");
+  matplotlibcpp::subplot(3, 1, 2);
+  matplotlibcpp::ylabel("y-error (deg)");
+  matplotlibcpp::subplot(3, 1, 3);
+  matplotlibcpp::ylabel("z-error (deg)");
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::show(false);
+  //=====================================================
+
+  //=====================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(1000, 500);
+  plot_3errors(error_pos[0], error_pos[1], error_pos[2], "blue", "red");
+  // Update the title and axis labels
+  matplotlibcpp::subplot(3, 1, 1);
+  matplotlibcpp::title("IMU Position Error");
+  matplotlibcpp::ylabel("x-error (m)");
+  matplotlibcpp::subplot(3, 1, 2);
+  matplotlibcpp::ylabel("y-error (m)");
+  matplotlibcpp::subplot(3, 1, 3);
+  matplotlibcpp::ylabel("z-error (m)");
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::show(false);
+  //=====================================================
+
+  //=====================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(1000, 500);
+  plot_3errors(error_vel[0], error_vel[1], error_vel[2], "blue", "red");
+  // Update the title and axis labels
+  matplotlibcpp::subplot(3, 1, 1);
+  matplotlibcpp::title("IMU Velocity Error");
+  matplotlibcpp::ylabel("x-error (m/s)");
+  matplotlibcpp::subplot(3, 1, 2);
+  matplotlibcpp::ylabel("y-error (m/s)");
+  matplotlibcpp::subplot(3, 1, 3);
+  matplotlibcpp::ylabel("z-error (m/s)");
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::show(false);
+  //=====================================================
+
+  //=====================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(1000, 500);
+  plot_3errors(error_bg[0], error_bg[1], error_bg[2], "blue", "red");
+  // Update the title and axis labels
+  matplotlibcpp::subplot(3, 1, 1);
+  matplotlibcpp::title("IMU Gyroscope Bias Error");
+  matplotlibcpp::ylabel("x-error (rad/s)");
+  matplotlibcpp::subplot(3, 1, 2);
+  matplotlibcpp::ylabel("y-error (rad/s)");
+  matplotlibcpp::subplot(3, 1, 3);
+  matplotlibcpp::ylabel("z-error (rad/s)");
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::show(false);
+  //=====================================================
+
+  //=====================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(1000, 500);
+  plot_3errors(error_ba[0], error_ba[1], error_ba[2], "blue", "red");
+  // Update the title and axis labels
+  matplotlibcpp::subplot(3, 1, 1);
+  matplotlibcpp::title("IMU Accelerometer Bias Error");
+  matplotlibcpp::ylabel("x-error (m/s^2)");
+  matplotlibcpp::subplot(3, 1, 2);
+  matplotlibcpp::ylabel("y-error (m/s^2)");
+  matplotlibcpp::subplot(3, 1, 3);
+  matplotlibcpp::ylabel("z-error (m/s^2)");
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::show(false);
+  //=====================================================
+
+#endif
+}
+
+void ResultSimulation::plot_timeoff(bool doplotting, double max_time) {
+
+  // Calculate the time offset error at every timestep
+  Statistics error_time;
+  double start_time = est_state.at(0)(0);
+  for (size_t i = 0; i < est_state.size(); i++) {
+
+    // Exit if we have reached our max time
+    if ((est_state.at(i)(0) - start_time) > max_time)
+      break;
+
+    // Assert our times are the same
+    assert(est_state.at(i)(0) == gt_state.at(i)(0));
+
+    // If we are not calibrating then don't plot it!
+    if (state_cov.at(i)(16) == 0.0) {
+      PRINT_WARNING(YELLOW "Time offset was not calibrated online, so will not plot...\n" RESET);
+      return;
+    }
+
+    // Calculate time difference
+    error_time.timestamps.push_back(est_state.at(i)(0));
+    error_time.values.push_back(est_state.at(i)(17) - gt_state.at(i)(17));
+    error_time.values_bound.push_back(3 * state_cov.at(i)(16));
+    error_time.calculate();
+  }
+
+  // return if we don't want to plot
+  if (!doplotting)
+    return;
+
+#ifndef HAVE_PYTHONLIBS
+  PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET);
+  return;
+#else
+
+  //=====================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(800, 250);
+
+  // Zero our time array
+  double starttime = (error_time.timestamps.empty()) ? 0 : error_time.timestamps.at(0);
+  double endtime = (error_time.timestamps.empty()) ? 0 : error_time.timestamps.at(error_time.timestamps.size() - 1);
+  for (size_t i = 0; i < error_time.timestamps.size(); i++) {
+    error_time.timestamps.at(i) -= starttime;
+  }
+
+  // Parameters that define the line styles
+  std::map<std::string, std::string> params_value, params_bound;
+  params_value.insert({"label", "error"});
+  params_value.insert({"linestyle", "-"});
+  params_value.insert({"color", "blue"});
+  params_bound.insert({"label", "3 sigma bound"});
+  params_bound.insert({"linestyle", "--"});
+  params_bound.insert({"color", "red"});
+
+  // Plot our error value
+  matplotlibcpp::plot(error_time.timestamps, error_time.values, params_value);
+  if (!error_time.values_bound.empty()) {
+    matplotlibcpp::plot(error_time.timestamps, error_time.values_bound, params_bound);
+    for (size_t i = 0; i < error_time.timestamps.size(); i++) {
+      error_time.values_bound.at(i) *= -1;
+    }
+    matplotlibcpp::plot(error_time.timestamps, error_time.values_bound, "r--");
+  }
+  matplotlibcpp::xlim(0.0, endtime - starttime);
+
+  // Update the title and axis labels
+  matplotlibcpp::title("Camera IMU Time Offset Error");
+  matplotlibcpp::ylabel("error (sec)");
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::show(false);
+  //=====================================================
+
+#endif
+}
+
+void ResultSimulation::plot_cam_instrinsics(bool doplotting, double max_time) {
+
+  // Check that we have cameras
+  if ((int)est_state.at(0)(18) < 1) {
+    PRINT_ERROR(YELLOW "You need at least one camera to plot intrinsics...\n" RESET);
+    return;
+  }
+
+  // Camera intrinsics statistic storage
+  std::vector<std::vector<Statistics>> error_cam_k, error_cam_d;
+  for (int i = 0; i < (int)est_state.at(0)(18); i++) {
+    std::vector<Statistics> temp1, temp2;
+    for (int j = 0; j < 4; j++) {
+      temp1.push_back(Statistics());
+      temp2.push_back(Statistics());
+    }
+    error_cam_k.push_back(temp1);
+    error_cam_d.push_back(temp2);
+  }
+
+  // Loop through and calculate error
+  double start_time = est_state.at(0)(0);
+  for (size_t i = 0; i < est_state.size(); i++) {
+
+    // Exit if we have reached our max time
+    if ((est_state.at(i)(0) - start_time) > max_time)
+      break;
+
+    // Assert our times are the same
+    assert(est_state.at(i)(0) == gt_state.at(i)(0));
+
+    // If we are not calibrating then don't plot it!
+    if (state_cov.at(i)(18) == 0.0) {
+      PRINT_WARNING(YELLOW "Camera intrinsics not calibrated online, so will not plot...\n" RESET);
+      return;
+    }
+
+    // Loop through each camera and calculate error
+    for (int n = 0; n < (int)est_state.at(0)(18); n++) {
+      for (int j = 0; j < 4; j++) {
+        error_cam_k.at(n).at(j).timestamps.push_back(est_state.at(i)(0));
+        error_cam_k.at(n).at(j).values.push_back(est_state.at(i)(19 + 15 * n + j) - gt_state.at(i)(19 + 15 * n + j));
+        error_cam_k.at(n).at(j).values_bound.push_back(3 * state_cov.at(i)(18 + 14 * n + j));
+        error_cam_d.at(n).at(j).timestamps.push_back(est_state.at(i)(0));
+        error_cam_d.at(n).at(j).values.push_back(est_state.at(i)(19 + 4 + 15 * n + j) - gt_state.at(i)(19 + 4 + 15 * n + j));
+        error_cam_d.at(n).at(j).values_bound.push_back(3 * state_cov.at(i)(18 + 4 + 14 * n + j));
+      }
+    }
+  }
+
+  // return if we don't want to plot
+  if (!doplotting)
+    return;
+
+#ifndef HAVE_PYTHONLIBS
+  PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET);
+  return;
+#else
+
+  // Plot line colors
+  std::vector<std::string> colors = {"blue", "red", "black", "green", "cyan", "magenta"};
+  assert(error_cam_k.size() <= colors.size());
+
+  //=====================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(800, 600);
+  for (int n = 0; n < (int)est_state.at(0)(18); n++) {
+    std::string estcolor = ((int)est_state.at(0)(18) == 1) ? "blue" : colors.at(n);
+    std::string stdcolor = ((int)est_state.at(0)(18) == 1) ? "red" : colors.at(n);
+    plot_4errors(error_cam_k.at(n)[0], error_cam_k.at(n)[1], error_cam_k.at(n)[2], error_cam_k.at(n)[3], colors.at(n), stdcolor);
+  }
+
+  // Update the title and axis labels
+  matplotlibcpp::subplot(4, 1, 1);
+  matplotlibcpp::title("Intrinsics Projection Error");
+  matplotlibcpp::ylabel("fx (px)");
+  matplotlibcpp::subplot(4, 1, 2);
+  matplotlibcpp::ylabel("fy (px)");
+  matplotlibcpp::subplot(4, 1, 3);
+  matplotlibcpp::ylabel("cx (px)");
+  matplotlibcpp::subplot(4, 1, 4);
+  matplotlibcpp::ylabel("cy (px)");
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::show(false);
+  //=====================================================
+
+  //=====================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(800, 600);
+  for (int n = 0; n < (int)est_state.at(0)(18); n++) {
+    std::string estcolor = ((int)est_state.at(0)(18) == 1) ? "blue" : colors.at(n);
+    std::string stdcolor = ((int)est_state.at(0)(18) == 1) ? "red" : colors.at(n);
+    plot_4errors(error_cam_d.at(n)[0], error_cam_d.at(n)[1], error_cam_d.at(n)[2], error_cam_d.at(n)[3], estcolor, stdcolor);
+  }
+
+  // Update the title and axis labels
+  matplotlibcpp::subplot(4, 1, 1);
+  matplotlibcpp::title("Intrinsics Distortion Error");
+  matplotlibcpp::ylabel("d1");
+  matplotlibcpp::subplot(4, 1, 2);
+  matplotlibcpp::ylabel("d2");
+  matplotlibcpp::subplot(4, 1, 3);
+  matplotlibcpp::ylabel("d3");
+  matplotlibcpp::subplot(4, 1, 4);
+  matplotlibcpp::ylabel("d4");
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::show(false);
+  //=====================================================
+
+#endif
+}
+
+void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) {
+
+  // Check that we have cameras
+  if ((int)est_state.at(0)(18) < 1) {
+    PRINT_ERROR(YELLOW "You need at least one camera to plot intrinsics...\n" RESET);
+    return;
+  }
+
+  // Camera extrinsics statistic storage
+  std::vector<std::vector<Statistics>> error_cam_ori, error_cam_pos;
+  for (int i = 0; i < (int)est_state.at(0)(18); i++) {
+    std::vector<Statistics> temp1, temp2;
+    for (int j = 0; j < 3; j++) {
+      temp1.push_back(Statistics());
+      temp2.push_back(Statistics());
+    }
+    error_cam_ori.push_back(temp1);
+    error_cam_pos.push_back(temp2);
+  }
+
+  // Loop through and calculate error
+  double start_time = est_state.at(0)(0);
+  for (size_t i = 0; i < est_state.size(); i++) {
+
+    // Exit if we have reached our max time
+    if ((est_state.at(i)(0) - start_time) > max_time)
+      break;
+
+    // Assert our times are the same
+    assert(est_state.at(i)(0) == gt_state.at(i)(0));
+
+    // If we are not calibrating then don't plot it!
+    if (state_cov.at(i)(26) == 0.0) {
+      PRINT_WARNING(YELLOW "Camera extrinsics not calibrated online, so will not plot...\n" RESET);
+      return;
+    }
+
+    // Loop through each camera and calculate error
+    for (int n = 0; n < (int)est_state.at(0)(18); n++) {
+      // NOTE: we define our error as e_R = -Log(R*Rhat^T)
+      Eigen::Matrix3d e_R = ov_core::quat_2_Rot(gt_state.at(i).block(27 + 15 * n, 0, 4, 1)) *
+                            ov_core::quat_2_Rot(est_state.at(i).block(27 + 15 * n, 0, 4, 1)).transpose();
+      Eigen::Vector3d ori_err = -180.0 / M_PI * ov_core::log_so3(e_R);
+      // Eigen::Matrix3d e_R = Math::quat_2_Rot(est_state.at(i).block(27+15*n,0,4,1)).transpose() *
+      // Math::quat_2_Rot(gt_state.at(i).block(27+15*n,0,4,1)); Eigen::Vector3d ori_err = 180.0/M_PI*Math::log_so3(e_R);
+      for (int j = 0; j < 3; j++) {
+        error_cam_ori.at(n).at(j).timestamps.push_back(est_state.at(i)(0));
+        error_cam_ori.at(n).at(j).values.push_back(ori_err(j));
+        error_cam_ori.at(n).at(j).values_bound.push_back(3 * 180.0 / M_PI * state_cov.at(i)(26 + 14 * n + j));
+        error_cam_pos.at(n).at(j).timestamps.push_back(est_state.at(i)(0));
+        error_cam_pos.at(n).at(j).values.push_back(est_state.at(i)(27 + 4 + 15 * n + j) - gt_state.at(i)(27 + 4 + 15 * n + j));
+        error_cam_pos.at(n).at(j).values_bound.push_back(3 * state_cov.at(i)(26 + 3 + 14 * n + j));
+      }
+    }
+  }
+
+  // return if we don't want to plot
+  if (!doplotting)
+    return;
+
+#ifndef HAVE_PYTHONLIBS
+  PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET);
+  return;
+#else
+
+  // Plot line colors
+  std::vector<std::string> colors = {"blue", "red", "black", "green", "cyan", "magenta"};
+  assert(error_cam_ori.size() <= colors.size());
+
+  //=====================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(800, 500);
+  for (int n = 0; n < (int)est_state.at(0)(18); n++) {
+    std::string estcolor = ((int)est_state.at(0)(18) == 1) ? "blue" : colors.at(n);
+    std::string stdcolor = ((int)est_state.at(0)(18) == 1) ? "red" : colors.at(n);
+    plot_3errors(error_cam_ori.at(n)[0], error_cam_ori.at(n)[1], error_cam_ori.at(n)[2], colors.at(n), stdcolor);
+  }
+
+  // Update the title and axis labels
+  matplotlibcpp::subplot(3, 1, 1);
+  matplotlibcpp::title("Camera Calibration Orientation Error");
+  matplotlibcpp::ylabel("x-error (deg)");
+  matplotlibcpp::subplot(3, 1, 2);
+  matplotlibcpp::ylabel("y-error (deg)");
+  matplotlibcpp::subplot(3, 1, 3);
+  matplotlibcpp::ylabel("z-error (deg)");
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::show(false);
+  //=====================================================
+
+  //=====================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(800, 500);
+  for (int n = 0; n < (int)est_state.at(0)(18); n++) {
+    std::string estcolor = ((int)est_state.at(0)(18) == 1) ? "blue" : colors.at(n);
+    std::string stdcolor = ((int)est_state.at(0)(18) == 1) ? "red" : colors.at(n);
+    plot_3errors(error_cam_pos.at(n)[0], error_cam_pos.at(n)[1], error_cam_pos.at(n)[2], estcolor, stdcolor);
+  }
+
+  // Update the title and axis labels
+  matplotlibcpp::subplot(3, 1, 1);
+  matplotlibcpp::title("Camera Calibration Position Error");
+  matplotlibcpp::ylabel("x-error (m)");
+  matplotlibcpp::subplot(3, 1, 2);
+  matplotlibcpp::ylabel("y-error (m)");
+  matplotlibcpp::subplot(3, 1, 3);
+  matplotlibcpp::ylabel("z-error (m)");
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::show(false);
+  //=====================================================
+
+#endif
+}

ov_eval/src/calc/ResultSimulation.h

@@ -0,0 +1,275 @@
+/*
+ * 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/>.
+ */
+
+#ifndef OV_EVAL_SIMULATION_H
+#define OV_EVAL_SIMULATION_H
+
+#include <fstream>
+#include <random>
+#include <sstream>
+#include <string>
+#include <unordered_map>
+
+#include <Eigen/Eigen>
+
+#include "utils/Loader.h"
+#include "utils/Statistics.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"
+
+#endif
+
+namespace ov_eval {
+
+/**
+ * @brief A single simulation run (the full state not just pose).
+ *
+ * This should match the recording logic that is in the ov_msckf::RosVisualizer in which we write both estimate, their deviation, and
+ * groundtruth to three files. We enforce that these files first contain the current IMU state, then time offset, number of cameras, then
+ * the camera calibration states. If we are not performing calibration these should all be written to file, just their deviation should be
+ * zero as they are 100% certain.
+ */
+class ResultSimulation {
+
+public:
+  /**
+   * @brief Default constructor that will load our data from file
+   * @param path_est Path to the estimate text file
+   * @param path_std Path to the standard deviation file
+   * @param path_gt Path to the groundtruth text file
+   */
+  ResultSimulation(std::string path_est, std::string path_std, std::string path_gt);
+
+  /**
+   * @brief Will plot the state error and its three sigma bounds
+   * @param doplotting True if you want to display the plots
+   * @param max_time Max number of second we want to plot
+   */
+  void plot_state(bool doplotting, double max_time = INFINITY);
+
+  /**
+   * @brief Will plot the state imu camera offset and its sigma bound
+   * @param doplotting True if you want to display the plots
+   * @param max_time Max number of second we want to plot
+   */
+  void plot_timeoff(bool doplotting, double max_time = INFINITY);
+
+  /**
+   * @brief Will plot the camera calibration intrinsics
+   * @param doplotting True if you want to display the plots
+   * @param max_time Max number of second we want to plot
+   */
+  void plot_cam_instrinsics(bool doplotting, double max_time = INFINITY);
+
+  /**
+   * @brief Will plot the camera calibration extrinsic transform
+   * @param doplotting True if you want to display the plots
+   * @param max_time Max number of second we want to plot
+   */
+  void plot_cam_extrinsics(bool doplotting, double max_time = INFINITY);
+
+protected:
+  // Trajectory data (loaded from file and timestamp intersected)
+  std::vector<Eigen::VectorXd> est_state, gt_state;
+  std::vector<Eigen::VectorXd> state_cov;
+
+#ifdef HAVE_PYTHONLIBS
+
+  /**
+   * @brief Plots three different statistic values and sigma bounds
+   * @param sx X-axis error
+   * @param sy Y-axis error
+   * @param sz Z-axis error
+   * @param color_err MATLAB color string for error line (blue, red, etc.)
+   * @param color_std MATLAB color string for deviation (blue, red, etc.)
+   */
+  void plot_3errors(ov_eval::Statistics sx, ov_eval::Statistics sy, ov_eval::Statistics sz, std::string color_err, std::string color_std) {
+
+    // Zero our time arrays
+    double starttime1 = (sx.timestamps.empty()) ? 0 : sx.timestamps.at(0);
+    double endtime1 = (sx.timestamps.empty()) ? 0 : sx.timestamps.at(sx.timestamps.size() - 1);
+    for (size_t i = 0; i < sx.timestamps.size(); i++) {
+      sx.timestamps.at(i) -= starttime1;
+    }
+    double starttime2 = (sy.timestamps.empty()) ? 0 : sy.timestamps.at(0);
+    double endtime2 = (sy.timestamps.empty()) ? 0 : sy.timestamps.at(sy.timestamps.size() - 1);
+    for (size_t i = 0; i < sy.timestamps.size(); i++) {
+      sy.timestamps.at(i) -= starttime2;
+    }
+    double starttime3 = (sz.timestamps.empty()) ? 0 : sz.timestamps.at(0);
+    double endtime3 = (sz.timestamps.empty()) ? 0 : sz.timestamps.at(sz.timestamps.size() - 1);
+    for (size_t i = 0; i < sz.timestamps.size(); i++) {
+      sz.timestamps.at(i) -= starttime3;
+    }
+
+    // Parameters that define the line styles
+    std::map<std::string, std::string> params_value, params_bound;
+    // params_value.insert({"label","error"});
+    params_value.insert({"linestyle", "-"});
+    params_value.insert({"color", color_err});
+    // params_bound.insert({"label","3 sigma bound"});
+    params_bound.insert({"linestyle", "--"});
+    params_bound.insert({"color", color_std});
+
+    // Plot our error value
+    matplotlibcpp::subplot(3, 1, 1);
+    matplotlibcpp::plot(sx.timestamps, sx.values, params_value);
+    if (!sx.values_bound.empty()) {
+      matplotlibcpp::plot(sx.timestamps, sx.values_bound, params_bound);
+      for (size_t i = 0; i < sx.timestamps.size(); i++) {
+        sx.values_bound.at(i) *= -1;
+      }
+      matplotlibcpp::plot(sx.timestamps, sx.values_bound, params_bound);
+    }
+    matplotlibcpp::xlim(0.0, endtime1 - starttime1);
+
+    // Plot our error value
+    matplotlibcpp::subplot(3, 1, 2);
+    matplotlibcpp::plot(sy.timestamps, sy.values, params_value);
+    if (!sy.values_bound.empty()) {
+      matplotlibcpp::plot(sy.timestamps, sy.values_bound, params_bound);
+      for (size_t i = 0; i < sy.timestamps.size(); i++) {
+        sy.values_bound.at(i) *= -1;
+      }
+      matplotlibcpp::plot(sy.timestamps, sy.values_bound, params_bound);
+    }
+    matplotlibcpp::xlim(0.0, endtime2 - starttime2);
+
+    // Plot our error value
+    matplotlibcpp::subplot(3, 1, 3);
+    matplotlibcpp::plot(sz.timestamps, sz.values, params_value);
+    if (!sz.values_bound.empty()) {
+      matplotlibcpp::plot(sz.timestamps, sz.values_bound, params_bound);
+      for (size_t i = 0; i < sz.timestamps.size(); i++) {
+        sz.values_bound.at(i) *= -1;
+      }
+      matplotlibcpp::plot(sz.timestamps, sz.values_bound, params_bound);
+    }
+    matplotlibcpp::xlim(0.0, endtime3 - starttime3);
+  }
+
+  /**
+   * @brief Plots four different statistic values and sigma bounds
+   * @param sx Error one
+   * @param sy Error two
+   * @param sz Error three
+   * @param sk Error four
+   * @param color_err MATLAB color string for error line (blue, red, etc.)
+   * @param color_std MATLAB color string for deviation (blue, red, etc.)
+   */
+  void plot_4errors(ov_eval::Statistics sx, ov_eval::Statistics sy, ov_eval::Statistics sz, ov_eval::Statistics sk, std::string color_err,
+                    std::string color_std) {
+
+    // Zero our time arrays
+    double starttime1 = (sx.timestamps.empty()) ? 0 : sx.timestamps.at(0);
+    double endtime1 = (sx.timestamps.empty()) ? 0 : sx.timestamps.at(sx.timestamps.size() - 1);
+    for (size_t i = 0; i < sx.timestamps.size(); i++) {
+      sx.timestamps.at(i) -= starttime1;
+    }
+    double starttime2 = (sy.timestamps.empty()) ? 0 : sy.timestamps.at(0);
+    double endtime2 = (sy.timestamps.empty()) ? 0 : sy.timestamps.at(sy.timestamps.size() - 1);
+    for (size_t i = 0; i < sy.timestamps.size(); i++) {
+      sy.timestamps.at(i) -= starttime2;
+    }
+    double starttime3 = (sz.timestamps.empty()) ? 0 : sz.timestamps.at(0);
+    double endtime3 = (sz.timestamps.empty()) ? 0 : sz.timestamps.at(sz.timestamps.size() - 1);
+    for (size_t i = 0; i < sz.timestamps.size(); i++) {
+      sz.timestamps.at(i) -= starttime3;
+    }
+    double starttime4 = (sk.timestamps.empty()) ? 0 : sk.timestamps.at(0);
+    double endtime4 = (sk.timestamps.empty()) ? 0 : sk.timestamps.at(sk.timestamps.size() - 1);
+    for (size_t i = 0; i < sk.timestamps.size(); i++) {
+      sk.timestamps.at(i) -= starttime4;
+    }
+
+    // Parameters that define the line styles
+    std::map<std::string, std::string> params_value, params_bound;
+    // params_value.insert({"label","error"});
+    params_value.insert({"linestyle", "-"});
+    params_value.insert({"color", color_err});
+    // params_bound.insert({"label","3 sigma bound"});
+    params_bound.insert({"linestyle", "--"});
+    params_bound.insert({"color", color_std});
+
+    // Plot our error value
+    matplotlibcpp::subplot(4, 1, 1);
+    matplotlibcpp::plot(sx.timestamps, sx.values, params_value);
+    if (!sx.values_bound.empty()) {
+      matplotlibcpp::plot(sx.timestamps, sx.values_bound, params_bound);
+      for (size_t i = 0; i < sx.timestamps.size(); i++) {
+        sx.values_bound.at(i) *= -1;
+      }
+      matplotlibcpp::plot(sx.timestamps, sx.values_bound, params_bound);
+    }
+    matplotlibcpp::xlim(0.0, endtime1 - starttime1);
+
+    // Plot our error value
+    matplotlibcpp::subplot(4, 1, 2);
+    matplotlibcpp::plot(sy.timestamps, sy.values, params_value);
+    if (!sy.values_bound.empty()) {
+      matplotlibcpp::plot(sy.timestamps, sy.values_bound, params_bound);
+      for (size_t i = 0; i < sy.timestamps.size(); i++) {
+        sy.values_bound.at(i) *= -1;
+      }
+      matplotlibcpp::plot(sy.timestamps, sy.values_bound, params_bound);
+    }
+    matplotlibcpp::xlim(0.0, endtime2 - starttime2);
+
+    // Plot our error value
+    matplotlibcpp::subplot(4, 1, 3);
+    matplotlibcpp::plot(sz.timestamps, sz.values, params_value);
+    if (!sz.values_bound.empty()) {
+      matplotlibcpp::plot(sz.timestamps, sz.values_bound, params_bound);
+      for (size_t i = 0; i < sz.timestamps.size(); i++) {
+        sz.values_bound.at(i) *= -1;
+      }
+      matplotlibcpp::plot(sz.timestamps, sz.values_bound, params_bound);
+    }
+    matplotlibcpp::xlim(0.0, endtime3 - starttime3);
+
+    // Plot our error value
+    matplotlibcpp::subplot(4, 1, 4);
+    matplotlibcpp::plot(sk.timestamps, sk.values, params_value);
+    if (!sk.values_bound.empty()) {
+      matplotlibcpp::plot(sk.timestamps, sk.values_bound, params_bound);
+      for (size_t i = 0; i < sk.timestamps.size(); i++) {
+        sk.values_bound.at(i) *= -1;
+      }
+      matplotlibcpp::plot(sk.timestamps, sk.values_bound, params_bound);
+    }
+    matplotlibcpp::xlim(0.0, endtime4 - starttime4);
+  }
+
+#endif
+};
+
+} // namespace ov_eval
+
+#endif // OV_EVAL_SIMULATION_H

ov_eval/src/calc/ResultTrajectory.cpp

@@ -0,0 +1,393 @@
+/*
+ * 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 "ResultTrajectory.h"
+
+using namespace ov_eval;
+
+ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, std::string alignment_method) {
+
+  // Load from file
+  Loader::load_data(path_est, est_times, est_poses, est_covori, est_covpos);
+  Loader::load_data(path_gt, gt_times, gt_poses, gt_covori, gt_covpos);
+
+  // Debug print amount
+  // std::string base_filename1 = path_est.substr(path_est.find_last_of("/\\") + 1);
+  // std::string base_filename2 = path_gt.substr(path_gt.find_last_of("/\\") + 1);
+  // PRINT_DEBUG("[TRAJ]: loaded %d poses from %s\n",(int)est_times.size(),base_filename1.c_str());
+  // PRINT_DEBUG("[TRAJ]: loaded %d poses from %s\n",(int)gt_times.size(),base_filename2.c_str());
+
+  // Intersect timestamps
+  AlignUtils::perform_association(0, 0.02, est_times, gt_times, est_poses, gt_poses, est_covori, est_covpos, gt_covori, gt_covpos);
+
+  // Return failure if we didn't have any common timestamps
+  if (est_poses.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, R_GTtoEST;
+  Eigen::Vector3d t_ESTinGT, t_GTinEST;
+  double s_ESTtoGT, s_GTtoEST;
+  AlignTrajectory::align_trajectory(est_poses, gt_poses, R_ESTtoGT, t_ESTinGT, s_ESTtoGT, alignment_method);
+  AlignTrajectory::align_trajectory(gt_poses, est_poses, R_GTtoEST, t_GTinEST, s_GTtoEST, alignment_method);
+
+  // Debug print to the user
+  Eigen::Vector4d q_ESTtoGT = ov_core::rot_2_quat(R_ESTtoGT);
+  Eigen::Vector4d q_GTtoEST = ov_core::rot_2_quat(R_GTtoEST);
+  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);
+  // PRINT_DEBUG("[TRAJ]: q_GTtoEST = %.3f, %.3f, %.3f, %.3f | p_GTinEST = %.3f, %.3f, %.3f | s =
+  // %.2f\n",q_GTtoEST(0),q_GTtoEST(1),q_GTtoEST(2),q_GTtoEST(3),t_GTinEST(0),t_GTinEST(1),t_GTinEST(2),s_GTtoEST);
+
+  // Finally lets calculate the aligned trajectories
+  for (size_t i = 0; i < gt_times.size(); i++) {
+    Eigen::Matrix<double, 7, 1> pose_ESTinGT, pose_GTinEST;
+    pose_ESTinGT.block(0, 0, 3, 1) = s_ESTtoGT * R_ESTtoGT * est_poses.at(i).block(0, 0, 3, 1) + t_ESTinGT;
+    pose_ESTinGT.block(3, 0, 4, 1) = ov_core::quat_multiply(est_poses.at(i).block(3, 0, 4, 1), ov_core::Inv(q_ESTtoGT));
+    pose_GTinEST.block(0, 0, 3, 1) = s_GTtoEST * R_GTtoEST * gt_poses.at(i).block(0, 0, 3, 1) + t_GTinEST;
+    pose_GTinEST.block(3, 0, 4, 1) = ov_core::quat_multiply(gt_poses.at(i).block(3, 0, 4, 1), ov_core::Inv(q_GTtoEST));
+    est_poses_aignedtoGT.push_back(pose_ESTinGT);
+    gt_poses_aignedtoEST.push_back(pose_GTinEST);
+  }
+}
+
+void ResultTrajectory::calculate_ate(Statistics &error_ori, Statistics &error_pos) {
+
+  // Clear any old data
+  error_ori.clear();
+  error_pos.clear();
+
+  // Calculate the position and orientation error at every timestep
+  for (size_t i = 0; i < est_poses_aignedtoGT.size(); i++) {
+
+    // Calculate orientation error
+    Eigen::Matrix3d e_R = ov_core::quat_2_Rot(est_poses_aignedtoGT.at(i).block(3, 0, 4, 1)).transpose() *
+                          ov_core::quat_2_Rot(gt_poses.at(i).block(3, 0, 4, 1));
+    double ori_err = 180.0 / M_PI * ov_core::log_so3(e_R).norm();
+
+    // Calculate position error
+    double pos_err = (gt_poses.at(i).block(0, 0, 3, 1) - est_poses_aignedtoGT.at(i).block(0, 0, 3, 1)).norm();
+
+    // Append this error!
+    error_ori.timestamps.push_back(est_times.at(i));
+    error_ori.values.push_back(ori_err);
+    error_pos.timestamps.push_back(est_times.at(i));
+    error_pos.values.push_back(pos_err);
+  }
+
+  // Update stat information
+  error_ori.calculate();
+  error_pos.calculate();
+}
+
+void ResultTrajectory::calculate_ate_2d(Statistics &error_ori, Statistics &error_pos) {
+
+  // Clear any old data
+  error_ori.clear();
+  error_pos.clear();
+
+  // Calculate the position and orientation error at every timestep
+  for (size_t i = 0; i < est_poses_aignedtoGT.size(); i++) {
+
+    // Calculate orientation error
+    Eigen::Matrix3d e_R = ov_core::quat_2_Rot(est_poses_aignedtoGT.at(i).block(3, 0, 4, 1)).transpose() *
+                          ov_core::quat_2_Rot(gt_poses.at(i).block(3, 0, 4, 1));
+    double ori_err = 180.0 / M_PI * ov_core::log_so3(e_R)(2);
+
+    // Calculate position error
+    double pos_err = (gt_poses.at(i).block(0, 0, 2, 1) - est_poses_aignedtoGT.at(i).block(0, 0, 2, 1)).norm();
+
+    // Append this error!
+    error_ori.timestamps.push_back(est_times.at(i));
+    error_ori.values.push_back(ori_err);
+    error_pos.timestamps.push_back(est_times.at(i));
+    error_pos.values.push_back(pos_err);
+  }
+
+  // Update stat information
+  error_ori.calculate();
+  error_pos.calculate();
+}
+
+void ResultTrajectory::calculate_rpe(const std::vector<double> &segment_lengths,
+                                     std::map<double, std::pair<Statistics, Statistics>> &error_rpe) {
+
+  // Distance at each point along the trajectory
+  double average_pos_difference = 0;
+  std::vector<double> accum_distances(gt_poses.size());
+  accum_distances[0] = 0;
+  for (size_t i = 1; i < gt_poses.size(); i++) {
+    double pos_diff = (gt_poses[i].block(0, 0, 3, 1) - gt_poses[i - 1].block(0, 0, 3, 1)).norm();
+    accum_distances[i] = accum_distances[i - 1] + pos_diff;
+    average_pos_difference += pos_diff;
+  }
+  average_pos_difference /= gt_poses.size();
+
+  // Warn if large pos difference
+  double max_dist_diff = 0.5;
+  if (average_pos_difference > max_dist_diff) {
+    PRINT_WARNING(YELLOW "[COMP]: average groundtruth position difference %.2f > %.2f is too large\n" RESET, average_pos_difference,
+                  max_dist_diff);
+    PRINT_WARNING(YELLOW "[COMP]: this will prevent the RPE from finding valid trajectory segments!!!\n" RESET);
+    PRINT_WARNING(YELLOW
+                  "[COMP]: the recommendation is to use a higher frequency groundtruth, or relax this trajectory segment logic...\n" RESET);
+  }
+
+  // Loop through each segment length
+  for (const double &distance : segment_lengths) {
+
+    // Our stats for this length
+    Statistics error_ori, error_pos;
+
+    // Get end of subtrajectories for each possible starting point
+    // NOTE: is there a better way to select which end pos is a valid segments that are of the correct lenght?
+    // NOTE: right now this allows for longer segments to have bigger error in their start-end distance vs the desired segment length
+    // std::vector<int> comparisons = compute_comparison_indices_length(accum_distances, distance, 0.1*distance);
+    std::vector<int> comparisons = compute_comparison_indices_length(accum_distances, distance, max_dist_diff);
+    assert(comparisons.size() == gt_poses.size());
+
+    // Loop through each relative comparison
+    for (size_t id_start = 0; id_start < comparisons.size(); id_start++) {
+
+      // Get the end id (skip if we couldn't find an end)
+      int id_end = comparisons[id_start];
+      if (id_end == -1)
+        continue;
+
+      //===============================================================================
+      // Get T I1 to world EST at beginning of subtrajectory (at state idx)
+      Eigen::Matrix4d T_c1 = Eigen::Matrix4d::Identity();
+      T_c1.block(0, 0, 3, 3) = ov_core::quat_2_Rot(est_poses_aignedtoGT.at(id_start).block(3, 0, 4, 1)).transpose();
+      T_c1.block(0, 3, 3, 1) = est_poses_aignedtoGT.at(id_start).block(0, 0, 3, 1);
+
+      // Get T I2 to world EST at end of subtrajectory starting (at state comparisons[idx])
+      Eigen::Matrix4d T_c2 = Eigen::Matrix4d::Identity();
+      T_c2.block(0, 0, 3, 3) = ov_core::quat_2_Rot(est_poses_aignedtoGT.at(id_end).block(3, 0, 4, 1)).transpose();
+      T_c2.block(0, 3, 3, 1) = est_poses_aignedtoGT.at(id_end).block(0, 0, 3, 1);
+
+      // Get T I2 to I1 EST
+      Eigen::Matrix4d T_c1_c2 = ov_core::Inv_se3(T_c1) * T_c2;
+
+      //===============================================================================
+      // Get T I1 to world GT at beginning of subtrajectory (at state idx)
+      Eigen::Matrix4d T_m1 = Eigen::Matrix4d::Identity();
+      T_m1.block(0, 0, 3, 3) = ov_core::quat_2_Rot(gt_poses.at(id_start).block(3, 0, 4, 1)).transpose();
+      T_m1.block(0, 3, 3, 1) = gt_poses.at(id_start).block(0, 0, 3, 1);
+
+      // Get T I2 to world GT at end of subtrajectory starting (at state comparisons[idx])
+      Eigen::Matrix4d T_m2 = Eigen::Matrix4d::Identity();
+      T_m2.block(0, 0, 3, 3) = ov_core::quat_2_Rot(gt_poses.at(id_end).block(3, 0, 4, 1)).transpose();
+      T_m2.block(0, 3, 3, 1) = gt_poses.at(id_end).block(0, 0, 3, 1);
+
+      // Get T I2 to I1 GT
+      Eigen::Matrix4d T_m1_m2 = ov_core::Inv_se3(T_m1) * T_m2;
+
+      //===============================================================================
+      // Compute error transform between EST and GT start-end transform
+      Eigen::Matrix4d T_error_in_c2 = ov_core::Inv_se3(T_m1_m2) * T_c1_c2;
+
+      Eigen::Matrix4d T_c2_rot = Eigen::Matrix4d::Identity();
+      T_c2_rot.block(0, 0, 3, 3) = T_c2.block(0, 0, 3, 3);
+
+      Eigen::Matrix4d T_c2_rot_inv = Eigen::Matrix4d::Identity();
+      T_c2_rot_inv.block(0, 0, 3, 3) = T_c2.block(0, 0, 3, 3).transpose();
+
+      // Rotate rotation so that rotation error is in the global frame (allows us to look at yaw error)
+      Eigen::Matrix4d T_error_in_w = T_c2_rot * T_error_in_c2 * T_c2_rot_inv;
+
+      //===============================================================================
+      // Compute error for position
+      error_pos.timestamps.push_back(est_times.at(id_start));
+      error_pos.values.push_back(T_error_in_w.block(0, 3, 3, 1).norm());
+
+      // Calculate orientation error
+      double ori_err = 180.0 / M_PI * ov_core::log_so3(T_error_in_w.block(0, 0, 3, 3)).norm();
+      error_ori.timestamps.push_back(est_times.at(id_start));
+      error_ori.values.push_back(ori_err);
+    }
+
+    // Update stat information
+    error_ori.calculate();
+    error_pos.calculate();
+    error_rpe.insert({distance, {error_ori, error_pos}});
+  }
+}
+
+void ResultTrajectory::calculate_nees(Statistics &nees_ori, Statistics &nees_pos) {
+
+  // Check that we have our covariance matrices to normalize with
+  if (est_times.size() != est_covori.size() || est_times.size() != est_covpos.size() || gt_times.size() != gt_covori.size() ||
+      gt_times.size() != gt_covpos.size()) {
+    PRINT_WARNING(YELLOW "[TRAJ]: Normalized Estimation Error Squared called but trajectory does not have any covariances...\n" RESET);
+    PRINT_WARNING(YELLOW "[TRAJ]: Did you record using a Odometry or PoseWithCovarianceStamped????\n" RESET);
+    return;
+  }
+
+  // Clear any old data
+  nees_ori.clear();
+  nees_pos.clear();
+
+  // Calculate the position and orientation error at every timestep
+  for (size_t i = 0; i < est_poses.size(); i++) {
+
+    // Calculate orientation error
+    // NOTE: we define our error as e_R = -Log(R*Rhat^T)
+    Eigen::Matrix3d e_R =
+        ov_core::quat_2_Rot(gt_poses.at(i).block(3, 0, 4, 1)) * ov_core::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose();
+    Eigen::Vector3d errori = -ov_core::log_so3(e_R);
+    // Eigen::Vector4d e_q = Math::quat_multiply(gt_poses_aignedtoEST.at(i).block(3,0,4,1),Math::Inv(est_poses.at(i).block(3,0,4,1)));
+    // Eigen::Vector3d errori = 2*e_q.block(0,0,3,1);
+    double ori_nees = errori.transpose() * est_covori.at(i).inverse() * errori;
+
+    // Calculate nees position error
+    Eigen::Vector3d errpos = gt_poses_aignedtoEST.at(i).block(0, 0, 3, 1) - est_poses.at(i).block(0, 0, 3, 1);
+    double pos_nees = errpos.transpose() * est_covpos.at(i).inverse() * errpos;
+
+    // Skip if nan error value
+    if (std::isnan(ori_nees) || std::isnan(pos_nees)) {
+      PRINT_WARNING(YELLOW "[TRAJ]: nees calculation had nan number (covariance is wrong?) skipping...\n" RESET);
+      continue;
+    }
+
+    // Append this error!
+    nees_ori.timestamps.push_back(est_times.at(i));
+    nees_ori.values.push_back(ori_nees);
+    nees_pos.timestamps.push_back(est_times.at(i));
+    nees_pos.values.push_back(pos_nees);
+  }
+
+  // Update stat information
+  nees_ori.calculate();
+  nees_pos.calculate();
+}
+
+void ResultTrajectory::calculate_error(Statistics &posx, Statistics &posy, Statistics &posz, Statistics &orix, Statistics &oriy,
+                                       Statistics &oriz, Statistics &roll, Statistics &pitch, Statistics &yaw) {
+
+  // Clear any old data
+  posx.clear();
+  posy.clear();
+  posz.clear();
+  orix.clear();
+  oriy.clear();
+  oriz.clear();
+  roll.clear();
+  pitch.clear();
+  yaw.clear();
+
+  // Calculate the position and orientation error at every timestep
+  for (size_t i = 0; i < est_poses.size(); i++) {
+
+    // Calculate local orientation error, then propagate its covariance into the global frame of reference
+    Eigen::Vector4d e_q =
+        ov_core::quat_multiply(gt_poses_aignedtoEST.at(i).block(3, 0, 4, 1), ov_core::Inv(est_poses.at(i).block(3, 0, 4, 1)));
+    Eigen::Vector3d errori_local = 2 * e_q.block(0, 0, 3, 1);
+    Eigen::Vector3d errori_global = ov_core::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose() * errori_local;
+    Eigen::Matrix3d cov_global;
+    if (est_times.size() == est_covori.size()) {
+      cov_global = ov_core::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose() * est_covori.at(i) *
+                   ov_core::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1));
+    }
+
+    // Convert to roll pitch yaw (also need to "wrap" the error to -pi to pi)
+    // NOTE: our rot2rpy is in the form R_input = R_z(yaw)*R_y(pitch)*R_x(roll)
+    // NOTE: this error is in the "global" frame since we do rot2rpy on R_ItoG rotation
+    Eigen::Vector3d ypr_est_ItoG = ov_core::rot2rpy(ov_core::quat_2_Rot(est_poses.at(i).block(3, 0, 4, 1)).transpose());
+    Eigen::Vector3d ypr_gt_ItoG = ov_core::rot2rpy(ov_core::quat_2_Rot(gt_poses_aignedtoEST.at(i).block(3, 0, 4, 1)).transpose());
+    Eigen::Vector3d errori_rpy = ypr_gt_ItoG - ypr_est_ItoG;
+    for (size_t idx = 0; idx < 3; idx++) {
+      while (errori_rpy(idx) < -M_PI) {
+        errori_rpy(idx) += 2 * M_PI;
+      }
+      while (errori_rpy(idx) > M_PI) {
+        errori_rpy(idx) -= 2 * M_PI;
+      }
+    }
+
+    // Next need to propagate our covariance to the RPY frame of reference
+    // NOTE: one can derive this by perturbing the rpy error equation
+    // NOTE: R*Exp(theta_erro) = Rz*Rz(error)*Ry*Ry(error)*Rx*Rx(error)
+    // NOTE: example can be found here http://mars.cs.umn.edu/tr/reports/Trawny05c.pdf
+    Eigen::Matrix<double, 3, 3> H_xyz2rpy = Eigen::Matrix<double, 3, 3>::Identity();
+    H_xyz2rpy.block(0, 1, 3, 1) = ov_core::rot_x(-ypr_est_ItoG(0)) * H_xyz2rpy.block(0, 1, 3, 1);
+    H_xyz2rpy.block(0, 2, 3, 1) = ov_core::rot_x(-ypr_est_ItoG(0)) * ov_core::rot_y(-ypr_est_ItoG(1)) * H_xyz2rpy.block(0, 2, 3, 1);
+    Eigen::Matrix3d cov_rpy;
+    if (est_times.size() == est_covori.size()) {
+      cov_rpy = H_xyz2rpy.inverse() * est_covori.at(i) * H_xyz2rpy.inverse().transpose();
+    }
+
+    // Calculate position error
+    Eigen::Vector3d errpos = gt_poses_aignedtoEST.at(i).block(0, 0, 3, 1) - est_poses.at(i).block(0, 0, 3, 1);
+
+    // POSITION: Append this error!
+    posx.timestamps.push_back(est_times.at(i));
+    posx.values.push_back(errpos(0));
+    posy.timestamps.push_back(est_times.at(i));
+    posy.values.push_back(errpos(1));
+    posz.timestamps.push_back(est_times.at(i));
+    posz.values.push_back(errpos(2));
+    if (est_times.size() == est_covpos.size()) {
+      posx.values_bound.push_back(3 * std::sqrt(est_covpos.at(i)(0, 0)));
+      posy.values_bound.push_back(3 * std::sqrt(est_covpos.at(i)(1, 1)));
+      posz.values_bound.push_back(3 * std::sqrt(est_covpos.at(i)(2, 2)));
+    }
+
+    // ORIENTATION: Append this error!
+    orix.timestamps.push_back(est_times.at(i));
+    orix.values.push_back(180.0 / M_PI * errori_global(0));
+    oriy.timestamps.push_back(est_times.at(i));
+    oriy.values.push_back(180.0 / M_PI * errori_global(1));
+    oriz.timestamps.push_back(est_times.at(i));
+    oriz.values.push_back(180.0 / M_PI * errori_global(2));
+    if (est_times.size() == est_covori.size()) {
+      orix.values_bound.push_back(3 * 180.0 / M_PI * std::sqrt(cov_global(0, 0)));
+      oriy.values_bound.push_back(3 * 180.0 / M_PI * std::sqrt(cov_global(1, 1)));
+      oriz.values_bound.push_back(3 * 180.0 / M_PI * std::sqrt(cov_global(2, 2)));
+    }
+
+    // RPY: Append this error!
+    roll.timestamps.push_back(est_times.at(i));
+    roll.values.push_back(180.0 / M_PI * errori_rpy(0));
+    pitch.timestamps.push_back(est_times.at(i));
+    pitch.values.push_back(180.0 / M_PI * errori_rpy(1));
+    yaw.timestamps.push_back(est_times.at(i));
+    yaw.values.push_back(180.0 / M_PI * errori_rpy(2));
+    if (est_times.size() == est_covori.size()) {
+      roll.values_bound.push_back(3 * 180.0 / M_PI * std::sqrt(cov_rpy(0, 0)));
+      pitch.values_bound.push_back(3 * 180.0 / M_PI * std::sqrt(cov_rpy(1, 1)));
+      yaw.values_bound.push_back(3 * 180.0 / M_PI * std::sqrt(cov_rpy(2, 2)));
+    }
+  }
+
+  // Update stat information
+  posx.calculate();
+  posy.calculate();
+  posz.calculate();
+  orix.calculate();
+  oriy.calculate();
+  oriz.calculate();
+  roll.calculate();
+  pitch.calculate();
+  yaw.calculate();
+}

ov_eval/src/calc/ResultTrajectory.h

@@ -0,0 +1,203 @@
+/*
+ * 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/>.
+ */
+
+#ifndef OV_EVAL_TRAJECTORY_H
+#define OV_EVAL_TRAJECTORY_H
+
+#include <fstream>
+#include <map>
+#include <random>
+#include <sstream>
+#include <string>
+#include <unordered_map>
+
+#include <Eigen/Eigen>
+#include <Eigen/StdVector>
+
+#include "alignment/AlignTrajectory.h"
+#include "utils/Loader.h"
+#include "utils/Statistics.h"
+
+#include "utils/colors.h"
+#include "utils/print.h"
+#include "utils/quat_ops.h"
+
+namespace ov_eval {
+
+/**
+ * @brief A single run for a given dataset.
+ *
+ * This class has all the error function which can be calculated for the loaded trajectory.
+ * Given a groundtruth and trajectory we first align the two so that they are in the same frame.
+ * From there the following errors could be computed:
+ * - Absolute trajectory error
+ * - Relative pose Error
+ * - Normalized estimation error squared
+ * - Error and bound at each timestep
+ *
+ * Please see the @ref evaluation page for details and Zhang and Scaramuzza [A Tutorial on Quantitative Trajectory Evaluation for
+ * Visual(-Inertial) Odometry](http://rpg.ifi.uzh.ch/docs/IROS18_Zhang.pdf) paper for implementation specific details.
+ */
+class ResultTrajectory {
+
+public:
+  /**
+   * @brief Default constructor that will load, intersect, and align our trajectories.
+   * @param path_est Path to the estimate text file
+   * @param path_gt Path to the groundtruth text file
+   * @param alignment_method The alignment method to use [sim3, se3, posyaw, none]
+   */
+  ResultTrajectory(std::string path_est, std::string path_gt, std::string alignment_method);
+
+  /**
+   * @brief Computes the Absolute Trajectory Error (ATE) for this trajectory.
+   *
+   * This will first do our alignment of the two trajectories.
+   * Then at each point the error will be calculated and normed as follows:
+   * \f{align*}{
+   * e_{ATE} &= \sqrt{ \frac{1}{K} \sum_{k=1}^{K} ||\mathbf{x}_{k,i} \boxminus \hat{\mathbf{x}}^+_{k,i}||^2_{2} }
+   * \f}
+   *
+   * @param error_ori Error values for the orientation
+   * @param error_pos Error values for the position
+   */
+  void calculate_ate(Statistics &error_ori, Statistics &error_pos);
+
+  /**
+   * @brief Computes the Absolute Trajectory Error (ATE) for this trajectory in the 2d x-y plane.
+   *
+   * This will first do our alignment of the two trajectories.
+   * We just grab the yaw component of the orientation and the xy plane error.
+   * Then at each point the error will be calculated and normed as follows:
+   * \f{align*}{
+   * e_{ATE} &= \sqrt{ \frac{1}{K} \sum_{k=1}^{K} ||\mathbf{x}_{k,i} \boxminus \hat{\mathbf{x}}^+_{k,i}||^2_{2} }
+   * \f}
+   *
+   * @param error_ori Error values for the orientation (yaw error)
+   * @param error_pos Error values for the position (xy error)
+   */
+  void calculate_ate_2d(Statistics &error_ori, Statistics &error_pos);
+
+  /**
+   * @brief Computes the Relative Pose Error (RPE) for this trajectory
+   *
+   * For the given set of segment lengths, this will split the trajectory into segments.
+   * From there it will compute the relative pose error for all segments.
+   * These are then returned as a map for each segment length.
+   * \f{align*}{
+   * \tilde{\mathbf{x}}_{r} &= \mathbf{x}_{k} \boxminus \mathbf{x}_{k+d_i} \\
+   * e_{rpe,d_i} &= \frac{1}{D_i} \sum_{k=1}^{D_i} ||\tilde{\mathbf{x}}_{r} \boxminus \hat{\tilde{\mathbf{x}}}_{r}||^2_{2}
+   * \f}
+   *
+   * @param segment_lengths What segment lengths we want to calculate for
+   * @param error_rpe Map of segment lengths => errors for that length (orientation and position)
+   */
+  void calculate_rpe(const std::vector<double> &segment_lengths, std::map<double, std::pair<Statistics, Statistics>> &error_rpe);
+
+  /**
+   * @brief Computes the Normalized Estimation Error Squared (NEES) for this trajectory.
+   *
+   * If we have a covariance in addition to our pose estimate we can compute the NEES values.
+   * At each timestep we compute this for both orientation and position.
+   * \f{align*}{
+   * e_{nees,k} &= \frac{1}{N} \sum_{i=1}^{N} (\mathbf{x}_{k,i} \boxminus \hat{\mathbf{x}}_{k,i})^\top \mathbf{P}^{-1}_{k,i}
+   * (\mathbf{x}_{k,i} \boxminus \hat{\mathbf{x}}_{k,i}) \f}
+   *
+   * @param nees_ori NEES values for the orientation
+   * @param nees_pos NEES values for the position
+   */
+  void calculate_nees(Statistics &nees_ori, Statistics &nees_pos);
+
+  /**
+   * @brief Computes the error at each timestamp for this trajectory.
+   *
+   * As compared to ATE error (see @ref calculate_ate()) this will compute the error for each individual pose component.
+   * This is normally used if you just want to look at a single run on a single dataset.
+   * \f{align*}{
+   * e_{rmse,k} &= \sqrt{ \frac{1}{N} \sum_{i=1}^{N} ||\mathbf{x}_{k,i} \boxminus \hat{\mathbf{x}}_{k,i}||^2_{2} }
+   * \f}
+   *
+   * @param posx Position x-axis error and bound if we have it in our file
+   * @param posy Position y-axis error and bound if we have it in our file
+   * @param posz Position z-axis error and bound if we have it in our file
+   * @param orix Orientation x-axis error and bound if we have it in our file
+   * @param oriy Orientation y-axis error and bound if we have it in our file
+   * @param oriz Orientation z-axis error and bound if we have it in our file
+   * @param roll Orientation roll error and bound if we have it in our file
+   * @param pitch Orientation pitch error and bound if we have it in our file
+   * @param yaw Orientation yaw error and bound if we have it in our file
+   */
+  void calculate_error(Statistics &posx, Statistics &posy, Statistics &posz, Statistics &orix, Statistics &oriy, Statistics &oriz,
+                       Statistics &roll, Statistics &pitch, Statistics &yaw);
+
+protected:
+  // Trajectory data (loaded from file and timestamp intersected)
+  std::vector<double> est_times, gt_times;
+  std::vector<Eigen::Matrix<double, 7, 1>> est_poses, gt_poses;
+  std::vector<Eigen::Matrix3d> est_covori, est_covpos, gt_covori, gt_covpos;
+
+  // Aligned trajectories
+  std::vector<Eigen::Matrix<double, 7, 1>> est_poses_aignedtoGT;
+  std::vector<Eigen::Matrix<double, 7, 1>> gt_poses_aignedtoEST;
+
+  /**
+   * @brief Gets the indices at the end of subtractories of a given length when starting at each index.
+   * For each starting pose, find the end pose index which is the desired distance away.
+   * @param distances Total distance travelled from start at each index
+   * @param distance Distance of subtrajectory
+   * @param max_dist_diff Maximum error between current trajectory length and the desired
+   * @return End indices for each subtrajectory
+   */
+  std::vector<int> compute_comparison_indices_length(std::vector<double> &distances, double distance, double max_dist_diff) {
+
+    // Vector of end ids for our pose indexes
+    std::vector<int> comparisons;
+
+    // Loop through each pose in our trajectory (i.e. our distance vector generated from the trajectory).
+    for (size_t idx = 0; idx < distances.size(); idx++) {
+
+      // Loop through and find the pose that minimized the difference between
+      // The desired trajectory distance and our current trajectory distance
+      double distance_startpose = distances.at(idx);
+      int best_idx = -1;
+      double best_error = max_dist_diff;
+      for (size_t i = idx; i < distances.size(); i++) {
+        if (std::abs(distances.at(i) - (distance_startpose + distance)) < best_error) {
+          best_idx = i;
+          best_error = std::abs(distances.at(i) - (distance_startpose + distance));
+        }
+      }
+
+      // If we have an end id that reached this trajectory distance then add it!
+      // Else this isn't a valid segment, thus we shouldn't add it (we will try again at the next pose)
+      // NOTE: just because we searched through all poses and didn't find a close one doesn't mean we have ended
+      // NOTE: this could happen if there is a gap in the groundtruth poses and we just couldn't find a pose with low error
+      comparisons.push_back(best_idx);
+    }
+
+    // Finally return the ids for each starting pose that have this distance
+    return comparisons;
+  }
+};
+
+} // namespace ov_eval
+
+#endif // OV_EVAL_TRAJECTORY_H

ov_eval/src/dummy.cpp

@@ -0,0 +1,51 @@
+/*
+ * 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/>.
+ */
+
+/**
+ * @namespace ov_eval
+ * @brief Evaluation and recording utilities
+ *
+ * This project contains the key evaluation and research scripts for localization methods.
+ * These come from the necessity of trying to quantify the accuracy of the estimated trajectory while
+ * also allowing for the comparision to other methods.
+ * Please see the following documentation pages for details:
+ *
+ * - @ref eval-metrics --- Definitions of different metrics for estimator accuracy.
+ * - @ref eval-error --- Error evaluation methods for evaluating system performance.
+ * - @ref eval-timing --- Timing of estimator components and complexity.
+ *
+ * The key methods that we have included are as follows:
+ *
+ * - Absolute trajectory error
+ * - Relative pose error (for varying segment lengths)
+ * - Pose to text file recorder
+ * - Timing of system components
+ *
+ * The absolute and relative error scripts have been implemented in C++ to allow for fast computation on multiple runs.
+ * We recommend that people look at the [rpg_trajectory_evaluation](https://github.com/uzh-rpg/rpg_trajectory_evaluation) toolbox provided
+ * by Zhang and Scaramuzza. For a background we recommend reading their [A Tutorial on Quantitative Trajectory Evaluation for
+ * Visual(-Inertial) Odometry](http://rpg.ifi.uzh.ch/docs/IROS18_Zhang.pdf) @cite Zhang2018IROS and its use in [A Benchmark Comparison of
+ * Monocular Visual-Inertial Odometry Algorithms for Flying Robots](http://rpg.ifi.uzh.ch/docs/ICRA18_Delmerico.pdf)
+ * @cite Delmerico2018ICRA
+ *
+ *
+ */
+namespace ov_eval {}

ov_eval/src/error_comparison.cpp

@@ -0,0 +1,394 @@
+/*
+ * 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.hpp>
+#include <boost/filesystem.hpp>
+#include <iostream>
+#include <string>
+
+#include "calc/ResultTrajectory.h"
+#include "utils/Loader.h"
+#include "utils/colors.h"
+#include "utils/print.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"
+
+#endif
+
+int main(int argc, char **argv) {
+
+  // Verbosity setting
+  ov_core::Printer::setPrintLevel("INFO");
+
+  // Ensure we have a path
+  if (argc < 4) {
+    PRINT_ERROR(RED "ERROR: Please specify a align mode, folder, and algorithms\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./error_comparison <align_mode> <folder_groundtruth> <folder_algorithms>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval error_comparison <align_mode> <folder_groundtruth> <folder_algorithms>\n" RESET);
+    std::exit(EXIT_FAILURE);
+  }
+
+  // List the groundtruth files in this folder
+  std::string path_gts(argv[2]);
+  std::vector<boost::filesystem::path> path_groundtruths;
+  for (const auto &p : boost::filesystem::recursive_directory_iterator(path_gts)) {
+    if (p.path().extension() == ".txt") {
+      path_groundtruths.push_back(p.path());
+    }
+  }
+  std::sort(path_groundtruths.begin(), path_groundtruths.end());
+
+  // Try to load our paths
+  for (size_t i = 0; i < path_groundtruths.size(); i++) {
+    // Load it!
+    std::vector<double> times;
+    std::vector<Eigen::Matrix<double, 7, 1>> poses;
+    std::vector<Eigen::Matrix3d> cov_ori, cov_pos;
+    ov_eval::Loader::load_data(path_groundtruths.at(i).string(), times, poses, cov_ori, cov_pos);
+    // Print its length and stats
+    double length = ov_eval::Loader::get_total_length(poses);
+    PRINT_INFO("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_groundtruths.at(i).filename().c_str(), length);
+  }
+
+  // Get the algorithms we will process
+  // Also create empty statistic objects for each of our datasets
+  std::string path_algos(argv[3]);
+  std::vector<boost::filesystem::path> path_algorithms;
+  for (const auto &entry : boost::filesystem::directory_iterator(path_algos)) {
+    if (boost::filesystem::is_directory(entry)) {
+      path_algorithms.push_back(entry.path());
+    }
+  }
+  std::sort(path_algorithms.begin(), path_algorithms.end());
+
+  //===============================================================================
+  //===============================================================================
+  //===============================================================================
+
+  // ATE summery information
+  std::map<std::string, std::vector<std::pair<ov_eval::Statistics, ov_eval::Statistics>>> algo_ate;
+  for (const auto &p : path_algorithms) {
+    std::vector<std::pair<ov_eval::Statistics, ov_eval::Statistics>> temp;
+    for (size_t i = 0; i < path_groundtruths.size(); i++) {
+      temp.push_back({ov_eval::Statistics(), ov_eval::Statistics()});
+    }
+    algo_ate.insert({p.filename().string(), temp});
+  }
+
+  // Relative pose error segment lengths
+  std::vector<double> segments = {8.0, 16.0, 24.0, 32.0, 40.0, 48.0};
+  // std::vector<double> segments = {7.0, 14.0, 21.0, 28.0, 35.0};
+  // std::vector<double> segments = {10.0, 25.0, 50.0, 75.0, 120.0};
+  // std::vector<double> segments = {5.0, 15.0, 30.0, 45.0, 60.0};
+  // std::vector<double> segments = {40.0, 60.0, 80.0, 100.0, 120.0};
+
+  // The overall RPE error calculation for each algorithm type
+  std::map<std::string, std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>>> algo_rpe;
+  for (const auto &p : path_algorithms) {
+    std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>> temp;
+    for (const auto &len : segments) {
+      temp.insert({len, {ov_eval::Statistics(), ov_eval::Statistics()}});
+    }
+    algo_rpe.insert({p.filename().string(), temp});
+  }
+
+  //===============================================================================
+  //===============================================================================
+  //===============================================================================
+
+  // Loop through each algorithm type
+  for (size_t i = 0; i < path_algorithms.size(); i++) {
+
+    // Debug print
+    PRINT_DEBUG("======================================\n");
+    PRINT_DEBUG("[COMP]: processing %s algorithm\n", path_algorithms.at(i).filename().c_str());
+
+    // Get the list of datasets this algorithm records
+    std::map<std::string, boost::filesystem::path> path_algo_datasets;
+    for (auto &entry : boost::filesystem::directory_iterator(path_algorithms.at(i))) {
+      if (boost::filesystem::is_directory(entry)) {
+        path_algo_datasets.insert({entry.path().filename().string(), entry.path()});
+      }
+    }
+
+    // Loop through our list of groundtruth datasets, and see if we have it
+    for (size_t j = 0; j < path_groundtruths.size(); j++) {
+
+      // Check if we have runs for this dataset
+      if (path_algo_datasets.find(path_groundtruths.at(j).stem().string()) == path_algo_datasets.end()) {
+        PRINT_ERROR(RED "[COMP]: %s dataset does not have any runs for %s!!!!!\n" RESET, path_algorithms.at(i).filename().c_str(),
+                    path_groundtruths.at(j).stem().c_str());
+        continue;
+      }
+
+      // Debug print
+      PRINT_DEBUG("[COMP]: processing %s algorithm => %s dataset\n", path_algorithms.at(i).filename().c_str(),
+                  path_groundtruths.at(j).stem().c_str());
+
+      // Errors for this specific dataset (i.e. our averages over the total runs)
+      ov_eval::Statistics ate_dataset_ori;
+      ov_eval::Statistics ate_dataset_pos;
+      std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>> rpe_dataset;
+      for (const auto &len : segments) {
+        rpe_dataset.insert({len, {ov_eval::Statistics(), ov_eval::Statistics()}});
+      }
+
+      // Loop though the different runs for this dataset
+      std::vector<std::string> file_paths;
+      for (auto &entry : boost::filesystem::directory_iterator(path_algo_datasets.at(path_groundtruths.at(j).stem().string()))) {
+        if (entry.path().extension() != ".txt")
+          continue;
+        file_paths.push_back(entry.path().string());
+      }
+      std::sort(file_paths.begin(), file_paths.end());
+
+      // Now loop through the sorted vector
+      for (auto &path_esttxt : file_paths) {
+        // Our paths
+        std::string dataset = path_groundtruths.at(j).stem().string();
+        std::string path_gttxt = path_groundtruths.at(j).string();
+
+        // Create our trajectory object
+        ov_eval::ResultTrajectory traj(path_esttxt, path_gttxt, argv[1]);
+
+        // Calculate ATE error for this dataset
+        ov_eval::Statistics error_ori, error_pos;
+        traj.calculate_ate(error_ori, error_pos);
+        ate_dataset_ori.values.push_back(error_ori.rmse);
+        ate_dataset_pos.values.push_back(error_pos.rmse);
+
+        // Calculate RPE error for this dataset
+        std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>> error_rpe;
+        traj.calculate_rpe(segments, error_rpe);
+        for (const auto &elm : error_rpe) {
+          rpe_dataset.at(elm.first).first.values.insert(rpe_dataset.at(elm.first).first.values.end(), elm.second.first.values.begin(),
+                                                        elm.second.first.values.end());
+          rpe_dataset.at(elm.first).first.timestamps.insert(rpe_dataset.at(elm.first).first.timestamps.end(),
+                                                            elm.second.first.timestamps.begin(), elm.second.first.timestamps.end());
+          rpe_dataset.at(elm.first).second.values.insert(rpe_dataset.at(elm.first).second.values.end(), elm.second.second.values.begin(),
+                                                         elm.second.second.values.end());
+          rpe_dataset.at(elm.first).second.timestamps.insert(rpe_dataset.at(elm.first).second.timestamps.end(),
+                                                             elm.second.second.timestamps.begin(), elm.second.second.timestamps.end());
+        }
+      }
+
+      // Compute our mean ATE score
+      ate_dataset_ori.calculate();
+      ate_dataset_pos.calculate();
+
+      // Print stats for this specific dataset
+      std::string prefix = (ate_dataset_ori.mean > 10 || ate_dataset_pos.mean > 10) ? RED : "";
+      PRINT_DEBUG("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean,
+                  ate_dataset_pos.mean, (int)ate_dataset_pos.values.size());
+      PRINT_DEBUG("\tATE: std_ori  = %.3f | std_pos  = %.3f\n", ate_dataset_ori.std, ate_dataset_pos.std);
+      for (auto &seg : rpe_dataset) {
+        seg.second.first.calculate();
+        seg.second.second.calculate();
+        // PRINT_DEBUG("\tRPE: seg %d - mean_ori = %.3f | mean_pos = %.3f (%d
+        // samples)\n",(int)seg.first,seg.second.first.mean,seg.second.second.mean,(int)seg.second.second.values.size());
+        PRINT_DEBUG("\tRPE: seg %d - median_ori = %.4f | median_pos = %.4f (%d samples)\n", (int)seg.first, seg.second.first.median,
+                    seg.second.second.median, (int)seg.second.second.values.size());
+        // PRINT_DEBUG("RPE: seg %d - std_ori  = %.3f | std_pos  = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std);
+      }
+
+      // Update the global ATE error stats
+      std::string algo = path_algorithms.at(i).filename().string();
+      algo_ate.at(algo).at(j).first = ate_dataset_ori;
+      algo_ate.at(algo).at(j).second = ate_dataset_pos;
+
+      // Update the global RPE error stats
+      for (const auto &elm : rpe_dataset) {
+        algo_rpe.at(algo).at(elm.first).first.values.insert(algo_rpe.at(algo).at(elm.first).first.values.end(),
+                                                            elm.second.first.values.begin(), elm.second.first.values.end());
+        algo_rpe.at(algo).at(elm.first).first.timestamps.insert(algo_rpe.at(algo).at(elm.first).first.timestamps.end(),
+                                                                elm.second.first.timestamps.begin(), elm.second.first.timestamps.end());
+        algo_rpe.at(algo).at(elm.first).second.values.insert(algo_rpe.at(algo).at(elm.first).second.values.end(),
+                                                             elm.second.second.values.begin(), elm.second.second.values.end());
+        algo_rpe.at(algo).at(elm.first).second.timestamps.insert(algo_rpe.at(algo).at(elm.first).second.timestamps.end(),
+                                                                 elm.second.second.timestamps.begin(), elm.second.second.timestamps.end());
+      }
+    }
+  }
+  PRINT_DEBUG("\n\n");
+
+  //===============================================================================
+  //===============================================================================
+  //===============================================================================
+
+  // Finally print the ATE for all the runs
+  PRINT_INFO("============================================\n");
+  PRINT_INFO("ATE LATEX TABLE\n");
+  PRINT_INFO("============================================\n");
+  for (size_t i = 0; i < path_groundtruths.size(); i++) {
+    std::string gtname = path_groundtruths.at(i).stem().string();
+    boost::replace_all(gtname, "_", "\\_");
+    PRINT_INFO(" & \\textbf{%s}", gtname.c_str());
+  }
+  PRINT_INFO(" & \\textbf{Average} \\\\\\hline\n");
+  for (auto &algo : algo_ate) {
+    std::string algoname = algo.first;
+    boost::replace_all(algoname, "_", "\\_");
+    PRINT_INFO(algoname.c_str());
+    double sum_ori = 0.0;
+    double sum_pos = 0.0;
+    int sum_ct = 0;
+    for (auto &seg : algo.second) {
+      if (seg.first.values.empty() || seg.second.values.empty()) {
+        PRINT_INFO(" & - / -");
+      } else {
+        seg.first.calculate();
+        seg.second.calculate();
+        PRINT_INFO(" & %.3f / %.3f", seg.first.mean, seg.second.mean);
+        sum_ori += seg.first.mean;
+        sum_pos += seg.second.mean;
+        sum_ct++;
+      }
+    }
+    PRINT_INFO(" & %.3f / %.3f \\\\\n", sum_ori / sum_ct, sum_pos / sum_ct);
+  }
+  PRINT_INFO("============================================\n");
+
+  // Finally print the RPE for all the runs
+  PRINT_INFO("============================================\n");
+  PRINT_INFO("RPE LATEX TABLE\n");
+  PRINT_INFO("============================================\n");
+  for (const auto &len : segments) {
+    PRINT_INFO(" & \\textbf{%dm}", (int)len);
+  }
+  PRINT_INFO(" \\\\\\hline\n");
+  for (auto &algo : algo_rpe) {
+    std::string algoname = algo.first;
+    boost::replace_all(algoname, "_", "\\_");
+    PRINT_INFO(algoname.c_str());
+    for (auto &seg : algo.second) {
+      seg.second.first.calculate();
+      seg.second.second.calculate();
+      PRINT_INFO(" & %.3f / %.3f", seg.second.first.mean, seg.second.second.mean);
+    }
+    PRINT_INFO(" \\\\\n");
+  }
+  PRINT_INFO("============================================\n");
+
+#ifdef HAVE_PYTHONLIBS
+
+  // Plot line colors
+  std::vector<std::string> colors = {"blue", "red", "black", "green", "cyan", "magenta"};
+  std::vector<std::string> linestyle = {"-", "--", "-."};
+  assert(algo_rpe.size() <= colors.size() * linestyle.size());
+
+  // Parameters
+  std::map<std::string, std::string> params_rpe;
+  params_rpe.insert({"notch", "false"});
+  params_rpe.insert({"sym", ""});
+
+  // Plot this figure
+  matplotlibcpp::figure_size(1000, 700);
+  matplotlibcpp::subplot(2, 1, 1);
+  matplotlibcpp::title("Relative Orientation Error");
+
+  // Plot each RPE next to each other
+  double width = 1.0 / (algo_rpe.size() + 1);
+  double spacing = width / (algo_rpe.size() + 1);
+  std::vector<double> xticks;
+  std::vector<std::string> labels;
+  int ct_algo = 0;
+  double ct_pos = 0;
+  for (auto &algo : algo_rpe) {
+    // Start based on what algorithm we are doing
+    xticks.clear();
+    labels.clear();
+    ct_pos = 1 + ct_algo * (width + spacing);
+    // Loop through each length type
+    for (auto &seg : algo.second) {
+      xticks.push_back(ct_pos - (algo_rpe.size() * (width + spacing) - width) / 2);
+      labels.push_back(std::to_string((int)seg.first));
+      matplotlibcpp::boxplot(seg.second.first.values, ct_pos, width, colors.at(ct_algo % colors.size()),
+                             linestyle.at(ct_algo / colors.size()), params_rpe);
+      ct_pos += 1 + 3 * width;
+    }
+    // Move forward
+    ct_algo++;
+  }
+
+  // Add "fake" plots for our legend
+  ct_algo = 0;
+  for (const auto &algo : algo_rpe) {
+    std::map<std::string, std::string> params_empty;
+    params_empty.insert({"label", algo.first});
+    params_empty.insert({"linestyle", linestyle.at(ct_algo / colors.size())});
+    params_empty.insert({"color", colors.at(ct_algo % colors.size())});
+    std::vector<double> vec_empty;
+    matplotlibcpp::plot(vec_empty, vec_empty, params_empty);
+    ct_algo++;
+  }
+
+  // Plot each RPE next to each other
+  matplotlibcpp::xlim(0.5, ct_pos - 0.5 - 3 * width);
+  matplotlibcpp::xticks(xticks, labels);
+  matplotlibcpp::ylabel("orientation error (deg)");
+  matplotlibcpp::legend();
+  matplotlibcpp::subplot(2, 1, 2);
+  ct_algo = 0;
+  ct_pos = 0;
+  for (auto &algo : algo_rpe) {
+    // Start based on what algorithm we are doing
+    ct_pos = 1 + ct_algo * (width + spacing);
+    // Loop through each length type
+    for (auto &seg : algo.second) {
+      matplotlibcpp::boxplot(seg.second.second.values, ct_pos, width, colors.at(ct_algo % colors.size()),
+                             linestyle.at(ct_algo / colors.size()), params_rpe);
+      ct_pos += 1 + 3 * width;
+    }
+    // Move forward
+    ct_algo++;
+  }
+
+  // Add "fake" plots for our legend
+  ct_algo = 0;
+  for (const auto &algo : algo_rpe) {
+    std::map<std::string, std::string> params_empty;
+    params_empty.insert({"label", algo.first});
+    params_empty.insert({"linestyle", linestyle.at(ct_algo / colors.size())});
+    params_empty.insert({"color", colors.at(ct_algo % colors.size())});
+    std::vector<double> vec_empty;
+    matplotlibcpp::plot(vec_empty, vec_empty, params_empty);
+    ct_algo++;
+  }
+
+  // Display to the user
+  matplotlibcpp::xlim(0.5, ct_pos - 0.5 - 3 * width);
+  matplotlibcpp::xticks(xticks, labels);
+  matplotlibcpp::title("Relative Position Error");
+  matplotlibcpp::ylabel("translational error (m)");
+  matplotlibcpp::xlabel("sub-segment lengths (m)");
+  matplotlibcpp::show(true);
+
+#endif
+
+  // Done!
+  return EXIT_SUCCESS;
+}

ov_eval/src/error_dataset.cpp

@@ -0,0 +1,425 @@
+/*
+ * 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.hpp>
+#include <boost/filesystem.hpp>
+#include <string>
+
+#include "calc/ResultTrajectory.h"
+#include "utils/Loader.h"
+#include "utils/colors.h"
+#include "utils/print.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"
+
+#endif
+
+int main(int argc, char **argv) {
+
+  // Verbosity setting
+  ov_core::Printer::setPrintLevel("INFO");
+
+  // Ensure we have a path
+  if (argc < 4) {
+    PRINT_ERROR(RED "ERROR: Please specify a align mode, folder, and algorithms\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./error_dataset <align_mode> <file_gt.txt> <folder_algorithms>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval error_dataset <align_mode> <file_gt.txt> <folder_algorithms>\n" RESET);
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Load it!
+  boost::filesystem::path path_gt(argv[2]);
+  std::vector<double> times;
+  std::vector<Eigen::Matrix<double, 7, 1>> poses;
+  std::vector<Eigen::Matrix3d> cov_ori, cov_pos;
+  ov_eval::Loader::load_data(argv[2], times, poses, cov_ori, cov_pos);
+
+  // Print its length and stats
+  double length = ov_eval::Loader::get_total_length(poses);
+  PRINT_INFO("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_gt.stem().string().c_str(), length);
+
+  // Get the algorithms we will process
+  // Also create empty statistic objects for each of our datasets
+  std::string path_algos(argv[3]);
+  std::vector<boost::filesystem::path> path_algorithms;
+  for (const auto &entry : boost::filesystem::directory_iterator(path_algos)) {
+    if (boost::filesystem::is_directory(entry)) {
+      path_algorithms.push_back(entry.path());
+    }
+  }
+  std::sort(path_algorithms.begin(), path_algorithms.end());
+
+  //===============================================================================
+  //===============================================================================
+  //===============================================================================
+
+  // ATE summery information
+  std::map<std::string, std::pair<ov_eval::Statistics, ov_eval::Statistics>> algo_ate;
+  std::map<std::string, std::pair<ov_eval::Statistics, ov_eval::Statistics>> algo_nees;
+  for (const auto &p : path_algorithms) {
+    algo_ate.insert({p.filename().string(), {ov_eval::Statistics(), ov_eval::Statistics()}});
+    algo_nees.insert({p.filename().string(), {ov_eval::Statistics(), ov_eval::Statistics()}});
+  }
+
+  // Relative pose error segment lengths
+  // std::vector<double> segments = {8.0, 16.0, 24.0, 32.0, 40.0, 48.0};
+  std::vector<double> segments = {7.0, 14.0, 21.0, 28.0, 35.0};
+  // std::vector<double> segments = {10.0, 25.0, 50.0, 75.0, 120.0};
+  // std::vector<double> segments = {5.0, 15.0, 30.0, 45.0, 60.0};
+  // std::vector<double> segments = {40.0, 60.0, 80.0, 100.0, 120.0};
+
+  // The overall RPE error calculation for each algorithm type
+  std::map<std::string, std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>>> algo_rpe;
+  for (const auto &p : path_algorithms) {
+    std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>> temp;
+    for (const auto &len : segments) {
+      temp.insert({len, {ov_eval::Statistics(), ov_eval::Statistics()}});
+    }
+    algo_rpe.insert({p.filename().string(), temp});
+  }
+
+  //===============================================================================
+  //===============================================================================
+  //===============================================================================
+
+  // Loop through each algorithm type
+  for (size_t i = 0; i < path_algorithms.size(); i++) {
+
+    // Debug print
+    PRINT_DEBUG("======================================\n");
+    PRINT_DEBUG("[COMP]: processing %s algorithm\n", path_algorithms.at(i).filename().c_str());
+
+    // Get the list of datasets this algorithm records
+    std::map<std::string, boost::filesystem::path> path_algo_datasets;
+    for (auto &entry : boost::filesystem::directory_iterator(path_algorithms.at(i))) {
+      if (boost::filesystem::is_directory(entry)) {
+        path_algo_datasets.insert({entry.path().filename().string(), entry.path()});
+      }
+    }
+
+    // Check if we have runs for our dataset
+    if (path_algo_datasets.find(path_gt.stem().string()) == path_algo_datasets.end()) {
+      PRINT_DEBUG(RED "[COMP]: %s dataset does not have any runs for %s!!!!!\n" RESET, path_algorithms.at(i).filename().c_str(),
+                  path_gt.stem().c_str());
+      continue;
+    }
+
+    // Errors for this specific dataset (i.e. our averages over the total runs)
+    ov_eval::Statistics ate_dataset_ori, ate_dataset_pos;
+    ov_eval::Statistics ate_2d_dataset_ori, ate_2d_dataset_pos;
+    std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>> rpe_dataset;
+    for (const auto &len : segments) {
+      rpe_dataset.insert({len, {ov_eval::Statistics(), ov_eval::Statistics()}});
+    }
+    std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>> rmse_dataset;
+    std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>> rmse_2d_dataset;
+    std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>> nees_dataset;
+
+    // Loop though the different runs for this dataset
+    std::vector<std::string> file_paths;
+    for (auto &entry : boost::filesystem::directory_iterator(path_algo_datasets.at(path_gt.stem().string()))) {
+      if (entry.path().extension() != ".txt")
+        continue;
+      file_paths.push_back(entry.path().string());
+    }
+    std::sort(file_paths.begin(), file_paths.end());
+
+    // Check if we have runs
+    if (file_paths.empty()) {
+      PRINT_DEBUG(RED "\tERROR: No runs found for %s, is the folder structure right??\n" RESET, path_algorithms.at(i).filename().c_str());
+      continue;
+    }
+
+    // Loop though the different runs for this dataset
+    for (auto &path_esttxt : file_paths) {
+
+      // Create our trajectory object
+      std::string path_gttxt = path_gt.string();
+      ov_eval::ResultTrajectory traj(path_esttxt, path_gttxt, argv[1]);
+
+      // Calculate ATE error for this dataset
+      ov_eval::Statistics error_ori, error_pos;
+      traj.calculate_ate(error_ori, error_pos);
+      ate_dataset_ori.values.push_back(error_ori.rmse);
+      ate_dataset_pos.values.push_back(error_pos.rmse);
+      for (size_t j = 0; j < error_ori.values.size(); j++) {
+        rmse_dataset[error_ori.timestamps.at(j)].first.values.push_back(error_ori.values.at(j));
+        rmse_dataset[error_pos.timestamps.at(j)].second.values.push_back(error_pos.values.at(j));
+        assert(error_ori.timestamps.at(j) == error_pos.timestamps.at(j));
+      }
+
+      // Calculate ATE 2D error for this dataset
+      ov_eval::Statistics error_ori_2d, error_pos_2d;
+      traj.calculate_ate_2d(error_ori_2d, error_pos_2d);
+      ate_2d_dataset_ori.values.push_back(error_ori_2d.rmse);
+      ate_2d_dataset_pos.values.push_back(error_pos_2d.rmse);
+      for (size_t j = 0; j < error_ori_2d.values.size(); j++) {
+        rmse_2d_dataset[error_ori_2d.timestamps.at(j)].first.values.push_back(error_ori_2d.values.at(j));
+        rmse_2d_dataset[error_pos_2d.timestamps.at(j)].second.values.push_back(error_pos_2d.values.at(j));
+        assert(error_ori_2d.timestamps.at(j) == error_pos_2d.timestamps.at(j));
+      }
+
+      // NEES error for this dataset
+      ov_eval::Statistics nees_ori, nees_pos;
+      traj.calculate_nees(nees_ori, nees_pos);
+      for (size_t j = 0; j < nees_ori.values.size(); j++) {
+        nees_dataset[nees_ori.timestamps.at(j)].first.values.push_back(nees_ori.values.at(j));
+        nees_dataset[nees_ori.timestamps.at(j)].second.values.push_back(nees_pos.values.at(j));
+        assert(nees_ori.timestamps.at(j) == nees_pos.timestamps.at(j));
+      }
+
+      // Calculate RPE error for this dataset
+      std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>> error_rpe;
+      traj.calculate_rpe(segments, error_rpe);
+      for (const auto &elm : error_rpe) {
+        rpe_dataset.at(elm.first).first.values.insert(rpe_dataset.at(elm.first).first.values.end(), elm.second.first.values.begin(),
+                                                      elm.second.first.values.end());
+        rpe_dataset.at(elm.first).first.timestamps.insert(rpe_dataset.at(elm.first).first.timestamps.end(),
+                                                          elm.second.first.timestamps.begin(), elm.second.first.timestamps.end());
+        rpe_dataset.at(elm.first).second.values.insert(rpe_dataset.at(elm.first).second.values.end(), elm.second.second.values.begin(),
+                                                       elm.second.second.values.end());
+        rpe_dataset.at(elm.first).second.timestamps.insert(rpe_dataset.at(elm.first).second.timestamps.end(),
+                                                           elm.second.second.timestamps.begin(), elm.second.second.timestamps.end());
+      }
+    }
+
+    // Compute our mean ATE score
+    ate_dataset_ori.calculate();
+    ate_dataset_pos.calculate();
+    ate_2d_dataset_ori.calculate();
+    ate_2d_dataset_pos.calculate();
+
+    // Print stats for this specific dataset
+    std::string prefix = (ate_dataset_ori.mean > 10 || ate_dataset_pos.mean > 10) ? RED : "";
+    PRINT_DEBUG("%s\tATE: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n" RESET, prefix.c_str(), ate_dataset_ori.mean, ate_dataset_pos.mean,
+                (int)ate_dataset_ori.values.size());
+    PRINT_DEBUG("\tATE: std_ori  = %.5f | std_pos  = %.5f\n", ate_dataset_ori.std, ate_dataset_pos.std);
+    PRINT_DEBUG("\tATE 2D: mean_ori = %.3f | mean_pos = %.3f (%d runs)\n", ate_2d_dataset_ori.mean, ate_2d_dataset_pos.mean,
+                (int)ate_2d_dataset_ori.values.size());
+    PRINT_DEBUG("\tATE 2D: std_ori  = %.5f | std_pos  = %.5f\n", ate_2d_dataset_ori.std, ate_2d_dataset_pos.std);
+    for (auto &seg : rpe_dataset) {
+      seg.second.first.calculate();
+      seg.second.second.calculate();
+      PRINT_DEBUG("\tRPE: seg %d - mean_ori = %.3f | mean_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.mean,
+                  seg.second.second.mean, (int)seg.second.second.values.size());
+      // PRINT_DEBUG("RPE: seg %d - std_ori  = %.3f | std_pos  = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std);
+    }
+
+    // RMSE: Convert into the right format (only use times where all runs have an error)
+    ov_eval::Statistics rmse_ori, rmse_pos;
+    for (auto &elm : rmse_dataset) {
+      if (elm.second.first.values.size() == file_paths.size()) {
+        elm.second.first.calculate();
+        elm.second.second.calculate();
+        rmse_ori.timestamps.push_back(elm.first);
+        rmse_ori.values.push_back(elm.second.first.rmse);
+        rmse_pos.timestamps.push_back(elm.first);
+        rmse_pos.values.push_back(elm.second.second.rmse);
+      }
+    }
+    rmse_ori.calculate();
+    rmse_pos.calculate();
+    PRINT_DEBUG("\tRMSE: mean_ori = %.3f | mean_pos = %.3f\n", rmse_ori.mean, rmse_pos.mean);
+
+    // RMSE: Convert into the right format (only use times where all runs have an error)
+    ov_eval::Statistics rmse_2d_ori, rmse_2d_pos;
+    for (auto &elm : rmse_2d_dataset) {
+      if (elm.second.first.values.size() == file_paths.size()) {
+        elm.second.first.calculate();
+        elm.second.second.calculate();
+        rmse_2d_ori.timestamps.push_back(elm.first);
+        rmse_2d_ori.values.push_back(elm.second.first.rmse);
+        rmse_2d_pos.timestamps.push_back(elm.first);
+        rmse_2d_pos.values.push_back(elm.second.second.rmse);
+      }
+    }
+    rmse_2d_ori.calculate();
+    rmse_2d_pos.calculate();
+    PRINT_DEBUG("\tRMSE 2D: mean_ori = %.3f | mean_pos = %.3f\n", rmse_2d_ori.mean, rmse_2d_pos.mean);
+
+    // NEES: Convert into the right format (only use times where all runs have an error)
+    ov_eval::Statistics nees_ori, nees_pos;
+    for (auto &elm : nees_dataset) {
+      if (elm.second.first.values.size() == file_paths.size()) {
+        elm.second.first.calculate();
+        elm.second.second.calculate();
+        nees_ori.timestamps.push_back(elm.first);
+        nees_ori.values.push_back(elm.second.first.mean);
+        nees_pos.timestamps.push_back(elm.first);
+        nees_pos.values.push_back(elm.second.second.mean);
+      }
+    }
+    nees_ori.calculate();
+    nees_pos.calculate();
+    PRINT_DEBUG("\tNEES: mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean);
+
+#ifdef HAVE_PYTHONLIBS
+
+    //=====================================================
+    // RMSE plot at each timestep
+    matplotlibcpp::figure_size(1000, 600);
+
+    // Zero our time arrays
+    double starttime1 = (rmse_ori.timestamps.empty()) ? 0 : rmse_ori.timestamps.at(0);
+    double endtime1 = (rmse_ori.timestamps.empty()) ? 0 : rmse_ori.timestamps.at(rmse_ori.timestamps.size() - 1);
+    for (size_t j = 0; j < rmse_ori.timestamps.size(); j++) {
+      rmse_ori.timestamps.at(j) -= starttime1;
+      rmse_pos.timestamps.at(j) -= starttime1;
+    }
+
+    // Update the title and axis labels
+    matplotlibcpp::subplot(2, 1, 1);
+    matplotlibcpp::title("Root Mean Squared Error - " + path_algorithms.at(i).filename().string());
+    matplotlibcpp::ylabel("Error Orientation (deg)");
+    matplotlibcpp::plot(rmse_ori.timestamps, rmse_ori.values);
+    matplotlibcpp::xlim(0.0, endtime1 - starttime1);
+    matplotlibcpp::subplot(2, 1, 2);
+    matplotlibcpp::ylabel("Error Position (m)");
+    matplotlibcpp::xlabel("dataset time (s)");
+    matplotlibcpp::plot(rmse_pos.timestamps, rmse_pos.values);
+    matplotlibcpp::xlim(0.0, endtime1 - starttime1);
+
+    // Display to the user
+    matplotlibcpp::tight_layout();
+    matplotlibcpp::show(false);
+
+    //=====================================================
+
+    if (!nees_ori.values.empty() && !nees_pos.values.empty()) {
+      // NEES plot at each timestep
+      matplotlibcpp::figure_size(1000, 600);
+
+      // Zero our time arrays
+      double starttime2 = (nees_ori.timestamps.empty()) ? 0 : nees_ori.timestamps.at(0);
+      double endtime2 = (nees_ori.timestamps.empty()) ? 0 : nees_ori.timestamps.at(nees_ori.timestamps.size() - 1);
+      for (size_t j = 0; j < nees_ori.timestamps.size(); j++) {
+        nees_ori.timestamps.at(j) -= starttime2;
+        nees_pos.timestamps.at(j) -= starttime2;
+      }
+
+      // Update the title and axis labels
+      matplotlibcpp::subplot(2, 1, 1);
+      matplotlibcpp::title("Normalized Estimation Error Squared - " + path_algorithms.at(i).filename().string());
+      matplotlibcpp::ylabel("NEES Orientation");
+      matplotlibcpp::plot(nees_ori.timestamps, nees_ori.values);
+      matplotlibcpp::xlim(0.0, endtime2 - starttime2);
+      matplotlibcpp::subplot(2, 1, 2);
+      matplotlibcpp::ylabel("NEES Position");
+      matplotlibcpp::xlabel("dataset time (s)");
+      matplotlibcpp::plot(nees_pos.timestamps, nees_pos.values);
+      matplotlibcpp::xlim(0.0, endtime2 - starttime2);
+
+      // Display to the user
+      matplotlibcpp::tight_layout();
+      matplotlibcpp::show(false);
+    }
+
+#endif
+
+    // Update the global ATE error stats
+    std::string algo = path_algorithms.at(i).filename().string();
+    algo_ate.at(algo).first = ate_dataset_ori;
+    algo_ate.at(algo).second = ate_dataset_pos;
+    algo_nees.at(algo).first = nees_ori;
+    algo_nees.at(algo).second = nees_pos;
+
+    // Update the global RPE error stats
+    for (const auto &elm : rpe_dataset) {
+      algo_rpe.at(algo).at(elm.first).first.values.insert(algo_rpe.at(algo).at(elm.first).first.values.end(),
+                                                          elm.second.first.values.begin(), elm.second.first.values.end());
+      algo_rpe.at(algo).at(elm.first).first.timestamps.insert(algo_rpe.at(algo).at(elm.first).first.timestamps.end(),
+                                                              elm.second.first.timestamps.begin(), elm.second.first.timestamps.end());
+      algo_rpe.at(algo).at(elm.first).second.values.insert(algo_rpe.at(algo).at(elm.first).second.values.end(),
+                                                           elm.second.second.values.begin(), elm.second.second.values.end());
+      algo_rpe.at(algo).at(elm.first).second.timestamps.insert(algo_rpe.at(algo).at(elm.first).second.timestamps.end(),
+                                                               elm.second.second.timestamps.begin(), elm.second.second.timestamps.end());
+    }
+  }
+  PRINT_DEBUG("\n\n");
+
+  // Finally print the ATE for all the runs
+  PRINT_INFO("============================================\n");
+  PRINT_INFO("ATE AND NEES LATEX TABLE\n");
+  PRINT_INFO("============================================\n");
+  PRINT_INFO(" & \\textbf{ATE (deg/m)} & \\textbf{NEES (deg/m)} \\\\\\hline\n");
+  for (auto &algo : algo_ate) {
+    std::string algoname = algo.first;
+    boost::replace_all(algoname, "_", "\\_");
+    PRINT_INFO(algoname.c_str());
+    // ate
+    auto ate_oripos = algo.second;
+    if (ate_oripos.first.values.empty() || ate_oripos.second.values.empty()) {
+      PRINT_INFO(" & - / -");
+    } else {
+      ate_oripos.first.calculate();
+      ate_oripos.second.calculate();
+      PRINT_INFO(" & %.3f / %.3f", ate_oripos.first.mean, ate_oripos.second.mean);
+    }
+    // nees
+    auto nees_oripos = algo_nees.at(algo.first);
+    if (nees_oripos.first.values.empty() || nees_oripos.second.values.empty()) {
+      PRINT_INFO(" & - / -");
+    } else {
+      nees_oripos.first.calculate();
+      nees_oripos.second.calculate();
+      PRINT_INFO(" & %.3f / %.3f", nees_oripos.first.mean, nees_oripos.second.mean);
+    }
+    PRINT_INFO(" \\\\\n");
+  }
+  PRINT_INFO("============================================\n");
+
+  // Finally print the RPE for all the runs
+  PRINT_INFO("============================================\n");
+  PRINT_INFO("RPE LATEX TABLE\n");
+  PRINT_INFO("============================================\n");
+  for (const auto &len : segments) {
+    PRINT_INFO(" & \\textbf{%dm}", (int)len);
+  }
+  PRINT_INFO(" \\\\\\hline\n");
+  for (auto &algo : algo_rpe) {
+    std::string algoname = algo.first;
+    boost::replace_all(algoname, "_", "\\_");
+    PRINT_INFO(algoname.c_str());
+    for (auto &seg : algo.second) {
+      seg.second.first.calculate();
+      seg.second.second.calculate();
+      PRINT_INFO(" & %.3f / %.3f", seg.second.first.mean, seg.second.second.mean);
+    }
+    PRINT_INFO(" \\\\\n");
+  }
+  PRINT_INFO("============================================\n");
+
+#ifdef HAVE_PYTHONLIBS
+
+  // Wait till the user kills this node
+  matplotlibcpp::show(true);
+
+#endif
+
+  // Done!
+  return EXIT_SUCCESS;
+}

ov_eval/src/error_simulation.cpp

@@ -0,0 +1,72 @@
+/*
+ * 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 "calc/ResultSimulation.h"
+#include "utils/colors.h"
+#include "utils/print.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"
+
+#endif
+
+int main(int argc, char **argv) {
+
+  // Verbosity setting
+  ov_core::Printer::setPrintLevel("INFO");
+
+  // Ensure we have a path
+  if (argc < 4) {
+    PRINT_ERROR(RED "ERROR: ./error_simulation <file_est.txt> <file_std.txt> <file_gt.txt>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval error_simulation <file_est.txt> <file_std.txt> <file_gt.txt>\n" RESET);
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Create our trajectory object
+  ov_eval::ResultSimulation traj(argv[1], argv[2], argv[3]);
+
+  // Plot the state errors
+  PRINT_INFO("Plotting state variable errors...\n");
+  traj.plot_state(true);
+
+  // Plot time offset
+  PRINT_INFO("Plotting time offset error...\n");
+  traj.plot_timeoff(true, 10);
+
+  // Plot camera intrinsics
+  PRINT_INFO("Plotting camera intrinsics...\n");
+  traj.plot_cam_instrinsics(true, 60);
+
+  // Plot camera extrinsics
+  PRINT_INFO("Plotting camera extrinsics...\n");
+  traj.plot_cam_extrinsics(true, 60);
+
+#ifdef HAVE_PYTHONLIBS
+  matplotlibcpp::show(true);
+#endif
+
+  // Done!
+  return EXIT_SUCCESS;
+}

ov_eval/src/error_singlerun.cpp

@@ -0,0 +1,353 @@
+/*
+ * 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/filesystem.hpp>
+#include <string>
+
+#include "calc/ResultTrajectory.h"
+#include "utils/colors.h"
+#include "utils/print.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 three error values in three sub-plots in our current figure
+void plot_3errors(ov_eval::Statistics sx, ov_eval::Statistics sy, ov_eval::Statistics sz) {
+
+  // Parameters that define the line styles
+  std::map<std::string, std::string> params_value, params_bound;
+  params_value.insert({"label", "error"});
+  params_value.insert({"linestyle", "-"});
+  params_value.insert({"color", "blue"});
+  params_bound.insert({"label", "3 sigma bound"});
+  params_bound.insert({"linestyle", "--"});
+  params_bound.insert({"color", "red"});
+
+  // Plot our error value
+  matplotlibcpp::subplot(3, 1, 1);
+  matplotlibcpp::plot(sx.timestamps, sx.values, params_value);
+  if (!sx.values_bound.empty()) {
+    matplotlibcpp::plot(sx.timestamps, sx.values_bound, params_bound);
+    for (size_t i = 0; i < sx.timestamps.size(); i++) {
+      sx.values_bound.at(i) *= -1;
+    }
+    matplotlibcpp::plot(sx.timestamps, sx.values_bound, "r--");
+  }
+
+  // Plot our error value
+  matplotlibcpp::subplot(3, 1, 2);
+  matplotlibcpp::plot(sy.timestamps, sy.values, params_value);
+  if (!sy.values_bound.empty()) {
+    matplotlibcpp::plot(sy.timestamps, sy.values_bound, params_bound);
+    for (size_t i = 0; i < sy.timestamps.size(); i++) {
+      sy.values_bound.at(i) *= -1;
+    }
+    matplotlibcpp::plot(sy.timestamps, sy.values_bound, "r--");
+  }
+
+  // Plot our error value
+  matplotlibcpp::subplot(3, 1, 3);
+  matplotlibcpp::plot(sz.timestamps, sz.values, params_value);
+  if (!sz.values_bound.empty()) {
+    matplotlibcpp::plot(sz.timestamps, sz.values_bound, params_bound);
+    for (size_t i = 0; i < sz.timestamps.size(); i++) {
+      sz.values_bound.at(i) *= -1;
+    }
+    matplotlibcpp::plot(sz.timestamps, sz.values_bound, "r--");
+  }
+}
+
+#endif
+
+int main(int argc, char **argv) {
+
+  // Verbosity setting
+  ov_core::Printer::setPrintLevel("INFO");
+
+  // Ensure we have a path
+  if (argc < 4) {
+    PRINT_ERROR(RED "ERROR: Please specify a align mode, groudtruth, and algorithm run file\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./error_singlerun <align_mode> <file_gt.txt> <file_est.txt>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval error_singlerun <align_mode> <file_gt.txt> <file_est.txt>\n" RESET);
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Load it!
+  boost::filesystem::path path_gt(argv[2]);
+  std::vector<double> times;
+  std::vector<Eigen::Matrix<double, 7, 1>> poses;
+  std::vector<Eigen::Matrix3d> cov_ori, cov_pos;
+  ov_eval::Loader::load_data(argv[2], times, poses, cov_ori, cov_pos);
+  // Print its length and stats
+  double length = ov_eval::Loader::get_total_length(poses);
+  PRINT_DEBUG("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_gt.stem().string().c_str(), length);
+
+  // Create our trajectory object
+  ov_eval::ResultTrajectory traj(argv[3], argv[2], argv[1]);
+
+  //===========================================================
+  // Absolute trajectory error
+  //===========================================================
+
+  // Calculate
+  ov_eval::Statistics error_ori, error_pos;
+  traj.calculate_ate(error_ori, error_pos);
+
+  // Print it
+  PRINT_INFO("======================================\n");
+  PRINT_INFO("Absolute Trajectory Error\n");
+  PRINT_INFO("======================================\n");
+  PRINT_INFO("rmse_ori = %.3f | rmse_pos = %.3f\n", error_ori.rmse, error_pos.rmse);
+  PRINT_INFO("mean_ori = %.3f | mean_pos = %.3f\n", error_ori.mean, error_pos.mean);
+  PRINT_INFO("min_ori  = %.3f | min_pos  = %.3f\n", error_ori.min, error_pos.min);
+  PRINT_INFO("max_ori  = %.3f | max_pos  = %.3f\n", error_ori.max, error_pos.max);
+  PRINT_INFO("std_ori  = %.3f | std_pos  = %.3f\n", error_ori.std, error_pos.std);
+
+  //===========================================================
+  // Relative pose error
+  //===========================================================
+
+  // Calculate
+  std::vector<double> segments = {8.0, 16.0, 24.0, 32.0, 40.0};
+  std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>> error_rpe;
+  traj.calculate_rpe(segments, error_rpe);
+
+  // Print it
+  PRINT_INFO("======================================\n");
+  PRINT_INFO("Relative Pose Error\n");
+  PRINT_INFO("======================================\n");
+  for (const auto &seg : error_rpe) {
+    PRINT_INFO("seg %d - median_ori = %.3f | median_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.median,
+               seg.second.second.median, (int)seg.second.second.values.size());
+    // PRINT_DEBUG("seg %d - std_ori  = %.3f | std_pos  = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std);
+  }
+
+#ifdef HAVE_PYTHONLIBS
+
+  // Parameters
+  std::map<std::string, std::string> params_rpe;
+  params_rpe.insert({"notch", "true"});
+  params_rpe.insert({"sym", ""});
+
+  // Plot this figure
+  matplotlibcpp::figure_size(800, 600);
+
+  // Plot each RPE next to each other
+  double ct = 1;
+  double width = 0.50;
+  std::vector<double> xticks;
+  std::vector<std::string> labels;
+  for (const auto &seg : error_rpe) {
+    xticks.push_back(ct);
+    labels.push_back(std::to_string((int)seg.first));
+    matplotlibcpp::boxplot(seg.second.first.values, ct++, width, "blue", "-", params_rpe);
+  }
+
+  // Display to the user
+  matplotlibcpp::xlim(0.5, ct - 0.5);
+  matplotlibcpp::xticks(xticks, labels);
+  matplotlibcpp::title("Relative Orientation Error");
+  matplotlibcpp::ylabel("orientation error (deg)");
+  matplotlibcpp::xlabel("sub-segment lengths (m)");
+  matplotlibcpp::show(false);
+
+  // Plot this figure
+  matplotlibcpp::figure_size(800, 600);
+
+  // Plot each RPE next to each other
+  ct = 1;
+  for (const auto &seg : error_rpe) {
+    matplotlibcpp::boxplot(seg.second.second.values, ct++, width, "blue", "-", params_rpe);
+  }
+
+  // Display to the user
+  matplotlibcpp::xlim(0.5, ct - 0.5);
+  matplotlibcpp::xticks(xticks, labels);
+  matplotlibcpp::title("Relative Position Error");
+  matplotlibcpp::ylabel("translation error (m)");
+  matplotlibcpp::xlabel("sub-segment lengths (m)");
+  matplotlibcpp::show(false);
+
+#endif
+
+  //===========================================================
+  // Normalized Estimation Error Squared
+  //===========================================================
+
+  // Calculate
+  ov_eval::Statistics nees_ori, nees_pos;
+  traj.calculate_nees(nees_ori, nees_pos);
+
+  // Print it
+  PRINT_INFO("======================================\n");
+  PRINT_INFO("Normalized Estimation Error Squared\n");
+  PRINT_INFO("======================================\n");
+  PRINT_INFO("mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean);
+  PRINT_INFO("min_ori  = %.3f | min_pos  = %.3f\n", nees_ori.min, nees_pos.min);
+  PRINT_INFO("max_ori  = %.3f | max_pos  = %.3f\n", nees_ori.max, nees_pos.max);
+  PRINT_INFO("std_ori  = %.3f | std_pos  = %.3f\n", nees_ori.std, nees_pos.std);
+  PRINT_INFO("======================================\n");
+
+#ifdef HAVE_PYTHONLIBS
+
+  if (!nees_ori.values.empty() && !nees_pos.values.empty()) {
+    // Zero our time arrays
+    double starttime1 = (nees_ori.timestamps.empty()) ? 0 : nees_ori.timestamps.at(0);
+    double endtime1 = (nees_ori.timestamps.empty()) ? 0 : nees_ori.timestamps.at(nees_ori.timestamps.size() - 1);
+    for (size_t i = 0; i < nees_ori.timestamps.size(); i++) {
+      nees_ori.timestamps.at(i) -= starttime1;
+      nees_pos.timestamps.at(i) -= starttime1;
+    }
+
+    // Plot this figure
+    matplotlibcpp::figure_size(1000, 600);
+
+    // Parameters that define the line styles
+    std::map<std::string, std::string> params_neesp, params_neeso;
+    params_neesp.insert({"label", "nees position"});
+    params_neesp.insert({"linestyle", "-"});
+    params_neesp.insert({"color", "blue"});
+    params_neeso.insert({"label", "nees orientation"});
+    params_neeso.insert({"linestyle", "-"});
+    params_neeso.insert({"color", "blue"});
+
+    // Update the title and axis labels
+    matplotlibcpp::subplot(2, 1, 1);
+    matplotlibcpp::title("Normalized Estimation Error Squared");
+    matplotlibcpp::ylabel("NEES Orientation");
+    matplotlibcpp::plot(nees_ori.timestamps, nees_ori.values, params_neeso);
+    matplotlibcpp::xlim(0.0, endtime1 - starttime1);
+    matplotlibcpp::subplot(2, 1, 2);
+    matplotlibcpp::ylabel("NEES Position");
+    matplotlibcpp::xlabel("dataset time (s)");
+    matplotlibcpp::plot(nees_pos.timestamps, nees_pos.values, params_neesp);
+    matplotlibcpp::xlim(0.0, endtime1 - starttime1);
+
+    // Display to the user
+    matplotlibcpp::tight_layout();
+    matplotlibcpp::show(false);
+  }
+
+#endif
+
+  //===========================================================
+  // Plot the error if we have matplotlib to plot!
+  //===========================================================
+
+  // Calculate
+  ov_eval::Statistics posx, posy, posz;
+  ov_eval::Statistics orix, oriy, oriz;
+  ov_eval::Statistics roll, pitch, yaw;
+  traj.calculate_error(posx, posy, posz, orix, oriy, oriz, roll, pitch, yaw);
+
+  // Zero our time arrays
+  double starttime2 = (posx.timestamps.empty()) ? 0 : posx.timestamps.at(0);
+  double endtime2 = (posx.timestamps.empty()) ? 0 : posx.timestamps.at(posx.timestamps.size() - 1);
+  for (size_t i = 0; i < posx.timestamps.size(); i++) {
+    posx.timestamps.at(i) -= starttime2;
+    posy.timestamps.at(i) -= starttime2;
+    posz.timestamps.at(i) -= starttime2;
+    orix.timestamps.at(i) -= starttime2;
+    oriy.timestamps.at(i) -= starttime2;
+    oriz.timestamps.at(i) -= starttime2;
+    roll.timestamps.at(i) -= starttime2;
+    pitch.timestamps.at(i) -= starttime2;
+    yaw.timestamps.at(i) -= starttime2;
+  }
+
+#ifdef HAVE_PYTHONLIBS
+
+  //=====================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(1000, 600);
+  plot_3errors(posx, posy, posz);
+
+  // Update the title and axis labels
+  matplotlibcpp::subplot(3, 1, 1);
+  matplotlibcpp::title("IMU Position Error");
+  matplotlibcpp::ylabel("x-error (m)");
+  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
+  matplotlibcpp::subplot(3, 1, 2);
+  matplotlibcpp::ylabel("y-error (m)");
+  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
+  matplotlibcpp::subplot(3, 1, 3);
+  matplotlibcpp::ylabel("z-error (m)");
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
+
+  // Display to the user
+  matplotlibcpp::tight_layout();
+  matplotlibcpp::show(false);
+
+  //=====================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(1000, 600);
+  plot_3errors(orix, oriy, oriz);
+
+  // Update the title and axis labels
+  matplotlibcpp::subplot(3, 1, 1);
+  matplotlibcpp::title("IMU Orientation Error");
+  matplotlibcpp::ylabel("x-error (deg)");
+  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
+  matplotlibcpp::subplot(3, 1, 2);
+  matplotlibcpp::ylabel("y-error (deg)");
+  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
+  matplotlibcpp::subplot(3, 1, 3);
+  matplotlibcpp::ylabel("z-error (deg)");
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
+
+  // Display to the user
+  matplotlibcpp::tight_layout();
+  matplotlibcpp::show(false);
+
+  //=====================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(1000, 600);
+  plot_3errors(roll, pitch, yaw);
+
+  // Update the title and axis labels
+  matplotlibcpp::subplot(3, 1, 1);
+  matplotlibcpp::title("Global Orientation RPY Error");
+  matplotlibcpp::ylabel("roll error (deg)");
+  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
+  matplotlibcpp::subplot(3, 1, 2);
+  matplotlibcpp::ylabel("pitch error (deg)");
+  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
+  matplotlibcpp::subplot(3, 1, 3);
+  matplotlibcpp::ylabel("yaw error (deg)");
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
+
+  // Display to the user
+  matplotlibcpp::tight_layout();
+  matplotlibcpp::show(true);
+
+#endif
+
+  // Done!
+  return EXIT_SUCCESS;
+}

ov_eval/src/format_converter.cpp

@@ -0,0 +1,161 @@
+/*
+ * 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 <fstream>
+#include <iostream>
+#include <sstream>
+#include <string>
+
+#include "utils/colors.h"
+#include "utils/print.h"
+
+/**
+ * Given a CSV file this will convert it to our text file format.
+ */
+void process_csv(std::string infile) {
+
+  // Verbosity setting
+  ov_core::Printer::setPrintLevel("INFO");
+
+  // Check if file paths are good
+  std::ifstream file1;
+  std::string line;
+  file1.open(infile);
+  PRINT_INFO("Opening file %s\n", boost::filesystem::path(infile).filename().c_str());
+
+  // Check that it was successful
+  if (!file1) {
+    PRINT_ERROR(RED "ERROR: Unable to open input file...\n" RESET);
+    PRINT_ERROR(RED "ERROR: %s\n" RESET, infile.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Loop through each line of this file
+  std::vector<Eigen::VectorXd> traj_data;
+  std::string current_line;
+  while (std::getline(file1, current_line)) {
+
+    // Skip if we start with a comment
+    if (!current_line.find("#"))
+      continue;
+
+    // Loop variables
+    int i = 0;
+    std::istringstream s(current_line);
+    std::string field;
+    Eigen::Matrix<double, 8, 1> data;
+
+    // Loop through this line (timestamp(ns) tx ty tz qw qx qy qz)
+    while (std::getline(s, field, ',')) {
+      // Skip if empty
+      if (field.empty() || i >= data.rows())
+        continue;
+      // save the data to our vector
+      data(i) = std::atof(field.c_str());
+      i++;
+    }
+
+    // Only a valid line if we have all the parameters
+    if (i > 7) {
+      traj_data.push_back(data);
+      // std::stringstream ss;
+      // ss << std::setprecision(5) << data.transpose() << std::endl;
+      // PRINT_DEBUG(ss.str().c_str());
+    }
+  }
+
+  // Finally close the file
+  file1.close();
+
+  // Error if we don't have any data
+  if (traj_data.empty()) {
+    PRINT_ERROR(RED "ERROR: Could not parse any data from the file!!\n" RESET);
+    PRINT_ERROR(RED "ERROR: %s\n" RESET, infile.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+  PRINT_INFO("\t- Loaded %d poses from file\n", (int)traj_data.size());
+
+  // If file exists already then crash
+  std::string outfile = infile.substr(0, infile.find_last_of('.')) + ".txt";
+  if (boost::filesystem::exists(outfile)) {
+    PRINT_ERROR(RED "\t- ERROR: Output file already exists, please delete and re-run this script!!\n" RESET);
+    PRINT_ERROR(RED "\t- ERROR: %s\n" RESET, outfile.c_str());
+    return;
+  }
+
+  // Open this file we want to write to
+  std::ofstream file2;
+  file2.open(outfile.c_str());
+  if (file2.fail()) {
+    PRINT_ERROR(RED "ERROR: Unable to open output file!!\n" RESET);
+    PRINT_ERROR(RED "ERROR: %s\n" RESET, outfile.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+  file2 << "# timestamp(s) tx ty tz qx qy qz qw" << std::endl;
+
+  // Write to disk in the correct order!
+  for (size_t i = 0; i < traj_data.size(); i++) {
+    file2.precision(5);
+    file2.setf(std::ios::fixed, std::ios::floatfield);
+    file2 << 1e-9 * traj_data.at(i)(0) << " ";
+    file2.precision(6);
+    file2 << traj_data.at(i)(1) << " " << traj_data.at(i)(2) << " " << traj_data.at(i)(3) << " " << traj_data.at(i)(5) << " "
+          << traj_data.at(i)(6) << " " << traj_data.at(i)(7) << " " << traj_data.at(i)(4) << std::endl;
+  }
+  PRINT_INFO("\t- Saved to file %s\n", boost::filesystem::path(outfile).filename().c_str());
+
+  // Finally close the file
+  file2.close();
+}
+
+int main(int argc, char **argv) {
+
+  // Ensure we have a path
+  if (argc < 2) {
+    PRINT_ERROR(RED "ERROR: Please specify a file to convert\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./format_convert <file.csv or folder\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval format_convert <file.csv or folder>\n" RESET);
+    std::exit(EXIT_FAILURE);
+  }
+
+  // If we do not have a wildcard, then process this one csv
+  if (boost::algorithm::ends_with(argv[1], "csv")) {
+
+    // Process this single file
+    process_csv(argv[1]);
+
+  } else {
+
+    // Loop through this directory
+    boost::filesystem::path infolder(argv[1]);
+    for (auto &p : boost::filesystem::recursive_directory_iterator(infolder)) {
+      if (p.path().extension() == ".csv") {
+        process_csv(p.path().string());
+      }
+    }
+  }
+
+  // Done!
+  return EXIT_SUCCESS;
+}

ov_eval/src/live_align_trajectory.cpp

@@ -0,0 +1,150 @@
+/*
+ * 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 <geometry_msgs/PoseStamped.h>
+#include <nav_msgs/Path.h>
+#include <ros/ros.h>
+
+#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"
+
+ros::Publisher pub_path;
+void align_and_publish(const nav_msgs::Path::ConstPtr &msg);
+std::vector<double> times_gt;
+std::vector<Eigen::Matrix<double, 7, 1>> poses_gt;
+std::string alignment_type;
+
+int main(int argc, char **argv) {
+
+  // Create ros node
+  ros::init(argc, argv, "live_align_trajectory");
+  ros::NodeHandle nh("~");
+
+  // Verbosity setting
+  std::string verbosity;
+  nh.param<std::string>("verbosity", verbosity, "INFO");
+  ov_core::Printer::setPrintLevel(verbosity);
+
+  // Load what type of alignment we should use
+  nh.param<std::string>("alignment_type", alignment_type, "posyaw");
+
+  // If we don't have it, or it is empty then error
+  if (!nh.hasParam("path_gt")) {
+    ROS_ERROR("[LOAD]: Please provide a groundtruth file path!!!");
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Load our groundtruth from file
+  std::string path_to_gt;
+  nh.param<std::string>("path_gt", path_to_gt, "");
+  boost::filesystem::path infolder(path_to_gt);
+  if (infolder.extension() == ".csv") {
+    std::vector<Eigen::Matrix3d> cov_ori_temp, cov_pos_temp;
+    ov_eval::Loader::load_data_csv(path_to_gt, times_gt, poses_gt, cov_ori_temp, cov_pos_temp);
+  } else {
+    std::vector<Eigen::Matrix3d> cov_ori_temp, cov_pos_temp;
+    ov_eval::Loader::load_data(path_to_gt, times_gt, poses_gt, cov_ori_temp, cov_pos_temp);
+  }
+
+  // Our subscribe and publish nodes
+  ros::Subscriber sub = nh.subscribe("/ov_msckf/pathimu", 1, align_and_publish);
+  pub_path = nh.advertise<nav_msgs::Path>("/ov_msckf/pathgt", 2);
+  ROS_INFO("Subscribing: %s", sub.getTopic().c_str());
+  ROS_INFO("Publishing: %s", pub_path.getTopic().c_str());
+
+  // Spin
+  ros::spin();
+}
+
+void align_and_publish(const nav_msgs::Path::ConstPtr &msg) {
+
+  // Convert the message into correct vector format
+  // tx ty tz qx qy qz qw
+  std::vector<double> times_temp;
+  std::vector<Eigen::Matrix<double, 7, 1>> poses_temp;
+  for (auto const &pose : msg->poses) {
+//    ROS_INFO("the received timestamp from the estimated nav_msgs::Path::ConstPtr &msg is: %f", pose.header.stamp.toSec());
+    times_temp.push_back(pose.header.stamp.toSec());
+    Eigen::Matrix<double, 7, 1> pose_tmp;
+    pose_tmp << pose.pose.position.x, pose.pose.position.y, pose.pose.position.z, pose.pose.orientation.x, pose.pose.orientation.y,
+        pose.pose.orientation.z, pose.pose.orientation.w;
+    poses_temp.push_back(pose_tmp);
+  }
+
+//  for (int i=0;i<poses_gt.size();i++){
+//      ROS_INFO("the loaded timestamp from gt file: %f %f %f %f %f %f %f", poses_gt[i][0], poses_gt[i][1], poses_gt[i][2], poses_gt[i][3],
+//               poses_gt[i][4], poses_gt[i][5], poses_gt[i][6] );
+//        ROS_INFO("the gt timestamp:  %f", times_gt[i]);
+//  }
+
+  // Intersect timestamps
+  std::vector<double> gt_times_temp = times_gt;
+  std::vector<Eigen::Matrix<double, 7, 1>> gt_poses_temp = poses_gt;
+  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);
+    return;
+  }
+
+  // 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, alignment_type);
+  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
+  // TODO: maybe in the future we could live publish trajectory errors...
+  // NOTE: We downsample the number of poses as needed to prevent rviz crashes
+  // NOTE: https://github.com/ros-visualization/rviz/issues/1107
+  nav_msgs::Path arr_groundtruth;
+  arr_groundtruth.header = msg->header;
+  for (size_t i = 0; i < gt_times_temp.size(); i += std::floor(gt_times_temp.size() / 16384.0) + 1) {
+
+    // Convert into the correct frame
+    double timestamp = gt_times_temp.at(i);
+    Eigen::Matrix<double, 7, 1> pose_inGT = gt_poses_temp.at(i);
+    Eigen::Vector3d pos_IinEST = R_ESTtoGT.transpose() * (pose_inGT.block(0, 0, 3, 1) - t_ESTinGT) / s_ESTtoGT;
+    Eigen::Vector4d quat_ESTtoI = ov_core::quat_multiply(pose_inGT.block(3, 0, 4, 1), q_ESTtoGT);
+    // Finally push back
+    geometry_msgs::PoseStamped posetemp;
+    posetemp.header = msg->header;
+    posetemp.header.stamp = ros::Time(timestamp);
+    posetemp.pose.orientation.x = quat_ESTtoI(0);
+    posetemp.pose.orientation.y = quat_ESTtoI(1);
+    posetemp.pose.orientation.z = quat_ESTtoI(2);
+    posetemp.pose.orientation.w = quat_ESTtoI(3);
+    posetemp.pose.position.x = pos_IinEST(0);
+    posetemp.pose.position.y = pos_IinEST(1);
+    posetemp.pose.position.z = pos_IinEST(2);
+    arr_groundtruth.poses.push_back(posetemp);
+  }
+  pub_path.publish(arr_groundtruth);
+}

ov_eval/src/plot_trajectories.cpp

@@ -0,0 +1,210 @@
+/*
+ * 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> &times,
+                      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;
+}

ov_eval/src/pose_to_file.cpp

@@ -0,0 +1,81 @@
+/*
+ * 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 <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+
+#include "utils/Recorder.h"
+#include "utils/print.h"
+
+int main(int argc, char **argv) {
+
+  // Create ros node
+  ros::init(argc, argv, "pose_to_file");
+  ros::NodeHandle nh("~");
+
+  // Verbosity setting
+  std::string verbosity;
+  nh.param<std::string>("verbosity", verbosity, "DEBUG");
+  ov_core::Printer::setPrintLevel(verbosity);
+
+  // Get parameters to subscribe
+  std::string topic, topic_type, fileoutput;
+  nh.getParam("topic", topic);
+  nh.getParam("topic_type", topic_type);
+  nh.getParam("output", fileoutput);
+
+  // Debug
+  PRINT_DEBUG("Done reading config values");
+  PRINT_DEBUG(" - topic = %s", topic.c_str());
+  PRINT_DEBUG(" - topic_type = %s", topic_type.c_str());
+  PRINT_DEBUG(" - file = %s", fileoutput.c_str());
+
+  // Create the recorder object
+  ov_eval::Recorder recorder(fileoutput);
+  PRINT_DEBUG("Opened the recorder with the file: %s", fileoutput.c_str());
+  ROS_INFO("Opened the recorder with the file: %s", fileoutput.c_str());
+
+  // Subscribe to topic
+  ros::Subscriber sub;
+  if (topic_type == std::string("PoseWithCovarianceStamped")) {
+    sub = nh.subscribe(topic, 9999, &ov_eval::Recorder::callback_posecovariance, &recorder);
+  } else if (topic_type == std::string("PoseStamped")) {
+    sub = nh.subscribe(topic, 9999, &ov_eval::Recorder::callback_pose, &recorder);
+//    PRINT_DEBUG("The PoseStamped topic type has been chosen");
+    ROS_INFO("The PoseStamped topic type has been chosen");
+  } else if (topic_type == std::string("TransformStamped")) {
+    sub = nh.subscribe(topic, 9999, &ov_eval::Recorder::callback_transform, &recorder);
+  } else if (topic_type == std::string("Odometry")) {
+    sub = nh.subscribe(topic, 9999, &ov_eval::Recorder::callback_odometry, &recorder);
+  } else {
+    PRINT_ERROR("The specified topic type is not supported");
+    PRINT_ERROR("topic_type = %s", topic_type.c_str());
+    PRINT_ERROR("please select from: PoseWithCovarianceStamped, PoseStamped, TransformStamped, Odometry");
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Done!
+  ros::spin();
+  return EXIT_SUCCESS;
+}

ov_eval/src/timing_comparison.cpp

@@ -0,0 +1,167 @@
+/*
+ * 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 "utils/Loader.h"
+#include "utils/Statistics.h"
+#include "utils/colors.h"
+#include "utils/print.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"
+
+#endif
+
+int main(int argc, char **argv) {
+
+  // Verbosity setting
+  ov_core::Printer::setPrintLevel("INFO");
+
+  // Ensure we have a path
+  if (argc < 2) {
+    PRINT_ERROR(RED "ERROR: Please specify a timing file\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./timing_comparison <file_times1.txt> ... <file_timesN.txt>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval timing_comparison <file_times1.txt> ... <file_timesN.txt>\n" RESET);
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Read in all our trajectories from file
+  std::vector<std::string> names;
+  std::vector<ov_eval::Statistics> total_times;
+  PRINT_INFO("======================================\n");
+  for (int z = 1; z < argc; z++) {
+
+    // Parse the name of this timing
+    boost::filesystem::path path(argv[z]);
+    std::string name = path.stem().string();
+    PRINT_INFO("[TIME]: loading data for %s\n", name.c_str());
+
+    // Load it!!
+    std::vector<std::string> names_temp;
+    std::vector<double> times;
+    std::vector<Eigen::VectorXd> timing_values;
+    ov_eval::Loader::load_timing_flamegraph(argv[z], names_temp, times, timing_values);
+    PRINT_DEBUG("[TIME]: loaded %d timestamps from file (%d categories)!!\n", (int)times.size(), (int)names_temp.size());
+
+    // Our categories
+    std::vector<ov_eval::Statistics> stats;
+    for (size_t i = 0; i < names_temp.size(); i++)
+      stats.push_back(ov_eval::Statistics());
+
+    // Loop through each and report the average timing information
+    for (size_t i = 0; i < times.size(); i++) {
+      for (size_t c = 0; c < names_temp.size(); c++) {
+        stats.at(c).timestamps.push_back(times.at(i));
+        stats.at(c).values.push_back(timing_values.at(i)(c));
+      }
+    }
+
+    // Now print the statistic for this run
+    for (size_t i = 0; i < names_temp.size(); i++) {
+      stats.at(i).calculate();
+      PRINT_INFO("mean_time = %.4f | std = %.4f | 99th = %.4f  | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std,
+                 stats.at(i).ninetynine, stats.at(i).max, names_temp.at(i).c_str());
+    }
+
+    // Append the total stats to the big vector
+    if (!stats.empty()) {
+      names.push_back(name);
+      total_times.push_back(stats.at(stats.size() - 1));
+    } else {
+      PRINT_ERROR(RED "[TIME]: unable to load any data.....\n" RESET);
+    }
+    PRINT_INFO("======================================\n");
+  }
+
+#ifdef HAVE_PYTHONLIBS
+
+  // Valid colors
+  // https://matplotlib.org/tutorials/colors/colors.html
+  // std::vector<std::string> colors = {"blue","aqua","lightblue","lightgreen","yellowgreen","green"};
+  // std::vector<std::string> colors = {"navy","blue","lightgreen","green","gold","goldenrod"};
+  std::vector<std::string> colors = {"black", "blue", "red", "green", "cyan", "magenta"};
+
+  // Plot this figure
+  matplotlibcpp::figure_size(1200, 400);
+
+  // Zero our time arrays
+  double starttime = (total_times.at(0).timestamps.empty()) ? 0 : total_times.at(0).timestamps.at(0);
+  double endtime = (total_times.at(0).timestamps.empty()) ? 0 : total_times.at(0).timestamps.at(total_times.at(0).timestamps.size() - 1);
+  for (size_t i = 0; i < total_times.size(); i++) {
+    for (size_t j = 0; j < total_times.at(i).timestamps.size(); j++) {
+      total_times.at(i).timestamps.at(j) -= starttime;
+    }
+  }
+
+  // Now loop through each and plot it!
+  for (size_t n = 0; n < names.size(); n++) {
+
+    // Sub-sample the time and values
+    int keep_every = 10;
+    std::vector<double> times_skipped;
+    for (size_t t = 0; t < total_times.at(n).timestamps.size(); t++) {
+      if (t % keep_every == 0) {
+        times_skipped.push_back(total_times.at(n).timestamps.at(t));
+      }
+    }
+    std::vector<double> values_skipped;
+    for (size_t t = 0; t < total_times.at(n).values.size(); t++) {
+      if (t % keep_every == 0) {
+        values_skipped.push_back(total_times.at(n).values.at(t));
+      }
+    }
+
+    // Paramters for our line
+    std::map<std::string, std::string> params;
+    params.insert({"label", names.at(n)});
+    params.insert({"linestyle", "-"});
+    params.insert({"color", colors.at(n % colors.size())});
+
+    // Finally plot
+    matplotlibcpp::plot(times_skipped, values_skipped, params);
+  }
+
+  // Finally add labels and show it
+  matplotlibcpp::ylabel("execution time (s)");
+  matplotlibcpp::xlim(0.0, endtime - starttime);
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::legend();
+  matplotlibcpp::tight_layout();
+
+  // Display to the user
+  matplotlibcpp::show(true);
+
+#endif
+
+  // Done!
+  return EXIT_SUCCESS;
+}

ov_eval/src/timing_flamegraph.cpp

@@ -0,0 +1,142 @@
+/*
+ * 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 "utils/Loader.h"
+#include "utils/Statistics.h"
+#include "utils/colors.h"
+#include "utils/print.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"
+
+#endif
+
+int main(int argc, char **argv) {
+
+  // Verbosity setting
+  ov_core::Printer::setPrintLevel("INFO");
+
+  // Ensure we have a path
+  if (argc < 2) {
+    PRINT_ERROR(RED "ERROR: Please specify a timing file\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./timing_flamegraph <file_times.txt>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval timing_flamegraph <file_times.txt>\n" RESET);
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Load it!!
+  std::vector<std::string> names;
+  std::vector<double> times;
+  std::vector<Eigen::VectorXd> timing_values;
+  ov_eval::Loader::load_timing_flamegraph(argv[1], names, times, timing_values);
+  PRINT_INFO("[TIME]: loaded %d timestamps from file (%d categories)!!\n", (int)times.size(), (int)names.size());
+
+  // Our categories
+  std::vector<ov_eval::Statistics> stats;
+  for (size_t i = 0; i < names.size(); i++)
+    stats.push_back(ov_eval::Statistics());
+
+  // Loop through each and report the average timing information
+  for (size_t i = 0; i < times.size(); i++) {
+    for (size_t c = 0; c < names.size(); c++) {
+      stats.at(c).timestamps.push_back(times.at(i));
+      stats.at(c).values.push_back(timing_values.at(i)(c));
+    }
+  }
+
+  // Now print the statistic for this run
+  for (size_t i = 0; i < names.size(); i++) {
+    stats.at(i).calculate();
+    PRINT_INFO("mean_time = %.4f | std = %.4f | 99th = %.4f  | max = %.4f (%s)\n", stats.at(i).mean, stats.at(i).std,
+               stats.at(i).ninetynine, stats.at(i).max, names.at(i).c_str());
+  }
+
+#ifdef HAVE_PYTHONLIBS
+
+  // Sub-sample the time
+  int keep_every = 10;
+  std::vector<double> times_skipped;
+  for (size_t t = 0; t < times.size(); t++) {
+    if (t % keep_every == 0) {
+      times_skipped.push_back(times.at(t));
+    }
+  }
+
+  // Zero our time arrays
+  double starttime1 = (times_skipped.empty()) ? 0 : times_skipped.at(0);
+  double endtime1 = (times_skipped.empty()) ? 0 : times_skipped.at(times_skipped.size() - 1);
+  for (size_t j = 0; j < times_skipped.size(); j++) {
+    times_skipped.at(j) -= starttime1;
+  }
+
+  // Valid colors
+  // https://matplotlib.org/tutorials/colors/colors.html
+  // std::vector<std::string> colors_valid = {"blue","aqua","lightblue","lightgreen","yellowgreen","green"};
+  std::vector<std::string> colors_valid = {"navy", "blue", "lightgreen", "green", "gold", "goldenrod"};
+
+  // Create vector for each category
+  // NOTE we skip the last category since it is the "total" time by convention
+  std::vector<std::string> labels;
+  std::vector<std::string> colors;
+  std::vector<std::vector<double>> timings;
+  for (size_t i = 0; i < names.size() - 1; i++) {
+    labels.push_back(names.at(i));
+    colors.push_back(colors_valid.at(i % colors_valid.size()));
+    std::vector<double> values_skipped;
+    for (size_t t = 0; t < stats.at(i).values.size(); t++) {
+      if (t % keep_every == 0) {
+        values_skipped.push_back(stats.at(i).values.at(t));
+      }
+    }
+    timings.push_back(values_skipped);
+  }
+
+  // Plot this figure
+  matplotlibcpp::figure_size(1200, 400);
+  matplotlibcpp::stackplot(times_skipped, timings, labels, colors, "zero");
+  matplotlibcpp::ylabel("execution time (s)");
+  matplotlibcpp::xlim(0.0, endtime1 - starttime1);
+  // matplotlibcpp::ylim(0.0,stats.at(stats.size()-1).ninetynine);
+  matplotlibcpp::ylim(0.0, stats.at(stats.size() - 1).max);
+  matplotlibcpp::xlabel("dataset time (s)");
+  matplotlibcpp::legend();
+  matplotlibcpp::tight_layout();
+
+  // Display to the user
+  matplotlibcpp::show(true);
+
+#endif
+
+  // Done!
+  return EXIT_SUCCESS;
+}

ov_eval/src/timing_percentages.cpp

@@ -0,0 +1,238 @@
+/*
+ * 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 "utils/Loader.h"
+#include "utils/Statistics.h"
+#include "utils/colors.h"
+#include "utils/print.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"
+
+#endif
+
+int main(int argc, char **argv) {
+
+  // Verbosity setting
+  ov_core::Printer::setPrintLevel("ALL");
+
+  // Ensure we have a path
+  if (argc < 2) {
+    PRINT_ERROR(RED "ERROR: Please specify a timing and memory percent folder\n" RESET);
+    PRINT_ERROR(RED "ERROR: ./timing_percentages <timings_folder>\n" RESET);
+    PRINT_ERROR(RED "ERROR: rosrun ov_eval timing_percentages <timings_folder>\n" RESET);
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Get the algorithms we will process
+  // Also create empty statistic objects for each of our datasets
+  std::string path_algos(argv[1]);
+  std::vector<boost::filesystem::path> path_algorithms;
+  for (const auto &entry : boost::filesystem::directory_iterator(path_algos)) {
+    if (boost::filesystem::is_directory(entry)) {
+      path_algorithms.push_back(entry.path());
+    }
+  }
+  std::sort(path_algorithms.begin(), path_algorithms.end());
+
+  //===============================================================================
+  //===============================================================================
+  //===============================================================================
+
+  // Summary information (%cpu, %mem, threads)
+  std::map<std::string, std::vector<ov_eval::Statistics>> algo_timings;
+  for (const auto &p : path_algorithms) {
+    std::vector<ov_eval::Statistics> temp = {ov_eval::Statistics(), ov_eval::Statistics(), ov_eval::Statistics()};
+    algo_timings.insert({p.stem().string(), temp});
+  }
+
+  // Loop through each algorithm type
+  for (size_t i = 0; i < path_algorithms.size(); i++) {
+
+    // Debug print
+    PRINT_DEBUG("======================================\n");
+    PRINT_DEBUG("[COMP]: processing %s algorithm\n", path_algorithms.at(i).stem().c_str());
+
+    // our total summed values
+    std::vector<double> total_times;
+    std::vector<Eigen::Vector3d> total_summed_values;
+
+    // Loop through each sub-directory in this folder
+    for (auto &entry : boost::filesystem::recursive_directory_iterator(path_algorithms.at(i))) {
+
+      // skip if not a directory
+      if (boost::filesystem::is_directory(entry))
+        continue;
+
+      // skip if not a text file
+      if (entry.path().extension() != ".txt")
+        continue;
+
+      // Load the data from file
+      std::vector<double> times;
+      std::vector<Eigen::Vector3d> summed_values;
+      std::vector<Eigen::VectorXd> node_values;
+      ov_eval::Loader::load_timing_percent(entry.path().string(), times, summed_values, node_values);
+
+      // Append to our summed values
+      total_times.insert(total_times.end(), times.begin(), times.end());
+      total_summed_values.insert(total_summed_values.end(), summed_values.begin(), summed_values.end());
+    }
+
+    // append to the map
+    std::string algo = path_algorithms.at(i).stem().string();
+    for (size_t j = 0; j < total_times.size(); j++) {
+      algo_timings.at(algo).at(0).timestamps.push_back(total_times.at(j));
+      algo_timings.at(algo).at(0).values.push_back(total_summed_values.at(j)(0));
+      algo_timings.at(algo).at(1).timestamps.push_back(total_times.at(j));
+      algo_timings.at(algo).at(1).values.push_back(total_summed_values.at(j)(1));
+      algo_timings.at(algo).at(2).timestamps.push_back(total_times.at(j));
+      algo_timings.at(algo).at(2).values.push_back(total_summed_values.at(j)(2));
+    }
+
+    // Display for the user
+    PRINT_DEBUG("\tloaded %d timestamps from file!!\n", (int)algo_timings.at(algo).at(0).timestamps.size());
+    algo_timings.at(algo).at(0).calculate();
+    algo_timings.at(algo).at(1).calculate();
+    algo_timings.at(algo).at(2).calculate();
+    PRINT_DEBUG("\tPREC: mean_cpu = %.3f +- %.3f\n", algo_timings.at(algo).at(0).mean, algo_timings.at(algo).at(0).std);
+    PRINT_DEBUG("\tPREC: mean_mem = %.3f +- %.3f\n", algo_timings.at(algo).at(1).mean, algo_timings.at(algo).at(1).std);
+    PRINT_DEBUG("\tTHR: mean_threads = %.3f +- %.3f\n", algo_timings.at(algo).at(2).mean, algo_timings.at(algo).at(2).std);
+    PRINT_DEBUG("======================================\n");
+  }
+
+  //===============================================================================
+  //===============================================================================
+  //===============================================================================
+
+#ifdef HAVE_PYTHONLIBS
+
+  // Plot line colors
+  std::vector<std::string> colors = {"blue", "red", "black", "green", "cyan", "magenta"};
+  std::vector<std::string> linestyle = {"-", "--", "-."};
+  assert(algo_timings.size() <= colors.size() * linestyle.size());
+
+  // Parameters
+  std::map<std::string, std::string> params_rpe;
+  params_rpe.insert({"notch", "false"});
+  params_rpe.insert({"sym", ""});
+
+  //============================================================
+  //============================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(1500, 400);
+
+  // Plot each RPE next to each other
+  double width = 0.1 / (algo_timings.size() + 1);
+  std::vector<double> yticks;
+  std::vector<std::string> labels;
+  int ct_algo = 0;
+  double ct_pos = 0;
+  for (auto &algo : algo_timings) {
+    // Start based on what algorithm we are doing
+    ct_pos = 1 + 1.5 * ct_algo * width;
+    yticks.push_back(ct_pos);
+    labels.push_back(algo.first);
+    // Plot it!!!
+    matplotlibcpp::boxplot(algo.second.at(0).values, ct_pos, width, colors.at(ct_algo % colors.size()),
+                           linestyle.at(ct_algo / colors.size()), params_rpe, false);
+    // Move forward
+    ct_algo++;
+  }
+
+  // Add "fake" plots for our legend
+  ct_algo = 0;
+  for (const auto &algo : algo_timings) {
+    std::map<std::string, std::string> params_empty;
+    params_empty.insert({"label", algo.first});
+    params_empty.insert({"linestyle", linestyle.at(ct_algo / colors.size())});
+    params_empty.insert({"color", colors.at(ct_algo % colors.size())});
+    std::vector<double> vec_empty;
+    matplotlibcpp::plot(vec_empty, vec_empty, params_empty);
+    ct_algo++;
+  }
+
+  // Display to the user
+  matplotlibcpp::ylim(1.0 - 1 * width, ct_pos + 1 * width);
+  matplotlibcpp::yticks(yticks, labels);
+  matplotlibcpp::xlabel("CPU Percent Usage");
+  matplotlibcpp::tight_layout();
+  matplotlibcpp::show(false);
+
+  //============================================================
+  //============================================================
+  // Plot this figure
+  matplotlibcpp::figure_size(1500, 400);
+
+  // Plot each RPE next to each other
+  width = 0.1 / (algo_timings.size() + 1);
+  yticks.clear();
+  labels.clear();
+  ct_algo = 0;
+  ct_pos = 0;
+  for (auto &algo : algo_timings) {
+    // Start based on what algorithm we are doing
+    ct_pos = 1 + 1.5 * ct_algo * width;
+    yticks.push_back(ct_pos);
+    labels.push_back(algo.first);
+    // Plot it!!!
+    matplotlibcpp::boxplot(algo.second.at(1).values, ct_pos, width, colors.at(ct_algo % colors.size()),
+                           linestyle.at(ct_algo / colors.size()), params_rpe, false);
+    // Move forward
+    ct_algo++;
+  }
+
+  // Add "fake" plots for our legend
+  ct_algo = 0;
+  for (const auto &algo : algo_timings) {
+    std::map<std::string, std::string> params_empty;
+    params_empty.insert({"label", algo.first});
+    params_empty.insert({"linestyle", linestyle.at(ct_algo / colors.size())});
+    params_empty.insert({"color", colors.at(ct_algo % colors.size())});
+    std::vector<double> vec_empty;
+    matplotlibcpp::plot(vec_empty, vec_empty, params_empty);
+    ct_algo++;
+  }
+
+  // Display to the user
+  matplotlibcpp::ylim(1.0 - 1 * width, ct_pos + 1 * width);
+  matplotlibcpp::yticks(yticks, labels);
+  matplotlibcpp::xlabel("Memory Percent Usage");
+  matplotlibcpp::tight_layout();
+  matplotlibcpp::show(true);
+
+#endif
+
+  // Done!
+  return EXIT_SUCCESS;
+}

ov_eval/src/utils/Loader.cpp

@@ -0,0 +1,398 @@
+/*
+ * 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 "Loader.h"
+
+using namespace ov_eval;
+
+void Loader::load_data(std::string path_traj, std::vector<double> &times, std::vector<Eigen::Matrix<double, 7, 1>> &poses,
+                       std::vector<Eigen::Matrix3d> &cov_ori, std::vector<Eigen::Matrix3d> &cov_pos) {
+
+  // Try to open our trajectory file
+  std::ifstream file(path_traj);
+  if (!file.is_open()) {
+    PRINT_ERROR(RED "[LOAD]: Unable to open trajectory file...\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Loop through each line of this file
+  std::string current_line;
+  while (std::getline(file, current_line)) {
+
+    // Skip if we start with a comment
+    if (!current_line.find("#"))
+      continue;
+
+    // Loop variables
+    int i = 0;
+    std::istringstream s(current_line);
+    std::string field;
+    Eigen::Matrix<double, 20, 1> data;
+
+    // Loop through this line (timestamp(s) tx ty tz qx qy qz qw Pr11 Pr12 Pr13 Pr22 Pr23 Pr33 Pt11 Pt12 Pt13 Pt22 Pt23 Pt33)
+    while (std::getline(s, field, ' ')) {
+      // Skip if empty
+      if (field.empty() || i >= data.rows())
+        continue;
+      // save the data to our vector
+      data(i) = std::atof(field.c_str());
+      i++;
+    }
+
+    // Only a valid line if we have all the parameters
+    if (i >= 20) {
+      // time and pose
+      times.push_back(data(0));
+      poses.push_back(data.block(1, 0, 7, 1));
+      // covariance values
+      Eigen::Matrix3d c_ori, c_pos;
+      c_ori << data(8), data(9), data(10), data(9), data(11), data(12), data(10), data(12), data(13);
+      c_pos << data(14), data(15), data(16), data(15), data(17), data(18), data(16), data(18), data(19);
+      c_ori = 0.5 * (c_ori + c_ori.transpose());
+      c_pos = 0.5 * (c_pos + c_pos.transpose());
+      cov_ori.push_back(c_ori);
+      cov_pos.push_back(c_pos);
+    } else if (i >= 8) {
+      times.push_back(data(0));
+      poses.push_back(data.block(1, 0, 7, 1));
+    }
+  }
+
+  // Finally close the file
+  file.close();
+
+  // Error if we don't have any data
+  if (times.empty()) {
+    PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Assert that they are all equal
+  if (times.size() != poses.size()) {
+    PRINT_ERROR(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Assert that they are all equal
+  if (!cov_ori.empty() && (times.size() != cov_ori.size() || times.size() != cov_pos.size())) {
+    PRINT_ERROR(RED "[LOAD]: Parsing error, timestamps covariance size do not match!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Debug print amount
+  // std::string base_filename = path_traj.substr(path_traj.find_last_of("/\\") + 1);
+  // PRINT_DEBUG("[LOAD]: loaded %d poses from %s\n",(int)poses.size(),base_filename.c_str());
+}
+
+void Loader::load_data_csv(std::string path_traj, std::vector<double> &times, std::vector<Eigen::Matrix<double, 7, 1>> &poses,
+                           std::vector<Eigen::Matrix3d> &cov_ori, std::vector<Eigen::Matrix3d> &cov_pos) {
+
+  // Try to open our trajectory file
+  std::ifstream file(path_traj);
+  if (!file.is_open()) {
+    PRINT_ERROR(RED "[LOAD]: Unable to open trajectory file...\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Loop through each line of this file
+  std::string current_line;
+  while (std::getline(file, current_line)) {
+
+    // Skip if we start with a comment
+    if (!current_line.find("#"))
+      continue;
+
+    // Loop variables
+    int i = 0;
+    std::istringstream s(current_line);
+    std::string field;
+    Eigen::Matrix<double, 20, 1> data;
+
+    // Loop through this line (groundtruth state [time(sec),q_GtoI,p_IinG,v_IinG,b_gyro,b_accel])
+    while (std::getline(s, field, ',')) {
+      // Skip if empty
+      if (field.empty() || i >= data.rows())
+        continue;
+      // save the data to our vector
+      data(i) = std::atof(field.c_str());
+      i++;
+    }
+
+    // Only a valid line if we have all the parameters
+    // Times are in nanoseconds -> convert to seconds
+    // Our "fixed" state vector from the ETH GT format [q,p,v,bg,ba]
+    if (i >= 8) {
+      times.push_back(1e-9 * data(0));
+      Eigen::Matrix<double, 7, 1> imustate;
+      imustate(0, 0) = data(1, 0); // pos
+      imustate(1, 0) = data(2, 0);
+      imustate(2, 0) = data(3, 0);
+      imustate(3, 0) = data(5, 0); // quat (xyzw)
+      imustate(4, 0) = data(6, 0);
+      imustate(5, 0) = data(7, 0);
+      imustate(6, 0) = data(4, 0);
+      poses.push_back(imustate);
+    }
+  }
+
+  // Finally close the file
+  file.close();
+
+  // Error if we don't have any data
+  if (times.empty()) {
+    PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Assert that they are all equal
+  if (times.size() != poses.size()) {
+    PRINT_ERROR(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path_traj.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+}
+
+void Loader::load_simulation(std::string path, std::vector<Eigen::VectorXd> &values) {
+
+  // Try to open our trajectory file
+  std::ifstream file(path);
+  if (!file.is_open()) {
+    PRINT_ERROR(RED "[LOAD]: Unable to open file...\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Loop through each line of this file
+  std::string current_line;
+  while (std::getline(file, current_line)) {
+
+    // Skip if we start with a comment
+    if (!current_line.find("#"))
+      continue;
+
+    // Loop variables
+    std::istringstream s(current_line);
+    std::string field;
+    std::vector<double> vec;
+
+    // Loop through this line (timestamp(s) values....)
+    while (std::getline(s, field, ' ')) {
+      // Skip if empty
+      if (field.empty())
+        continue;
+      // save the data to our vector
+      vec.push_back(std::atof(field.c_str()));
+    }
+
+    // Create eigen vector
+    Eigen::VectorXd temp(vec.size());
+    for (size_t i = 0; i < vec.size(); i++) {
+      temp(i) = vec.at(i);
+    }
+    values.push_back(temp);
+  }
+
+  // Finally close the file
+  file.close();
+
+  // Error if we don't have any data
+  if (values.empty()) {
+    PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Assert that all rows in this file are of the same length
+  int rowsize = values.at(0).rows();
+  for (size_t i = 0; i < values.size(); i++) {
+    if (values.at(i).rows() != rowsize) {
+      PRINT_ERROR(RED "[LOAD]: Invalid row size on line %d (of size %d instead of %d)\n" RESET, (int)i, (int)values.at(i).rows(), rowsize);
+      PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
+      std::exit(EXIT_FAILURE);
+    }
+  }
+}
+
+void Loader::load_timing_flamegraph(std::string path, std::vector<std::string> &names, std::vector<double> &times,
+                                    std::vector<Eigen::VectorXd> &timing_values) {
+
+  // Try to open our trajectory file
+  std::ifstream file(path);
+  if (!file.is_open()) {
+    PRINT_ERROR(RED "[LOAD]: Unable to open file...\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Loop through each line of this file
+  std::string current_line;
+  while (std::getline(file, current_line)) {
+
+    // We should have a commented line of the names of the categories
+    // Here we will process them (skip the first since it is just the timestamps)
+    if (!current_line.find("#")) {
+      // Loop variables
+      std::istringstream s(current_line);
+      std::string field;
+      names.clear();
+      // Loop through this line
+      bool skipped_first = false;
+      while (std::getline(s, field, ',')) {
+        // Skip if empty
+        if (field.empty())
+          continue;
+        // Skip the first ever one
+        if (skipped_first)
+          names.push_back(field);
+        skipped_first = true;
+      }
+      continue;
+    }
+
+    // Loop variables
+    std::istringstream s(current_line);
+    std::string field;
+    std::vector<double> vec;
+
+    // Loop through this line (timestamp(s) values....)
+    while (std::getline(s, field, ',')) {
+      // Skip if empty
+      if (field.empty())
+        continue;
+      // save the data to our vector
+      vec.push_back(std::atof(field.c_str()));
+    }
+
+    // Create eigen vector
+    Eigen::VectorXd temp(vec.size() - 1);
+    for (size_t i = 1; i < vec.size(); i++) {
+      temp(i - 1) = vec.at(i);
+    }
+    times.push_back(vec.at(0));
+    timing_values.push_back(temp);
+  }
+
+  // Finally close the file
+  file.close();
+
+  // Error if we don't have any data
+  if (timing_values.empty()) {
+    PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Assert that all rows in this file are of the same length
+  int rowsize = names.size();
+  for (size_t i = 0; i < timing_values.size(); i++) {
+    if (timing_values.at(i).rows() != rowsize) {
+      PRINT_ERROR(RED "[LOAD]: Invalid row size on line %d (of size %d instead of %d)\n" RESET, (int)i, (int)timing_values.at(i).rows(),
+                  rowsize);
+      PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
+      std::exit(EXIT_FAILURE);
+    }
+  }
+}
+
+void Loader::load_timing_percent(std::string path, std::vector<double> &times, std::vector<Eigen::Vector3d> &summed_values,
+                                 std::vector<Eigen::VectorXd> &node_values) {
+
+  // Try to open our trajectory file
+  std::ifstream file(path);
+  if (!file.is_open()) {
+    PRINT_ERROR(RED "[LOAD]: Unable to open timing file...\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Loop through each line of this file
+  std::string current_line;
+  while (std::getline(file, current_line)) {
+
+    // Skip if we start with a comment
+    if (!current_line.find("#"))
+      continue;
+
+    // Loop variables
+    std::istringstream s(current_line);
+    std::string field;
+    std::vector<double> vec;
+
+    // Loop through this line (timestamp(s) values....)
+    while (std::getline(s, field, ' ')) {
+      // Skip if empty
+      if (field.empty())
+        continue;
+      // save the data to our vector
+      vec.push_back(std::atof(field.c_str()));
+    }
+
+    // Create eigen vector
+    Eigen::VectorXd temp(vec.size());
+    for (size_t i = 0; i < vec.size(); i++) {
+      temp(i) = vec.at(i);
+    }
+
+    // Skip if there where no threads
+    if (temp(3) == 0.0)
+      continue;
+
+    // Save the summed value
+    times.push_back(temp(0));
+    summed_values.push_back(temp.block(1, 0, 3, 1));
+    node_values.push_back(temp.block(4, 0, temp.rows() - 4, 1));
+  }
+
+  // Finally close the file
+  file.close();
+
+  // Error if we don't have any data
+  if (times.empty()) {
+    PRINT_ERROR(RED "[LOAD]: Could not parse any data from the file!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+
+  // Assert that they are all equal
+  if (times.size() != summed_values.size() || times.size() != node_values.size()) {
+    PRINT_ERROR(RED "[LOAD]: Parsing error, pose and timestamps do not match!!\n" RESET);
+    PRINT_ERROR(RED "[LOAD]: %s\n" RESET, path.c_str());
+    std::exit(EXIT_FAILURE);
+  }
+}
+
+double Loader::get_total_length(const std::vector<Eigen::Matrix<double, 7, 1>> &poses) {
+
+  // Loop through every pose and append its segment
+  double distance = 0.0;
+  for (size_t i = 1; i < poses.size(); i++) {
+    distance += (poses[i].block(0, 0, 3, 1) - poses[i - 1].block(0, 0, 3, 1)).norm();
+  }
+
+  // return the distance
+  return distance;
+}

ov_eval/src/utils/Loader.h

@@ -0,0 +1,109 @@
+/*
+ * 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/>.
+ */
+
+#ifndef OV_EVAL_LOADER_H
+#define OV_EVAL_LOADER_H
+
+#include <fstream>
+#include <iostream>
+#include <string>
+
+#include <Eigen/Eigen>
+#include <boost/filesystem.hpp>
+
+#include "utils/colors.h"
+#include "utils/print.h"
+
+namespace ov_eval {
+
+/**
+ * @brief Has helper functions to load text files from disk and process them.
+ */
+class Loader {
+
+public:
+  /**
+   * @brief This will load *space* separated trajectory into memory
+   * @param path_traj Path to the trajectory file that we want to read in.
+   * @param times Timesteps in seconds for each pose
+   * @param poses Pose at every timestep [pos,quat]
+   * @param cov_ori Vector of orientation covariances at each timestep (empty if we can't load)
+   * @param cov_pos Vector of position covariances at each timestep (empty if we can't load)
+   */
+  static void load_data(std::string path_traj, std::vector<double> &times, std::vector<Eigen::Matrix<double, 7, 1>> &poses,
+                        std::vector<Eigen::Matrix3d> &cov_ori, std::vector<Eigen::Matrix3d> &cov_pos);
+
+  /**
+   * @brief This will load *comma* separated trajectory into memory (ASL/ETH format)
+   * @param path_traj Path to the trajectory file that we want to read in.
+   * @param times Timesteps in seconds for each pose
+   * @param poses Pose at every timestep [pos,quat]
+   * @param cov_ori Vector of orientation covariances at each timestep (empty if we can't load)
+   * @param cov_pos Vector of position covariances at each timestep (empty if we can't load)
+   */
+  static void load_data_csv(std::string path_traj, std::vector<double> &times, std::vector<Eigen::Matrix<double, 7, 1>> &poses,
+                            std::vector<Eigen::Matrix3d> &cov_ori, std::vector<Eigen::Matrix3d> &cov_pos);
+
+  /**
+   * @brief Load an arbitrary sized row of *space* separated values, used for our simulation
+   * @param path Path to our text file to load
+   * @param values Each row of values
+   */
+  static void load_simulation(std::string path, std::vector<Eigen::VectorXd> &values);
+
+  /**
+   * @brief Load *comma* separated timing file from pid_ros.py file
+   * @param path Path to our text file to load
+   * @param names Names of each timing category
+   * @param times Timesteps in seconds for each measurement
+   * @param timing_values Component timing values for the given timestamp
+   */
+  static void load_timing_flamegraph(std::string path, std::vector<std::string> &names, std::vector<double> &times,
+                                     std::vector<Eigen::VectorXd> &timing_values);
+
+  /**
+   * @brief Load space separated timing file from pid_ros.py file
+   * @param path Path to our text file to load
+   * @param times Timesteps in seconds for each measurement
+   * @param summed_values Summed node values [%cpu,%mem,num_threads]
+   * @param node_values Values for each separate node [%cpu,%mem,num_threads]
+   */
+  static void load_timing_percent(std::string path, std::vector<double> &times, std::vector<Eigen::Vector3d> &summed_values,
+                                  std::vector<Eigen::VectorXd> &node_values);
+
+  /**
+   * @brief Will calculate the total trajectory distance
+   * @param poses Pose at every timestep [pos,quat]
+   * @return Distance travels (meters)
+   */
+  static double get_total_length(const std::vector<Eigen::Matrix<double, 7, 1>> &poses);
+
+private:
+  /**
+   * All function in this class should be static.
+   * Thus an instance of this class cannot be created.
+   */
+  Loader() {}
+};
+
+} // namespace ov_eval
+
+#endif // OV_EVAL_LOADER_H

ov_eval/src/utils/Recorder.h

@@ -0,0 +1,194 @@
+/*
+ * 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/>.
+ */
+
+#ifndef OV_EVAL_RECORDER_H
+#define OV_EVAL_RECORDER_H
+
+#include <fstream>
+#include <iostream>
+#include <string>
+
+#include <Eigen/Eigen>
+#include <boost/filesystem.hpp>
+
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+
+namespace ov_eval {
+
+/**
+ * @brief This class takes in published poses and writes them to file.
+ *
+ * Original code is based on this modified [posemsg_to_file](https://github.com/rpng/posemsg_to_file/).
+ * Output is in a text file that is space deliminated and can be read by all scripts.
+ * If we have a covariance then we also save the upper triangular part to file so we can calculate NEES values.
+ */
+class Recorder {
+
+public:
+  /**
+   * @brief Default constructor that will open the specified file on disk.
+   * If the output directory does not exists this will also create the directory path to this file.
+   * @param filename Desired file we want to "record" into
+   */
+  Recorder(std::string filename) {
+    // Create folder path to this location if not exists
+    boost::filesystem::path dir(filename.c_str());
+    if (boost::filesystem::create_directories(dir.parent_path())) {
+      ROS_INFO("Created folder path to output file.");
+      ROS_INFO("Path: %s", dir.parent_path().c_str());
+    }
+    // If it exists, then delete it
+    if (boost::filesystem::exists(filename)) {
+      ROS_WARN("Output file exists, deleting old file....");
+      boost::filesystem::remove(filename);
+    }
+    // Open this file we want to write to
+    outfile.open(filename.c_str());
+    if (outfile.fail()) {
+      ROS_ERROR("Unable to open output file!!");
+      ROS_ERROR("Path: %s", filename.c_str());
+      std::exit(EXIT_FAILURE);
+    }
+    outfile << "# timestamp(s) tx ty tz qx qy qz qw Pr11 Pr12 Pr13 Pr22 Pr23 Pr33 Pt11 Pt12 Pt13 Pt22 Pt23 Pt33" << std::endl;
+    // Set initial state values
+    timestamp = -1;
+    q_ItoG << 0, 0, 0, 1;
+    p_IinG = Eigen::Vector3d::Zero();
+    cov_rot = Eigen::Matrix<double, 3, 3>::Zero();
+    cov_pos = Eigen::Matrix<double, 3, 3>::Zero();
+    has_covariance = false;
+  }
+
+  /**
+   * @brief Callback for nav_msgs::Odometry message types.
+   *
+   * Note that covariance is in the order (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis).
+   * http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovariance.html
+   *
+   * @param msg New message
+   */
+  void callback_odometry(const nav_msgs::OdometryPtr &msg) {
+    timestamp = msg->header.stamp.toSec();
+    q_ItoG << msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w;
+    p_IinG << msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z;
+    cov_pos << msg->pose.covariance.at(0), msg->pose.covariance.at(1), msg->pose.covariance.at(2), msg->pose.covariance.at(6),
+        msg->pose.covariance.at(7), msg->pose.covariance.at(8), msg->pose.covariance.at(12), msg->pose.covariance.at(13),
+        msg->pose.covariance.at(14);
+    cov_rot << msg->pose.covariance.at(21), msg->pose.covariance.at(22), msg->pose.covariance.at(23), msg->pose.covariance.at(27),
+        msg->pose.covariance.at(28), msg->pose.covariance.at(29), msg->pose.covariance.at(33), msg->pose.covariance.at(34),
+        msg->pose.covariance.at(35);
+    has_covariance = true;
+    write();
+  }
+
+  /**
+   * @brief Callback for geometry_msgs::PoseStamped message types
+   * @param msg New message
+   */
+  void callback_pose(const geometry_msgs::PoseStampedPtr &msg) {
+    timestamp = msg->header.stamp.toSec();
+    q_ItoG << msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z, msg->pose.orientation.w;
+    p_IinG << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
+    write();
+  }
+
+  /**
+   * @brief Callback for geometry_msgs::PoseWithCovarianceStamped message types.
+   *
+   * Note that covariance is in the order (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis).
+   * http://docs.ros.org/api/geometry_msgs/html/msg/PoseWithCovariance.html
+   *
+   * @param msg New message
+   */
+  void callback_posecovariance(const geometry_msgs::PoseWithCovarianceStampedPtr &msg) {
+    timestamp = msg->header.stamp.toSec();
+    q_ItoG << msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w;
+    p_IinG << msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z;
+    cov_pos << msg->pose.covariance.at(0), msg->pose.covariance.at(1), msg->pose.covariance.at(2), msg->pose.covariance.at(6),
+        msg->pose.covariance.at(7), msg->pose.covariance.at(8), msg->pose.covariance.at(12), msg->pose.covariance.at(13),
+        msg->pose.covariance.at(14);
+    cov_rot << msg->pose.covariance.at(21), msg->pose.covariance.at(22), msg->pose.covariance.at(23), msg->pose.covariance.at(27),
+        msg->pose.covariance.at(28), msg->pose.covariance.at(29), msg->pose.covariance.at(33), msg->pose.covariance.at(34),
+        msg->pose.covariance.at(35);
+    has_covariance = true;
+    write();
+  }
+
+  /**
+   * @brief Callback for geometry_msgs::TransformStamped message types
+   * @param msg New message
+   */
+  void callback_transform(const geometry_msgs::TransformStampedPtr &msg) {
+      ROS_INFO("Callback successfully called!");
+    timestamp = msg->header.stamp.toSec();
+    q_ItoG << msg->transform.rotation.x, msg->transform.rotation.y, msg->transform.rotation.z, msg->transform.rotation.w;
+    p_IinG << msg->transform.translation.x, msg->transform.translation.y, msg->transform.translation.z;
+    write();
+  }
+
+protected:
+  /**
+   * @brief This is the main write function that will save to disk.
+   * This should be called after we have saved the desired pose to our class variables.
+   */
+  void write() {
+
+    // timestamp
+    outfile.precision(5);
+    outfile.setf(std::ios::fixed, std::ios::floatfield);
+    outfile << timestamp << " ";
+
+    // pose
+    outfile.precision(6);
+    outfile << p_IinG.x() << " " << p_IinG.y() << " " << p_IinG.z() << " " << q_ItoG(0) << " " << q_ItoG(1) << " " << q_ItoG(2) << " "
+            << q_ItoG(3);
+
+    // output the covariance only if we have it
+    if (has_covariance) {
+      outfile.precision(10);
+      outfile << " " << cov_rot(0, 0) << " " << cov_rot(0, 1) << " " << cov_rot(0, 2) << " " << cov_rot(1, 1) << " " << cov_rot(1, 2) << " "
+              << cov_rot(2, 2) << " " << cov_pos(0, 0) << " " << cov_pos(0, 1) << " " << cov_pos(0, 2) << " " << cov_pos(1, 1) << " "
+              << cov_pos(1, 2) << " " << cov_pos(2, 2) << std::endl;
+    } else {
+      outfile << std::endl;
+    }
+    ROS_INFO("Successfully wrote to the file.");
+  }
+
+  // Output stream file
+  std::ofstream outfile;
+
+  // Temp storage objects for our pose and its certainty
+  bool has_covariance = false;
+  double timestamp;
+  Eigen::Vector4d q_ItoG;
+  Eigen::Vector3d p_IinG;
+  Eigen::Matrix<double, 3, 3> cov_rot;
+  Eigen::Matrix<double, 3, 3> cov_pos;
+};
+
+} // namespace ov_eval
+
+#endif // OV_EVAL_RECORDER_H

ov_eval/src/utils/Statistics.h

@@ -0,0 +1,133 @@
+/*
+ * 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/>.
+ */
+
+#ifndef OV_EVAL_STATISTICS_H
+#define OV_EVAL_STATISTICS_H
+
+#include <fstream>
+#include <iostream>
+#include <string>
+
+#include <Eigen/Eigen>
+
+namespace ov_eval {
+
+/**
+ * @brief Statistics object for a given set scalar time series values.
+ *
+ * Ensure that you call the calculate() function to update the values before using them.
+ * This will compute all the final results from the values in @ref values vector.
+ */
+struct Statistics {
+
+public:
+  /// Root mean squared for the given values
+  double rmse = 0.0;
+
+  /// Mean of the given values
+  double mean = 0.0;
+
+  /// Median of the given values
+  double median = 0.0;
+
+  /// Standard deviation of given values
+  double std = 0.0;
+
+  /// Max of the given values
+  double max = 0.0;
+
+  /// Min of the given values
+  double min = 0.0;
+
+  /// 99th percentile
+  double ninetynine = 0.0;
+
+  /// Timestamp when these values occured at
+  std::vector<double> timestamps;
+
+  /// Values (e.g. error or nees at a given time)
+  std::vector<double> values;
+
+  /// Bound of these values (e.g. our expected covariance bound)
+  std::vector<double> values_bound;
+
+  /// Will calculate all values from our vectors of information
+  void calculate() {
+
+    // Sort the data for easy finding of values
+    std::vector<double> values_sorted = values;
+    std::sort(values_sorted.begin(), values_sorted.end());
+
+    // If we don't have any data, just return :(
+    if (values_sorted.empty())
+      return;
+
+    // Now that its been sorted, can easily grab min and max
+    min = values_sorted.at(0);
+    max = values_sorted.at(values_sorted.size() - 1);
+
+    // Compute median
+    // ODD:  grab middle from the sorted vector
+    // EVEN: average the middle two numbers
+    if (values_sorted.size() == 1) {
+      median = values_sorted.at(values_sorted.size() - 1);
+    } else if (values_sorted.size() % 2 == 1) {
+      median = values_sorted.at(values_sorted.size() / 2);
+    } else if (values_sorted.size() > 1) {
+      median = 0.5 * (values_sorted.at(values_sorted.size() / 2 - 1) + values_sorted.at(values_sorted.size() / 2));
+    } else {
+      median = 0.0;
+    }
+
+    // Compute mean and rmse
+    mean = 0;
+    for (size_t i = 0; i < values_sorted.size(); i++) {
+      assert(!std::isnan(values_sorted.at(i)));
+      mean += values_sorted.at(i);
+      rmse += values_sorted.at(i) * values_sorted.at(i);
+    }
+    mean /= values_sorted.size();
+    rmse = std::sqrt(rmse / values_sorted.size());
+
+    // Using mean, compute standard deviation
+    std = 0;
+    for (size_t i = 0; i < values_sorted.size(); i++) {
+      std += std::pow(values_sorted.at(i) - mean, 2);
+    }
+    std = std::sqrt(std / (values_sorted.size() - 1));
+
+    // 99th percentile
+    // TODO: is this correct?
+    // TODO: http://sphweb.bumc.bu.edu/otlt/MPH-Modules/BS/BS704_Probability/BS704_Probability10.html
+    ninetynine = mean + 2.326 * std;
+  }
+
+  /// Will clear any old values
+  void clear() {
+    timestamps.clear();
+    values.clear();
+    values_bound.clear();
+  }
+};
+
+} // namespace ov_eval
+
+#endif // OV_EVAL_STATISTICS_H

ov_eval/src/utils/print.cpp

@@ -0,0 +1,104 @@
+/*
+ * 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 "print.h"
+
+using namespace ov_core;
+
+// Need to define the static variable for everything to work
+Printer::PrintLevel Printer::current_print_level = PrintLevel::INFO;
+
+void Printer::setPrintLevel(const std::string &level) {
+  if (level == "ALL")
+    setPrintLevel(PrintLevel::ALL);
+  else if (level == "DEBUG")
+    setPrintLevel(PrintLevel::DEBUG);
+  else if (level == "INFO")
+    setPrintLevel(PrintLevel::INFO);
+  else if (level == "WARNING")
+    setPrintLevel(PrintLevel::WARNING);
+  else if (level == "ERROR")
+    setPrintLevel(PrintLevel::ERROR);
+  else if (level == "SILENT")
+    setPrintLevel(PrintLevel::SILENT);
+  else {
+    std::cout << "Invalid print level requested: " << level << std::endl;
+    std::cout << "Valid levels are: ALL, DEBUG, INFO, WARNING, ERROR, SILENT" << std::endl;
+    std::exit(EXIT_FAILURE);
+  }
+}
+
+void Printer::setPrintLevel(PrintLevel level) {
+  Printer::current_print_level = level;
+  std::cout << "Setting printing level to: ";
+  switch (current_print_level) {
+  case PrintLevel::ALL:
+    std::cout << "ALL";
+    break;
+  case PrintLevel::DEBUG:
+    std::cout << "DEBUG";
+    break;
+  case PrintLevel::INFO:
+    std::cout << "INFO";
+    break;
+  case PrintLevel::WARNING:
+    std::cout << "WARNING";
+    break;
+  case PrintLevel::ERROR:
+    std::cout << "ERROR";
+    break;
+  case PrintLevel::SILENT:
+    std::cout << "SILENT";
+    break;
+  default:
+    std::cout << std::endl;
+    std::cout << "Invalid print level requested: " << level << std::endl;
+    std::cout << "Valid levels are: ALL, DEBUG, INFO, WARNING, ERROR, SILENT" << std::endl;
+    std::exit(EXIT_FAILURE);
+  }
+  std::cout << std::endl;
+}
+
+void Printer::debugPrint(PrintLevel level, const char location[], const char line[], const char *format, ...) {
+  // Only print for the current debug level
+  if (static_cast<int>(level) < static_cast<int>(Printer::current_print_level)) {
+    return;
+  }
+
+  // Print the location info first for our debug output
+  // Truncate the filename to the max size for the filepath
+  if (static_cast<int>(Printer::current_print_level) <= static_cast<int>(Printer::PrintLevel::DEBUG)) {
+    std::string path(location);
+    std::string base_filename = path.substr(path.find_last_of("/\\") + 1);
+    if (base_filename.size() > MAX_FILE_PATH_LEGTH) {
+      printf("%s", base_filename.substr(base_filename.size() - MAX_FILE_PATH_LEGTH, base_filename.size()).c_str());
+    } else {
+      printf("%s", base_filename.c_str());
+    }
+    printf(":%s ", line);
+  }
+
+  // Print the rest of the args
+  va_list args;
+  va_start(args, format);
+  vprintf(format, args);
+  va_end(args);
+}

ov_eval/src/utils/print.h

@@ -0,0 +1,113 @@
+/*
+ * 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/>.
+ */
+
+#ifndef OV_CORE_PRINT_H
+#define OV_CORE_PRINT_H
+
+#include <cstdarg>
+#include <cstdint>
+#include <cstring>
+#include <iostream>
+#include <string>
+
+namespace ov_core {
+
+/**
+ * @brief Printer for open_vins that allows for various levels of printing to be done
+ *
+ * To set the global verbosity level one can do the following:
+ * @code{.cpp}
+ * ov_core::Printer::setPrintLevel("WARNING");
+ * ov_core::Printer::setPrintLevel(ov_core::Printer::PrintLevel::WARNING);
+ * @endcode
+ */
+class Printer {
+public:
+  /**
+   * @brief The different print levels possible
+   *
+   * - PrintLevel::ALL : All PRINT_XXXX will output to the console
+   * - PrintLevel::DEBUG : "DEBUG", "INFO", "WARNING" and "ERROR" will be printed. "ALL" will be silenced
+   * - PrintLevel::INFO : "INFO", "WARNING" and "ERROR" will be printed. "ALL" and "DEBUG" will be silenced
+   * - PrintLevel::WARNING : "WARNING" and "ERROR" will be printed. "ALL", "DEBUG" and "INFO" will be silenced
+   * - PrintLevel::ERROR : Only "ERROR" will be printed. All the rest are silenced
+   * - PrintLevel::SILENT : All PRINT_XXXX will be silenced.
+   */
+  enum PrintLevel { ALL = 0, DEBUG = 1, INFO = 2, WARNING = 3, ERROR = 4, SILENT = 5 };
+
+  /**
+   * @brief Set the print level to use for all future printing to stdout.
+   * @param level The debug level to use
+   */
+  static void setPrintLevel(const std::string &level);
+
+  /**
+   * @brief Set the print level to use for all future printing to stdout.
+   * @param level The debug level to use
+   */
+  static void setPrintLevel(PrintLevel level);
+
+  /**
+   * @brief The print function that prints to stdout.
+   * @param level the print level for this print call
+   * @param location the location the print was made from
+   * @param line the line the print was made from
+   * @param format The printf format
+   */
+  static void debugPrint(PrintLevel level, const char location[], const char line[], const char *format, ...);
+
+  /// The current print level
+  static PrintLevel current_print_level;
+
+private:
+  /// The max length for the file path.  This is to avoid very long file paths from
+  static constexpr uint32_t MAX_FILE_PATH_LEGTH = 30;
+};
+
+} /* namespace ov_core */
+
+/*
+ * Converts anything to a string
+ */
+#define STRINGIFY(x) #x
+#define TOSTRING(x) STRINGIFY(x)
+
+/*
+ * The different Types of print levels
+ */
+#define PRINT_ALL(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::ALL, __FILE__, TOSTRING(__LINE__), x);
+#define PRINT_DEBUG(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::DEBUG, __FILE__, TOSTRING(__LINE__), x);
+#define PRINT_INFO(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::INFO, __FILE__, TOSTRING(__LINE__), x);
+#define PRINT_WARNING(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::WARNING, __FILE__, TOSTRING(__LINE__), x);
+#define PRINT_ERROR(x...) ov_core::Printer::debugPrint(ov_core::Printer::PrintLevel::ERROR, __FILE__, TOSTRING(__LINE__), x);
+
+// Assert that will always be here in release builds also
+// TODO: place this in a better header, just putting here for now...
+#define assert_r(EX) (void)((EX) || (__assert(#EX, __FILE__, __LINE__), 0))
+#ifdef __cplusplus
+extern "C" {
+#endif
+extern void __assert(const char *msg, const char *file, int line);
+#ifdef __cplusplus
+};
+#endif
+
+#endif /* OV_CORE_PRINT_H */