# ov_eval::AlignUtils class

Helper functions for the trajectory alignment class.

### Contents

• Reference

The key function is an implementation of Umeyama's Least-squares estimation of transformation parameters between two point patterns. This is what allows us to find the transform between the two trajectories without worrying about singularities for the absolute trajectory error.

## Public static functions

static auto get_best_yaw(const Eigen::Matrix<double, 3, 3>& C) -> double
Gets best yaw from Frobenius problem. Equation (17)-(18) in Zhang et al. 2018 IROS paper.
static auto get_mean(const std::vector<Eigen::Matrix<double, 3, 1>>& data) -> Eigen::Matrix<double, 3, 1>
Gets mean of the vector of data.
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)
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).
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)
Will intersect our loaded data so that we have common timestamps.
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)
Will intersect our loaded data so that we have common timestamps.

## Function documentation

### static double ov_eval::AlignUtils::get_best_yaw(const Eigen::Matrix<double, 3, 3>& C)

Gets best yaw from Frobenius problem. Equation (17)-(18) in Zhang et al. 2018 IROS paper.

Parameters
C Data matrix

### static Eigen::Matrix<double, 3, 1> ov_eval::AlignUtils::get_mean(const std::vector<Eigen::Matrix<double, 3, 1>>& data)

Gets mean of the vector of data.

Parameters
data Vector of data
Returns Mean value

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

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

Parameters
data Vector of points in data frame (i.e., estimates)
model Vector of points in model frame (i.e., gt)
R Output rotation from data frame to model frame
t Output translation from data frame to model frame
s Output scale from data frame to model frame
known_scale Whether to fix scale
yaw_only Whether to only use yaw orientation (such as when frames are already gravity aligned)

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

Will intersect our loaded data so that we have common timestamps.

Parameters
offset Time offset to append to our estimate
max_difference Biggest allowed difference between matched timesteps
est_times
gt_times
est_poses
gt_poses

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

Will intersect our loaded data so that we have common timestamps.

Parameters
offset Time offset to append to our estimate
max_difference Biggest allowed difference between matched timesteps
est_times
gt_times
est_poses
gt_poses
est_covori
est_covpos
gt_covori
gt_covpos