AlignTrajectory.cpp
Go to the documentation of this file.
1 /*
2  * OpenVINS: An Open Platform for Visual-Inertial Research
3  * Copyright (C) 2018-2023 Patrick Geneva
4  * Copyright (C) 2018-2023 Guoquan Huang
5  * Copyright (C) 2018-2023 OpenVINS Contributors
6  * Copyright (C) 2018-2019 Kevin Eckenhoff
7  *
8  * This program is free software: you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation, either version 3 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU General Public License for more details.
17  *
18  * You should have received a copy of the GNU General Public License
19  * along with this program. If not, see <https://www.gnu.org/licenses/>.
20  */
21 
22 #include "AlignTrajectory.h"
23 
24 using namespace ov_eval;
25 
26 void AlignTrajectory::align_trajectory(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
27  const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t,
28  double &s, std::string method, int n_aligned) {
29 
30  // Use the correct method
31  if (method == "posyaw") {
32  s = 1;
33  align_posyaw(traj_es, traj_gt, R, t, n_aligned);
34  } else if (method == "posyawsingle") {
35  s = 1;
36  align_posyaw_single(traj_es, traj_gt, R, t);
37  } else if (method == "se3") {
38  s = 1;
39  align_se3(traj_es, traj_gt, R, t, n_aligned);
40  } else if (method == "se3single") {
41  s = 1;
42  align_se3_single(traj_es, traj_gt, R, t);
43  } else if (method == "sim3") {
44  assert(n_aligned >= 2 || n_aligned == -1);
45  align_sim3(traj_es, traj_gt, R, t, s, n_aligned);
46  } else if (method == "none") {
47  s = 1;
48  R.setIdentity();
49  t.setZero();
50  } else {
51  PRINT_ERROR(RED "[ALIGN]: Invalid alignment method!\n" RESET);
52  PRINT_ERROR(RED "[ALIGN]: Possible options: posyaw, posyawsingle, se3, se3single, sim3, none\n" RESET);
53  std::exit(EXIT_FAILURE);
54  }
55 }
56 
57 void AlignTrajectory::align_posyaw_single(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
58  const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t) {
59 
60  // Get first ever poses
61  Eigen::Vector4d q_es_0 = traj_es.at(0).block(3, 0, 4, 1);
62  Eigen::Vector3d p_es_0 = traj_es.at(0).block(0, 0, 3, 1);
63 
64  Eigen::Vector4d q_gt_0 = traj_gt.at(0).block(3, 0, 4, 1);
65  Eigen::Vector3d p_gt_0 = traj_gt.at(0).block(0, 0, 3, 1);
66 
67  // Get rotations from IMU frame to World (note JPL!)
68  Eigen::Matrix3d g_rot = ov_core::quat_2_Rot(q_gt_0).transpose();
69  Eigen::Matrix3d est_rot = ov_core::quat_2_Rot(q_es_0).transpose();
70 
71  // Data matrix for the Frobenius problem
72  Eigen::Matrix3d C_R = est_rot * g_rot.transpose();
73 
74  // Recover yaw
75  double theta = AlignUtils::get_best_yaw(C_R);
76 
77  // Compute rotation
78  R = ov_core::rot_z(theta);
79 
80  // Compute translation
81  t.noalias() = p_gt_0 - R * p_es_0;
82 }
83 
84 void AlignTrajectory::align_posyaw(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
85  const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t,
86  int n_aligned) {
87 
88  // If we only have one, just use the single alignment
89  if (n_aligned == 1) {
90  align_posyaw_single(traj_es, traj_gt, R, t);
91  } else {
92 
93  // Get just position vectors
94  assert(!traj_es.empty());
95  std::vector<Eigen::Vector3d> pos_est, pos_gt;
96  for (size_t i = 0; i < traj_es.size() && i < traj_gt.size(); i++) {
97  pos_est.push_back(traj_es.at(i).block(0, 0, 3, 1));
98  pos_gt.push_back(traj_gt.at(i).block(0, 0, 3, 1));
99  }
100 
101  // Align using the method of Umeyama
102  double s;
103  AlignUtils::align_umeyama(pos_est, pos_gt, R, t, s, true, true);
104  assert(s == 1);
105  }
106 }
107 
108 void AlignTrajectory::align_se3_single(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
109  const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t) {
110 
111  // Get first ever poses
112  Eigen::Vector4d q_es_0 = traj_es.at(0).block(3, 0, 4, 1);
113  Eigen::Vector3d p_es_0 = traj_es.at(0).block(0, 0, 3, 1);
114 
115  Eigen::Vector4d q_gt_0 = traj_gt.at(0).block(3, 0, 4, 1);
116  Eigen::Vector3d p_gt_0 = traj_gt.at(0).block(0, 0, 3, 1);
117 
118  // Get rotations from IMU frame to World (note JPL!)
119  Eigen::Matrix3d g_rot = ov_core::quat_2_Rot(q_gt_0).transpose();
120  Eigen::Matrix3d est_rot = ov_core::quat_2_Rot(q_es_0).transpose();
121 
122  R.noalias() = g_rot * est_rot.transpose();
123  t.noalias() = p_gt_0 - R * p_es_0;
124 }
125 
126 void AlignTrajectory::align_se3(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
127  const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t,
128  int n_aligned) {
129 
130  // If we only have one, just use the single alignment
131  if (n_aligned == 1) {
132  align_se3_single(traj_es, traj_gt, R, t);
133  } else {
134 
135  // Get just position vectors
136  assert(!traj_es.empty());
137  std::vector<Eigen::Vector3d> pos_est, pos_gt;
138  for (size_t i = 0; i < traj_es.size() && i < traj_gt.size(); i++) {
139  pos_est.push_back(traj_es.at(i).block(0, 0, 3, 1));
140  pos_gt.push_back(traj_gt.at(i).block(0, 0, 3, 1));
141  }
142 
143  // Align using the method of Umeyama
144  double s;
145  AlignUtils::align_umeyama(pos_est, pos_gt, R, t, s, true, false);
146  }
147 }
148 
149 void AlignTrajectory::align_sim3(const std::vector<Eigen::Matrix<double, 7, 1>> &traj_es,
150  const std::vector<Eigen::Matrix<double, 7, 1>> &traj_gt, Eigen::Matrix3d &R, Eigen::Vector3d &t, double &s,
151  int n_aligned) {
152 
153  // Need to have more than two to get
154  assert(n_aligned >= 2 || n_aligned == -1);
155 
156  // Get just position vectors
157  assert(!traj_es.empty());
158  std::vector<Eigen::Vector3d> pos_est, pos_gt;
159  for (size_t i = 0; i < traj_es.size() && i < traj_gt.size(); i++) {
160  pos_est.push_back(traj_es.at(i).block(0, 0, 3, 1));
161  pos_gt.push_back(traj_gt.at(i).block(0, 0, 3, 1));
162  }
163 
164  // Align using the method of Umeyama
165  AlignUtils::align_umeyama(pos_est, pos_gt, R, t, s, false, false);
166 }
ov_eval::AlignUtils::get_best_yaw
static double 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.
Definition: AlignUtils.h:53
ov_eval
Evaluation and recording utilities.
Definition: AlignTrajectory.h:36
ov_eval::AlignUtils::align_umeyama
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 betw...
Definition: AlignUtils.cpp:26
ov_core::rot_z
Eigen::Matrix< double, 3, 3 > rot_z(double t)
s
XmlRpcServer s
PRINT_ERROR
#define PRINT_ERROR(x...)
ov_eval::AlignTrajectory::align_se3
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.
Definition: AlignTrajectory.cpp:126
AlignTrajectory.h
ov_eval::AlignTrajectory::align_sim3
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.
Definition: AlignTrajectory.cpp:149
ov_core::quat_2_Rot
Eigen::Matrix< double, 3, 3 > quat_2_Rot(const Eigen::Matrix< double, 4, 1 > &q)
ov_eval::AlignTrajectory::align_se3_single
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.
Definition: AlignTrajectory.cpp:108
ov_eval::AlignTrajectory::align_trajectory
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.
Definition: AlignTrajectory.cpp:26
RESET
#define RESET
ov_eval::AlignTrajectory::align_posyaw
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 in...
Definition: AlignTrajectory.cpp:84
ov_eval::AlignTrajectory::align_posyaw_single
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 fi...
Definition: AlignTrajectory.cpp:57
RED
RED

ov_eval
Author(s): Patrick Geneva , Kevin Eckenhoff , Guoquan Huang
autogenerated on Mon Dec 16 2024 03:06:49