ResultTrajectory.h

Go to the documentation of this file.

/*
 * OpenVINS: An Open Platform for Visual-Inertial Research
 * Copyright (C) 2018-2023 Patrick Geneva
 * Copyright (C) 2018-2023 Guoquan Huang
 * Copyright (C) 2018-2023 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 {

class ResultTrajectory {

public:
  ResultTrajectory(std::string path_est, std::string path_gt, std::string alignment_method);

  void calculate_ate(Statistics &error_ori, Statistics &error_pos);

  void calculate_ate_2d(Statistics &error_ori, Statistics &error_pos);

  void calculate_rpe(const std::vector<double> &segment_lengths, std::map<double, std::pair<Statistics, Statistics>> &error_rpe);

  void calculate_nees(Statistics &nees_ori, Statistics &nees_pos);

  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;

  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