error_singlerun.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 <Eigen/Eigen>
23 #include <boost/filesystem.hpp>
24 #include <string>
25 
26 #include "calc/ResultTrajectory.h"
27 #include "utils/colors.h"
28 #include "utils/print.h"
29 
30 #ifdef HAVE_PYTHONLIBS
31 
32 // import the c++ wrapper for matplot lib
33 // https://github.com/lava/matplotlib-cpp
34 // sudo apt-get install python-matplotlib python-numpy python2.7-dev
35 #include "plot/matplotlibcpp.h"
36 
37 // Will plot three error values in three sub-plots in our current figure
38 void plot_3errors(ov_eval::Statistics sx, ov_eval::Statistics sy, ov_eval::Statistics sz) {
39 
40  // Parameters that define the line styles
41  std::map<std::string, std::string> params_value, params_bound;
42  params_value.insert({"label", "error"});
43  params_value.insert({"linestyle", "-"});
44  params_value.insert({"color", "blue"});
45  params_bound.insert({"label", "3 sigma bound"});
46  params_bound.insert({"linestyle", "--"});
47  params_bound.insert({"color", "red"});
48 
49  // Plot our error value
50  matplotlibcpp::subplot(3, 1, 1);
51  matplotlibcpp::plot(sx.timestamps, sx.values, params_value);
52  if (!sx.values_bound.empty()) {
53  matplotlibcpp::plot(sx.timestamps, sx.values_bound, params_bound);
54  for (size_t i = 0; i < sx.timestamps.size(); i++) {
55  sx.values_bound.at(i) *= -1;
56  }
57  matplotlibcpp::plot(sx.timestamps, sx.values_bound, "r--");
58  }
59 
60  // Plot our error value
61  matplotlibcpp::subplot(3, 1, 2);
62  matplotlibcpp::plot(sy.timestamps, sy.values, params_value);
63  if (!sy.values_bound.empty()) {
64  matplotlibcpp::plot(sy.timestamps, sy.values_bound, params_bound);
65  for (size_t i = 0; i < sy.timestamps.size(); i++) {
66  sy.values_bound.at(i) *= -1;
67  }
68  matplotlibcpp::plot(sy.timestamps, sy.values_bound, "r--");
69  }
70 
71  // Plot our error value
72  matplotlibcpp::subplot(3, 1, 3);
73  matplotlibcpp::plot(sz.timestamps, sz.values, params_value);
74  if (!sz.values_bound.empty()) {
75  matplotlibcpp::plot(sz.timestamps, sz.values_bound, params_bound);
76  for (size_t i = 0; i < sz.timestamps.size(); i++) {
77  sz.values_bound.at(i) *= -1;
78  }
79  matplotlibcpp::plot(sz.timestamps, sz.values_bound, "r--");
80  }
81 }
82 
83 #endif
84 
85 int main(int argc, char **argv) {
86 
87  // Verbosity setting
88  ov_core::Printer::setPrintLevel("INFO");
89 
90  // Ensure we have a path
91  if (argc < 4) {
92  PRINT_ERROR(RED "ERROR: Please specify a align mode, groudtruth, and algorithm run file\n" RESET);
93  PRINT_ERROR(RED "ERROR: ./error_singlerun <align_mode> <file_gt.txt> <file_est.txt>\n" RESET);
94  PRINT_ERROR(RED "ERROR: rosrun ov_eval error_singlerun <align_mode> <file_gt.txt> <file_est.txt>\n" RESET);
95  std::exit(EXIT_FAILURE);
96  }
97 
98  // Load it!
99  boost::filesystem::path path_gt(argv[2]);
100  std::vector<double> times;
101  std::vector<Eigen::Matrix<double, 7, 1>> poses;
102  std::vector<Eigen::Matrix3d> cov_ori, cov_pos;
103  ov_eval::Loader::load_data(argv[2], times, poses, cov_ori, cov_pos);
104  // Print its length and stats
105  double length = ov_eval::Loader::get_total_length(poses);
106  PRINT_DEBUG("[COMP]: %d poses in %s => length of %.2f meters\n", (int)times.size(), path_gt.stem().string().c_str(), length);
107 
108  // Create our trajectory object
109  ov_eval::ResultTrajectory traj(argv[3], argv[2], argv[1]);
110 
111  //===========================================================
112  // Absolute trajectory error
113  //===========================================================
114 
115  // Calculate
116  ov_eval::Statistics error_ori, error_pos;
117  traj.calculate_ate(error_ori, error_pos);
118 
119  // Print it
120  PRINT_INFO("======================================\n");
121  PRINT_INFO("Absolute Trajectory Error\n");
122  PRINT_INFO("======================================\n");
123  PRINT_INFO("rmse_ori = %.3f | rmse_pos = %.3f\n", error_ori.rmse, error_pos.rmse);
124  PRINT_INFO("mean_ori = %.3f | mean_pos = %.3f\n", error_ori.mean, error_pos.mean);
125  PRINT_INFO("min_ori = %.3f | min_pos = %.3f\n", error_ori.min, error_pos.min);
126  PRINT_INFO("max_ori = %.3f | max_pos = %.3f\n", error_ori.max, error_pos.max);
127  PRINT_INFO("std_ori = %.3f | std_pos = %.3f\n", error_ori.std, error_pos.std);
128 
129  //===========================================================
130  // Relative pose error
131  //===========================================================
132 
133  // Calculate
134  std::vector<double> segments = {8.0, 16.0, 24.0, 32.0, 40.0};
135  std::map<double, std::pair<ov_eval::Statistics, ov_eval::Statistics>> error_rpe;
136  traj.calculate_rpe(segments, error_rpe);
137 
138  // Print it
139  PRINT_INFO("======================================\n");
140  PRINT_INFO("Relative Pose Error\n");
141  PRINT_INFO("======================================\n");
142  for (const auto &seg : error_rpe) {
143  PRINT_INFO("seg %d - median_ori = %.3f | median_pos = %.3f (%d samples)\n", (int)seg.first, seg.second.first.median,
144  seg.second.second.median, (int)seg.second.second.values.size());
145  // PRINT_DEBUG("seg %d - std_ori = %.3f | std_pos = %.3f\n",(int)seg.first,seg.second.first.std,seg.second.second.std);
146  }
147 
148 #ifdef HAVE_PYTHONLIBS
149 
150  // Parameters
151  std::map<std::string, std::string> params_rpe;
152  params_rpe.insert({"notch", "true"});
153  params_rpe.insert({"sym", ""});
154 
155  // Plot this figure
156  matplotlibcpp::figure_size(800, 600);
157 
158  // Plot each RPE next to each other
159  double ct = 1;
160  double width = 0.50;
161  std::vector<double> xticks;
162  std::vector<std::string> labels;
163  for (const auto &seg : error_rpe) {
164  xticks.push_back(ct);
165  labels.push_back(std::to_string((int)seg.first));
166  matplotlibcpp::boxplot(seg.second.first.values, ct++, width, "blue", "-", params_rpe);
167  }
168 
169  // Display to the user
170  matplotlibcpp::xlim(0.5, ct - 0.5);
171  matplotlibcpp::xticks(xticks, labels);
172  matplotlibcpp::title("Relative Orientation Error");
173  matplotlibcpp::ylabel("orientation error (deg)");
174  matplotlibcpp::xlabel("sub-segment lengths (m)");
175  matplotlibcpp::tight_layout();
176  matplotlibcpp::show(false);
177 
178  // Plot this figure
179  matplotlibcpp::figure_size(800, 600);
180 
181  // Plot each RPE next to each other
182  ct = 1;
183  for (const auto &seg : error_rpe) {
184  matplotlibcpp::boxplot(seg.second.second.values, ct++, width, "blue", "-", params_rpe);
185  }
186 
187  // Display to the user
188  matplotlibcpp::xlim(0.5, ct - 0.5);
189  matplotlibcpp::xticks(xticks, labels);
190  matplotlibcpp::title("Relative Position Error");
191  matplotlibcpp::ylabel("translation error (m)");
192  matplotlibcpp::xlabel("sub-segment lengths (m)");
193  matplotlibcpp::tight_layout();
194  matplotlibcpp::show(false);
195 
196 #endif
197 
198  //===========================================================
199  // Normalized Estimation Error Squared
200  //===========================================================
201 
202  // Calculate
203  ov_eval::Statistics nees_ori, nees_pos;
204  traj.calculate_nees(nees_ori, nees_pos);
205 
206  // Print it
207  PRINT_INFO("======================================\n");
208  PRINT_INFO("Normalized Estimation Error Squared\n");
209  PRINT_INFO("======================================\n");
210  PRINT_INFO("mean_ori = %.3f | mean_pos = %.3f\n", nees_ori.mean, nees_pos.mean);
211  PRINT_INFO("min_ori = %.3f | min_pos = %.3f\n", nees_ori.min, nees_pos.min);
212  PRINT_INFO("max_ori = %.3f | max_pos = %.3f\n", nees_ori.max, nees_pos.max);
213  PRINT_INFO("std_ori = %.3f | std_pos = %.3f\n", nees_ori.std, nees_pos.std);
214  PRINT_INFO("======================================\n");
215 
216 #ifdef HAVE_PYTHONLIBS
217 
218  if (!nees_ori.values.empty() && !nees_pos.values.empty()) {
219  // Zero our time arrays
220  double starttime1 = (nees_ori.timestamps.empty()) ? 0 : nees_ori.timestamps.at(0);
221  double endtime1 = (nees_ori.timestamps.empty()) ? 0 : nees_ori.timestamps.at(nees_ori.timestamps.size() - 1);
222  for (size_t i = 0; i < nees_ori.timestamps.size(); i++) {
223  nees_ori.timestamps.at(i) -= starttime1;
224  nees_pos.timestamps.at(i) -= starttime1;
225  }
226 
227  // Plot this figure
228  matplotlibcpp::figure_size(1000, 600);
229 
230  // Parameters that define the line styles
231  std::map<std::string, std::string> params_neesp, params_neeso;
232  params_neesp.insert({"label", "nees position"});
233  params_neesp.insert({"linestyle", "-"});
234  params_neesp.insert({"color", "blue"});
235  params_neeso.insert({"label", "nees orientation"});
236  params_neeso.insert({"linestyle", "-"});
237  params_neeso.insert({"color", "blue"});
238 
239  // Update the title and axis labels
240  matplotlibcpp::subplot(2, 1, 1);
241  matplotlibcpp::title("Normalized Estimation Error Squared");
242  matplotlibcpp::ylabel("NEES Orientation");
243  matplotlibcpp::plot(nees_ori.timestamps, nees_ori.values, params_neeso);
244  matplotlibcpp::xlim(0.0, endtime1 - starttime1);
245  matplotlibcpp::subplot(2, 1, 2);
246  matplotlibcpp::ylabel("NEES Position");
247  matplotlibcpp::xlabel("dataset time (s)");
248  matplotlibcpp::plot(nees_pos.timestamps, nees_pos.values, params_neesp);
249  matplotlibcpp::xlim(0.0, endtime1 - starttime1);
250 
251  // Display to the user
252  matplotlibcpp::tight_layout();
253  matplotlibcpp::show(false);
254  }
255 
256 #endif
257 
258  //===========================================================
259  // Plot the error if we have matplotlib to plot!
260  //===========================================================
261 
262  // Calculate
263  ov_eval::Statistics posx, posy, posz;
264  ov_eval::Statistics orix, oriy, oriz;
265  ov_eval::Statistics roll, pitch, yaw;
266  traj.calculate_error(posx, posy, posz, orix, oriy, oriz, roll, pitch, yaw);
267 
268  // Zero our time arrays
269  double starttime2 = (posx.timestamps.empty()) ? 0 : posx.timestamps.at(0);
270  double endtime2 = (posx.timestamps.empty()) ? 0 : posx.timestamps.at(posx.timestamps.size() - 1);
271  for (size_t i = 0; i < posx.timestamps.size(); i++) {
272  posx.timestamps.at(i) -= starttime2;
273  posy.timestamps.at(i) -= starttime2;
274  posz.timestamps.at(i) -= starttime2;
275  orix.timestamps.at(i) -= starttime2;
276  oriy.timestamps.at(i) -= starttime2;
277  oriz.timestamps.at(i) -= starttime2;
278  roll.timestamps.at(i) -= starttime2;
279  pitch.timestamps.at(i) -= starttime2;
280  yaw.timestamps.at(i) -= starttime2;
281  }
282 
283 #ifdef HAVE_PYTHONLIBS
284 
285  //=====================================================
286  // Plot this figure
287  matplotlibcpp::figure_size(1000, 600);
288  plot_3errors(posx, posy, posz);
289 
290  // Update the title and axis labels
291  matplotlibcpp::subplot(3, 1, 1);
292  matplotlibcpp::title("IMU Position Error");
293  matplotlibcpp::ylabel("x-error (m)");
294  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
295  matplotlibcpp::subplot(3, 1, 2);
296  matplotlibcpp::ylabel("y-error (m)");
297  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
298  matplotlibcpp::subplot(3, 1, 3);
299  matplotlibcpp::ylabel("z-error (m)");
300  matplotlibcpp::xlabel("dataset time (s)");
301  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
302 
303  // Display to the user
304  matplotlibcpp::tight_layout();
305  matplotlibcpp::show(false);
306 
307  //=====================================================
308  // Plot this figure
309  matplotlibcpp::figure_size(1000, 600);
310  plot_3errors(orix, oriy, oriz);
311 
312  // Update the title and axis labels
313  matplotlibcpp::subplot(3, 1, 1);
314  matplotlibcpp::title("IMU Orientation Error");
315  matplotlibcpp::ylabel("x-error (deg)");
316  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
317  matplotlibcpp::subplot(3, 1, 2);
318  matplotlibcpp::ylabel("y-error (deg)");
319  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
320  matplotlibcpp::subplot(3, 1, 3);
321  matplotlibcpp::ylabel("z-error (deg)");
322  matplotlibcpp::xlabel("dataset time (s)");
323  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
324 
325  // Display to the user
326  matplotlibcpp::tight_layout();
327  matplotlibcpp::show(false);
328 
329  //=====================================================
330  // Plot this figure
331  matplotlibcpp::figure_size(1000, 600);
332  plot_3errors(roll, pitch, yaw);
333 
334  // Update the title and axis labels
335  matplotlibcpp::subplot(3, 1, 1);
336  matplotlibcpp::title("Global Orientation RPY Error");
337  matplotlibcpp::ylabel("roll error (deg)");
338  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
339  matplotlibcpp::subplot(3, 1, 2);
340  matplotlibcpp::ylabel("pitch error (deg)");
341  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
342  matplotlibcpp::subplot(3, 1, 3);
343  matplotlibcpp::ylabel("yaw error (deg)");
344  matplotlibcpp::xlabel("dataset time (s)");
345  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
346 
347  // Display to the user
348  matplotlibcpp::tight_layout();
349  matplotlibcpp::show(true);
350 
351 #endif
352 
353  // Done!
354  return EXIT_SUCCESS;
355 }
ov_eval::ResultTrajectory::calculate_rpe
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.
Definition: ResultTrajectory.cpp:140
ov_eval::Statistics::timestamps
std::vector< double > timestamps
Timestamp when these values occured at.
Definition: Statistics.h:64
matplotlibcpp::xlim
double * xlim()
ov_eval::ResultTrajectory::calculate_nees
void calculate_nees(Statistics &nees_ori, Statistics &nees_pos)
Computes the Normalized Estimation Error Squared (NEES) for this trajectory.
Definition: ResultTrajectory.cpp:244
ov_eval::Loader::get_total_length
static double get_total_length(const std::vector< Eigen::Matrix< double, 7, 1 >> &poses)
Will calculate the total trajectory distance.
Definition: Loader.cpp:388
ov_eval::Statistics::values
std::vector< double > values
Values (e.g. error or nees at a given time)
Definition: Statistics.h:67
PRINT_DEBUG
#define PRINT_DEBUG(x...)
ov_eval::Statistics::mean
double mean
Mean of the given values.
Definition: Statistics.h:46
xticks
void xticks(const std::vector< Numeric > &ticks, const std::map< std::string, std::string > &keywords)
matplotlibcpp::figure_size
void figure_size(size_t w, size_t h)
PRINT_ERROR
#define PRINT_ERROR(x...)
ov_eval::ResultTrajectory::calculate_error
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.
Definition: ResultTrajectory.cpp:292
ov_eval::Statistics::min
double min
Min of the given values.
Definition: Statistics.h:58
ov_core::Printer::setPrintLevel
static void setPrintLevel(const std::string &level)
ov_eval::ResultTrajectory
A single run for a given dataset.
Definition: ResultTrajectory.h:59
matplotlibcpp::plot
bool plot()
ov_eval::Statistics::std
double std
Standard deviation of given values.
Definition: Statistics.h:52
ov_eval::Loader::load_data
static void load_data(std::string path_traj, std::vector< double > &times, std::vector< Eigen::Matrix< double, 7, 1 >> &poses, std::vector< Eigen::Matrix3d > &cov_ori, std::vector< Eigen::Matrix3d > &cov_pos)
This will load space separated trajectory into memory.
Definition: Loader.cpp:26
main
int main(int argc, char **argv)
Definition: error_singlerun.cpp:85
matplotlibcpp::show
void show(const bool block=true)
ov_eval::Statistics::values_bound
std::vector< double > values_bound
Bound of these values (e.g. our expected covariance bound)
Definition: Statistics.h:70
ov_eval::Statistics::max
double max
Max of the given values.
Definition: Statistics.h:55
matplotlibcpp.h
ResultTrajectory.h
print.h
colors.h
matplotlibcpp::tight_layout
void tight_layout()
PRINT_INFO
#define PRINT_INFO(x...)
matplotlibcpp::subplot
void subplot(long nrows, long ncols, long plot_number)
RESET
#define RESET
matplotlibcpp::boxplot
bool boxplot(const std::vector< NumericX > &x, const double &position, const double &width, const std::string &color, const std::string &linestyle, const std::map< std::string, std::string > &keywords={}, bool vert=true)
ov_eval::Statistics
Statistics object for a given set scalar time series values.
Definition: Statistics.h:39
matplotlibcpp::xlabel
void xlabel(const std::string &str, const std::map< std::string, std::string > &keywords={})
ov_eval::ResultTrajectory::calculate_ate
void calculate_ate(Statistics &error_ori, Statistics &error_pos)
Computes the Absolute Trajectory Error (ATE) for this trajectory.
Definition: ResultTrajectory.cpp:82
matplotlibcpp::title
void title(const std::string &titlestr, const std::map< std::string, std::string > &keywords={})
RED
RED
ov_eval::Statistics::rmse
double rmse
Root mean squared for the given values.
Definition: Statistics.h:43
matplotlibcpp::ylabel
void ylabel(const std::string &str, const std::map< std::string, std::string > &keywords={})

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