ov_eval::ResultTrajectory class

A single run for a given dataset.

Contents

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 System Evaluation page for details and Zhang and Scaramuzza A Tutorial on Quantitative Trajectory Evaluation for Visual(-Inertial) Odometry paper for implementation specific details.

Constructors, destructors, conversion operators

ResultTrajectory(std::string path_est, std::string path_gt, std::string alignment_method)
Default constructor that will load, intersect, and align our trajectories.

Public functions

void calculate_ate(Statistics& error_ori, Statistics& error_pos)
Computes the Absolute Trajectory Error (ATE) for this trajectory.
void calculate_ate_2d(Statistics& error_ori, Statistics& error_pos)
Computes the Absolute Trajectory Error (ATE) for this trajectory in the 2d x-y plane.
void calculate_rpe(const std::vector<double>& segment_lengths, std::map<double, std::pair<Statistics, Statistics>>& error_rpe)
Computes the Relative Pose Error (RPE) for this trajectory.
void calculate_nees(Statistics& nees_ori, Statistics& nees_pos)
Computes the Normalized Estimation Error Squared (NEES) for this trajectory.
void calculate_error(Statistics& posx, Statistics& posy, Statistics& posz, Statistics& orix, Statistics& oriy, Statistics& oriz, Statistics& roll, Statistics& pitch, Statistics& yaw)
Computes the error at each timestamp for this trajectory.

Protected functions

auto compute_comparison_indices_length(std::vector<double>& distances, double distance, double max_dist_diff) -> std::vector<int>
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.

Function documentation

ov_eval::ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, std::string alignment_method)

Default constructor that will load, intersect, and align our trajectories.

Parameters
path_est Path to the estimate text file
path_gt Path to the groundtruth text file
alignment_method The alignment method to use [sim3, se3, posyaw, none]

void ov_eval::ResultTrajectory::calculate_ate(Statistics& error_ori, Statistics& error_pos)

Computes the Absolute Trajectory Error (ATE) for this trajectory.

Parameters
error_ori Error values for the orientation
error_pos Error values for the position

This will first do our alignment of the two trajectories. Then at each point the error will be calculated and normed as follows:

\begin{align*} e_{ATE} &= \sqrt{ \frac{1}{K} \sum_{k=1}^{K} ||\mathbf{x}_{k,i} \boxminus \hat{\mathbf{x}}^+_{k,i}||^2_{2} } \end{align*}

void ov_eval::ResultTrajectory::calculate_ate_2d(Statistics& error_ori, Statistics& error_pos)

Computes the Absolute Trajectory Error (ATE) for this trajectory in the 2d x-y plane.

Parameters
error_ori Error values for the orientation (yaw error)
error_pos Error values for the position (xy error)

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:

\begin{align*} e_{ATE} &= \sqrt{ \frac{1}{K} \sum_{k=1}^{K} ||\mathbf{x}_{k,i} \boxminus \hat{\mathbf{x}}^+_{k,i}||^2_{2} } \end{align*}

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

Computes the Relative Pose Error (RPE) for this trajectory.

Parameters
segment_lengths What segment lengths we want to calculate for
error_rpe Map of segment lengths => errors for that length (orientation and position)

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.

\begin{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} \end{align*}

void ov_eval::ResultTrajectory::calculate_nees(Statistics& nees_ori, Statistics& nees_pos)

Computes the Normalized Estimation Error Squared (NEES) for this trajectory.

Parameters
nees_ori NEES values for the orientation
nees_pos NEES values for the position

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.

\begin{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}) \end{align*}

void ov_eval::ResultTrajectory::calculate_error(Statistics& posx, Statistics& posy, Statistics& posz, Statistics& orix, Statistics& oriy, Statistics& oriz, Statistics& roll, Statistics& pitch, Statistics& yaw)

Computes the error at each timestamp for this trajectory.

Parameters
posx Position x-axis error and bound if we have it in our file
posy Position y-axis error and bound if we have it in our file
posz Position z-axis error and bound if we have it in our file
orix Orientation x-axis error and bound if we have it in our file
oriy Orientation y-axis error and bound if we have it in our file
oriz Orientation z-axis error and bound if we have it in our file
roll Orientation roll error and bound if we have it in our file
pitch Orientation pitch error and bound if we have it in our file
yaw Orientation yaw error and bound if we have it in our file

As compared to ATE error (see 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.

\begin{align*} e_{rmse,k} &= \sqrt{ \frac{1}{N} \sum_{i=1}^{N} ||\mathbf{x}_{k,i} \boxminus \hat{\mathbf{x}}_{k,i}||^2_{2} } \end{align*}

std::vector<int> ov_eval::ResultTrajectory::compute_comparison_indices_length(std::vector<double>& distances, double distance, double max_dist_diff) protected

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.

Parameters
distances Total distance travelled from start at each index
distance Distance of subtrajectory
max_dist_diff Maximum error between current trajectory length and the desired
Returns End indices for each subtrajectory