A single run for a given dataset. More...

#include <ResultTrajectory.h>

Public Member Functions
void	calculate_ate (Statistics &error_ori, Statistics &error_pos)
	Computes the Absolute Trajectory Error (ATE) for this trajectory. More...

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. More...

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. More...

void	calculate_nees (Statistics &nees_ori, Statistics &nees_pos)
	Computes the Normalized Estimation Error Squared (NEES) for this trajectory. More...

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. More...

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

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

Protected Attributes
std::vector< Eigen::Matrix3d >	est_covori

std::vector< Eigen::Matrix3d >	est_covpos

std::vector< Eigen::Matrix< double, 7, 1 > >	est_poses

std::vector< Eigen::Matrix< double, 7, 1 > >	est_poses_aignedtoGT

std::vector< double >	est_times

std::vector< Eigen::Matrix3d >	gt_covori

std::vector< Eigen::Matrix3d >	gt_covpos

std::vector< Eigen::Matrix< double, 7, 1 > >	gt_poses

std::vector< Eigen::Matrix< double, 7, 1 > >	gt_poses_aignedtoEST

std::vector< double >	gt_times

Detailed Description

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

Definition at line 59 of file ResultTrajectory.h.

Constructor & Destructor Documentation

◆ ResultTrajectory()

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]

Definition at line 26 of file ResultTrajectory.cpp.

Member Function Documentation

◆ calculate_ate()

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

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:

$\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*}$

Parameters

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

Definition at line 82 of file ResultTrajectory.cpp.

◆ calculate_ate_2d()

void 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.

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*}$

Parameters

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

Definition at line 111 of file ResultTrajectory.cpp.

◆ calculate_error()

void 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.

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*}$

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

Definition at line 292 of file ResultTrajectory.cpp.

◆ calculate_nees()

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

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.

$\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*}$

Parameters

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

Definition at line 244 of file ResultTrajectory.cpp.

◆ calculate_rpe()

void 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.

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*}$

Parameters

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

Definition at line 140 of file ResultTrajectory.cpp.

◆ compute_comparison_indices_length()

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

inlineprotected

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

Definition at line 169 of file ResultTrajectory.h.

Member Data Documentation

◆ est_covori

std::vector<Eigen::Matrix3d> ov_eval::ResultTrajectory::est_covori

protected

Definition at line 155 of file ResultTrajectory.h.

◆ est_covpos

std::vector<Eigen::Matrix3d> ov_eval::ResultTrajectory::est_covpos

protected

Definition at line 155 of file ResultTrajectory.h.

◆ est_poses

std::vector<Eigen::Matrix<double, 7, 1> > ov_eval::ResultTrajectory::est_poses

protected

Definition at line 154 of file ResultTrajectory.h.

◆ est_poses_aignedtoGT

std::vector<Eigen::Matrix<double, 7, 1> > ov_eval::ResultTrajectory::est_poses_aignedtoGT

protected

Definition at line 158 of file ResultTrajectory.h.

◆ est_times

std::vector<double> ov_eval::ResultTrajectory::est_times

protected

Definition at line 153 of file ResultTrajectory.h.

◆ gt_covori

std::vector<Eigen::Matrix3d> ov_eval::ResultTrajectory::gt_covori

protected

Definition at line 155 of file ResultTrajectory.h.

◆ gt_covpos

std::vector<Eigen::Matrix3d> ov_eval::ResultTrajectory::gt_covpos

protected

Definition at line 155 of file ResultTrajectory.h.

◆ gt_poses

std::vector<Eigen::Matrix<double, 7, 1> > ov_eval::ResultTrajectory::gt_poses

protected

Definition at line 154 of file ResultTrajectory.h.

◆ gt_poses_aignedtoEST

std::vector<Eigen::Matrix<double, 7, 1> > ov_eval::ResultTrajectory::gt_poses_aignedtoEST

protected

Definition at line 159 of file ResultTrajectory.h.

◆ gt_times

std::vector<double> ov_eval::ResultTrajectory::gt_times

protected

Definition at line 153 of file ResultTrajectory.h.

The documentation for this class was generated from the following files:

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ResultTrajectory()

Member Function Documentation

◆ calculate_ate()

◆ calculate_ate_2d()

◆ calculate_error()

◆ calculate_nees()

◆ calculate_rpe()

◆ compute_comparison_indices_length()

Member Data Documentation

◆ est_covori

◆ est_covpos

◆ est_poses

◆ est_poses_aignedtoGT

◆ est_times

◆ gt_covori

◆ gt_covpos

◆ gt_poses

◆ gt_poses_aignedtoEST

◆ gt_times