ov_eval::AlignTrajectory class

Class that given two trajectories, will align the two.

Contents

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 toolkit by Zhang and Scaramuzza. Please take a look at their 2018 IROS paper on these methods.

Public static functions

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)
Align estimate to GT using a desired method using a set of initial poses.

Protected static functions

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)
Align estimate to GT using only position and yaw (for gravity aligned trajectories) using only the first poses.
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)
Align estimate to GT using only position and yaw (for gravity aligned trajectories) using a set of initial poses.
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)
Align estimate to GT using a full SE(3) transform using only the first poses.
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)
Align estimate to GT using a full SE(3) transform using a set of initial poses.
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)
Align estimate to GT using a full SIM(3) transform using a set of initial poses.

Function documentation

static void ov_eval::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 = -1)

Align estimate to GT using a desired method using a set of initial poses.

Parameters
traj_es Estimated trajectory values in estimate frame [pos,quat]
traj_gt Groundtruth trjaectory in groundtruth frame [pos,quat]
R Rotation from estimate to GT frame that will be computed
t translation from estimate to GT frame that will be computed
s scale from estimate to GT frame that will be computed
method Method used for alignment
n_aligned Number of poses to use for alignment (-1 will use all)

static void ov_eval::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) protected

Align estimate to GT using only position and yaw (for gravity aligned trajectories) using only the first poses.

Parameters
traj_es Estimated trajectory values in estimate frame [pos,quat]
traj_gt Groundtruth trjaectory in groundtruth frame [pos,quat]
R Rotation from estimate to GT frame that will be computed
t translation from estimate to GT frame that will be computed

static void ov_eval::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 = -1) protected

Align estimate to GT using only position and yaw (for gravity aligned trajectories) using a set of initial poses.

Parameters
traj_es Estimated trajectory values in estimate frame [pos,quat]
traj_gt Groundtruth trjaectory in groundtruth frame [pos,quat]
R Rotation from estimate to GT frame that will be computed
t translation from estimate to GT frame that will be computed
n_aligned Number of poses to use for alignment (-1 will use all)

static void ov_eval::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) protected

Align estimate to GT using a full SE(3) transform using only the first poses.

Parameters
traj_es Estimated trajectory values in estimate frame [pos,quat]
traj_gt Groundtruth trjaectory in groundtruth frame [pos,quat]
R Rotation from estimate to GT frame that will be computed
t translation from estimate to GT frame that will be computed

static void ov_eval::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 = -1) protected

Align estimate to GT using a full SE(3) transform using a set of initial poses.

Parameters
traj_es Estimated trajectory values in estimate frame [pos,quat]
traj_gt Groundtruth trjaectory in groundtruth frame [pos,quat]
R Rotation from estimate to GT frame that will be computed
t translation from estimate to GT frame that will be computed
n_aligned Number of poses to use for alignment (-1 will use all)

static void ov_eval::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 = -1) protected

Align estimate to GT using a full SIM(3) transform using a set of initial poses.

Parameters
traj_es Estimated trajectory values in estimate frame [pos,quat]
traj_gt Groundtruth trjaectory in groundtruth frame [pos,quat]
R Rotation from estimate to GT frame that will be computed
t translation from estimate to GT frame that will be computed
s scale from estimate to GT frame that will be computed
n_aligned Number of poses to use for alignment (-1 will use all)