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::
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::
Gets mean of the vector of data.
| Parameters | |
|---|---|
| data | Vector of data |
| Returns | Mean value |
static void ov_eval::
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::
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::
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 | |