ResultSimulation.h
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 #ifndef OV_EVAL_SIMULATION_H
23 #define OV_EVAL_SIMULATION_H
24 
25 #include <fstream>
26 #include <random>
27 #include <sstream>
28 #include <string>
29 #include <unordered_map>
30 
31 #include <Eigen/Eigen>
32 
33 #include "utils/Loader.h"
34 #include "utils/Statistics.h"
35 
36 #include "utils/colors.h"
37 #include "utils/print.h"
38 #include "utils/quat_ops.h"
39 
40 #ifdef HAVE_PYTHONLIBS
41 
42 // import the c++ wrapper for matplot lib
43 // https://github.com/lava/matplotlib-cpp
44 // sudo apt-get install python-matplotlib python-numpy python2.7-dev
45 #include "plot/matplotlibcpp.h"
46 
47 #endif
48 
49 namespace ov_eval {
50 
59 class ResultSimulation {
60 
61 public:
68  ResultSimulation(std::string path_est, std::string path_std, std::string path_gt);
69 
75  void plot_state(bool doplotting, double max_time = INFINITY);
76 
82  void plot_timeoff(bool doplotting, double max_time = INFINITY);
83 
89  void plot_cam_instrinsics(bool doplotting, double max_time = INFINITY);
90 
96  void plot_cam_extrinsics(bool doplotting, double max_time = INFINITY);
97 
103  void plot_imu_intrinsics(bool doplotting, double max_time = INFINITY);
104 
105 protected:
106  // Trajectory data (loaded from file and timestamp intersected)
107  std::vector<Eigen::VectorXd> est_state, gt_state;
108  std::vector<Eigen::VectorXd> state_cov;
109 
110 #ifdef HAVE_PYTHONLIBS
111 
120  void plot_3errors(ov_eval::Statistics sx, ov_eval::Statistics sy, ov_eval::Statistics sz, std::string color_err, std::string color_std) {
121 
122  // Zero our time arrays
123  double starttime1 = (sx.timestamps.empty()) ? 0 : sx.timestamps.at(0);
124  double endtime1 = (sx.timestamps.empty()) ? 0 : sx.timestamps.at(sx.timestamps.size() - 1);
125  for (size_t i = 0; i < sx.timestamps.size(); i++) {
126  sx.timestamps.at(i) -= starttime1;
127  }
128  double starttime2 = (sy.timestamps.empty()) ? 0 : sy.timestamps.at(0);
129  double endtime2 = (sy.timestamps.empty()) ? 0 : sy.timestamps.at(sy.timestamps.size() - 1);
130  for (size_t i = 0; i < sy.timestamps.size(); i++) {
131  sy.timestamps.at(i) -= starttime2;
132  }
133  double starttime3 = (sz.timestamps.empty()) ? 0 : sz.timestamps.at(0);
134  double endtime3 = (sz.timestamps.empty()) ? 0 : sz.timestamps.at(sz.timestamps.size() - 1);
135  for (size_t i = 0; i < sz.timestamps.size(); i++) {
136  sz.timestamps.at(i) -= starttime3;
137  }
138 
139  // Parameters that define the line styles
140  std::map<std::string, std::string> params_value, params_bound;
141  // params_value.insert({"label","error"});
142  params_value.insert({"linestyle", "-"});
143  params_value.insert({"color", color_err});
144  // params_bound.insert({"label","3 sigma bound"});
145  params_bound.insert({"linestyle", "--"});
146  params_bound.insert({"color", color_std});
147 
148  // Plot our error value
149  matplotlibcpp::subplot(3, 1, 1);
150  matplotlibcpp::plot(sx.timestamps, sx.values, params_value);
151  if (!sx.values_bound.empty()) {
152  matplotlibcpp::plot(sx.timestamps, sx.values_bound, params_bound);
153  for (size_t i = 0; i < sx.timestamps.size(); i++) {
154  sx.values_bound.at(i) *= -1;
155  }
156  matplotlibcpp::plot(sx.timestamps, sx.values_bound, params_bound);
157  }
158  matplotlibcpp::xlim(0.0, endtime1 - starttime1);
159 
160  // Plot our error value
161  matplotlibcpp::subplot(3, 1, 2);
162  matplotlibcpp::plot(sy.timestamps, sy.values, params_value);
163  if (!sy.values_bound.empty()) {
164  matplotlibcpp::plot(sy.timestamps, sy.values_bound, params_bound);
165  for (size_t i = 0; i < sy.timestamps.size(); i++) {
166  sy.values_bound.at(i) *= -1;
167  }
168  matplotlibcpp::plot(sy.timestamps, sy.values_bound, params_bound);
169  }
170  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
171 
172  // Plot our error value
173  matplotlibcpp::subplot(3, 1, 3);
174  matplotlibcpp::plot(sz.timestamps, sz.values, params_value);
175  if (!sz.values_bound.empty()) {
176  matplotlibcpp::plot(sz.timestamps, sz.values_bound, params_bound);
177  for (size_t i = 0; i < sz.timestamps.size(); i++) {
178  sz.values_bound.at(i) *= -1;
179  }
180  matplotlibcpp::plot(sz.timestamps, sz.values_bound, params_bound);
181  }
182  matplotlibcpp::xlim(0.0, endtime3 - starttime3);
183  }
184 
194  void plot_4errors(ov_eval::Statistics sx, ov_eval::Statistics sy, ov_eval::Statistics sz, ov_eval::Statistics sk, std::string color_err,
195  std::string color_std) {
196 
197  // Zero our time arrays
198  double starttime1 = (sx.timestamps.empty()) ? 0 : sx.timestamps.at(0);
199  double endtime1 = (sx.timestamps.empty()) ? 0 : sx.timestamps.at(sx.timestamps.size() - 1);
200  for (size_t i = 0; i < sx.timestamps.size(); i++) {
201  sx.timestamps.at(i) -= starttime1;
202  }
203  double starttime2 = (sy.timestamps.empty()) ? 0 : sy.timestamps.at(0);
204  double endtime2 = (sy.timestamps.empty()) ? 0 : sy.timestamps.at(sy.timestamps.size() - 1);
205  for (size_t i = 0; i < sy.timestamps.size(); i++) {
206  sy.timestamps.at(i) -= starttime2;
207  }
208  double starttime3 = (sz.timestamps.empty()) ? 0 : sz.timestamps.at(0);
209  double endtime3 = (sz.timestamps.empty()) ? 0 : sz.timestamps.at(sz.timestamps.size() - 1);
210  for (size_t i = 0; i < sz.timestamps.size(); i++) {
211  sz.timestamps.at(i) -= starttime3;
212  }
213  double starttime4 = (sk.timestamps.empty()) ? 0 : sk.timestamps.at(0);
214  double endtime4 = (sk.timestamps.empty()) ? 0 : sk.timestamps.at(sk.timestamps.size() - 1);
215  for (size_t i = 0; i < sk.timestamps.size(); i++) {
216  sk.timestamps.at(i) -= starttime4;
217  }
218 
219  // Parameters that define the line styles
220  std::map<std::string, std::string> params_value, params_bound;
221  // params_value.insert({"label","error"});
222  params_value.insert({"linestyle", "-"});
223  params_value.insert({"color", color_err});
224  // params_bound.insert({"label","3 sigma bound"});
225  params_bound.insert({"linestyle", "--"});
226  params_bound.insert({"color", color_std});
227 
228  // Plot our error value
229  matplotlibcpp::subplot(4, 1, 1);
230  matplotlibcpp::plot(sx.timestamps, sx.values, params_value);
231  if (!sx.values_bound.empty()) {
232  matplotlibcpp::plot(sx.timestamps, sx.values_bound, params_bound);
233  for (size_t i = 0; i < sx.timestamps.size(); i++) {
234  sx.values_bound.at(i) *= -1;
235  }
236  matplotlibcpp::plot(sx.timestamps, sx.values_bound, params_bound);
237  }
238  matplotlibcpp::xlim(0.0, endtime1 - starttime1);
239 
240  // Plot our error value
241  matplotlibcpp::subplot(4, 1, 2);
242  matplotlibcpp::plot(sy.timestamps, sy.values, params_value);
243  if (!sy.values_bound.empty()) {
244  matplotlibcpp::plot(sy.timestamps, sy.values_bound, params_bound);
245  for (size_t i = 0; i < sy.timestamps.size(); i++) {
246  sy.values_bound.at(i) *= -1;
247  }
248  matplotlibcpp::plot(sy.timestamps, sy.values_bound, params_bound);
249  }
250  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
251 
252  // Plot our error value
253  matplotlibcpp::subplot(4, 1, 3);
254  matplotlibcpp::plot(sz.timestamps, sz.values, params_value);
255  if (!sz.values_bound.empty()) {
256  matplotlibcpp::plot(sz.timestamps, sz.values_bound, params_bound);
257  for (size_t i = 0; i < sz.timestamps.size(); i++) {
258  sz.values_bound.at(i) *= -1;
259  }
260  matplotlibcpp::plot(sz.timestamps, sz.values_bound, params_bound);
261  }
262  matplotlibcpp::xlim(0.0, endtime3 - starttime3);
263 
264  // Plot our error value
265  matplotlibcpp::subplot(4, 1, 4);
266  matplotlibcpp::plot(sk.timestamps, sk.values, params_value);
267  if (!sk.values_bound.empty()) {
268  matplotlibcpp::plot(sk.timestamps, sk.values_bound, params_bound);
269  for (size_t i = 0; i < sk.timestamps.size(); i++) {
270  sk.values_bound.at(i) *= -1;
271  }
272  matplotlibcpp::plot(sk.timestamps, sk.values_bound, params_bound);
273  }
274  matplotlibcpp::xlim(0.0, endtime4 - starttime4);
275  }
287  void plot_6errors(ov_eval::Statistics s1, ov_eval::Statistics s2, ov_eval::Statistics s3, ov_eval::Statistics s4, ov_eval::Statistics s5,
288  ov_eval::Statistics s6, std::string color_err, std::string color_std) {
289 
290  // Zero our time arrays
291  double starttime1 = (s1.timestamps.empty()) ? 0 : s1.timestamps.at(0);
292  double endtime1 = (s1.timestamps.empty()) ? 0 : s1.timestamps.at(s1.timestamps.size() - 1);
293  for (size_t i = 0; i < s1.timestamps.size(); i++) {
294  s1.timestamps.at(i) -= starttime1;
295  }
296  double starttime2 = (s2.timestamps.empty()) ? 0 : s2.timestamps.at(0);
297  double endtime2 = (s2.timestamps.empty()) ? 0 : s2.timestamps.at(s2.timestamps.size() - 1);
298  for (size_t i = 0; i < s2.timestamps.size(); i++) {
299  s2.timestamps.at(i) -= starttime2;
300  }
301  double starttime3 = (s3.timestamps.empty()) ? 0 : s3.timestamps.at(0);
302  double endtime3 = (s3.timestamps.empty()) ? 0 : s3.timestamps.at(s3.timestamps.size() - 1);
303  for (size_t i = 0; i < s3.timestamps.size(); i++) {
304  s3.timestamps.at(i) -= starttime3;
305  }
306  double starttime4 = (s4.timestamps.empty()) ? 0 : s4.timestamps.at(0);
307  double endtime4 = (s4.timestamps.empty()) ? 0 : s4.timestamps.at(s4.timestamps.size() - 1);
308  for (size_t i = 0; i < s4.timestamps.size(); i++) {
309  s4.timestamps.at(i) -= starttime4;
310  }
311  double starttime5 = (s5.timestamps.empty()) ? 0 : s5.timestamps.at(0);
312  double endtime5 = (s5.timestamps.empty()) ? 0 : s5.timestamps.at(s5.timestamps.size() - 1);
313  for (size_t i = 0; i < s5.timestamps.size(); i++) {
314  s5.timestamps.at(i) -= starttime5;
315  }
316  double starttime6 = (s6.timestamps.empty()) ? 0 : s6.timestamps.at(0);
317  double endtime6 = (s6.timestamps.empty()) ? 0 : s6.timestamps.at(s6.timestamps.size() - 1);
318  for (size_t i = 0; i < s6.timestamps.size(); i++) {
319  s6.timestamps.at(i) -= starttime6;
320  }
321 
322  // Parameters that define the line styles
323  std::map<std::string, std::string> params_value, params_bound;
324  // params_value.insert({"label","error"});
325  params_value.insert({"linestyle", "-"});
326  params_value.insert({"color", color_err});
327  // params_bound.insert({"label","3 sigma bound"});
328  params_bound.insert({"linestyle", "--"});
329  params_bound.insert({"color", color_std});
330 
331  // Plot our error value
332  matplotlibcpp::subplot(3, 2, 1);
333  matplotlibcpp::plot(s1.timestamps, s1.values, params_value);
334  if (!s1.values_bound.empty()) {
335  matplotlibcpp::plot(s1.timestamps, s1.values_bound, params_bound);
336  for (size_t i = 0; i < s1.timestamps.size(); i++) {
337  s1.values_bound.at(i) *= -1;
338  }
339  matplotlibcpp::plot(s1.timestamps, s1.values_bound, params_bound);
340  }
341  matplotlibcpp::xlim(0.0, endtime1 - starttime1);
342 
343  // Plot our error value
344  matplotlibcpp::subplot(3, 2, 2);
345  matplotlibcpp::plot(s2.timestamps, s2.values, params_value);
346  if (!s2.values_bound.empty()) {
347  matplotlibcpp::plot(s2.timestamps, s2.values_bound, params_bound);
348  for (size_t i = 0; i < s2.timestamps.size(); i++) {
349  s2.values_bound.at(i) *= -1;
350  }
351  matplotlibcpp::plot(s2.timestamps, s2.values_bound, params_bound);
352  }
353  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
354 
355  // Plot our error value
356  matplotlibcpp::subplot(3, 2, 3);
357  matplotlibcpp::plot(s3.timestamps, s3.values, params_value);
358  if (!s3.values_bound.empty()) {
359  matplotlibcpp::plot(s3.timestamps, s3.values_bound, params_bound);
360  for (size_t i = 0; i < s3.timestamps.size(); i++) {
361  s3.values_bound.at(i) *= -1;
362  }
363  matplotlibcpp::plot(s3.timestamps, s3.values_bound, params_bound);
364  }
365  matplotlibcpp::xlim(0.0, endtime3 - starttime3);
366 
367  // Plot our error value
368  matplotlibcpp::subplot(3, 2, 4);
369  matplotlibcpp::plot(s4.timestamps, s4.values, params_value);
370  if (!s4.values_bound.empty()) {
371  matplotlibcpp::plot(s4.timestamps, s4.values_bound, params_bound);
372  for (size_t i = 0; i < s4.timestamps.size(); i++) {
373  s4.values_bound.at(i) *= -1;
374  }
375  matplotlibcpp::plot(s4.timestamps, s4.values_bound, params_bound);
376  }
377  matplotlibcpp::xlim(0.0, endtime4 - starttime4);
378 
379  // Plot our error value
380  matplotlibcpp::subplot(3, 2, 5);
381  matplotlibcpp::plot(s5.timestamps, s5.values, params_value);
382  if (!s5.values_bound.empty()) {
383  matplotlibcpp::plot(s5.timestamps, s5.values_bound, params_bound);
384  for (size_t i = 0; i < s5.timestamps.size(); i++) {
385  s5.values_bound.at(i) *= -1;
386  }
387  matplotlibcpp::plot(s5.timestamps, s5.values_bound, params_bound);
388  }
389  matplotlibcpp::xlim(0.0, endtime5 - starttime5);
390 
391  // Plot our error value
392  matplotlibcpp::subplot(3, 2, 6);
393  matplotlibcpp::plot(s6.timestamps, s6.values, params_value);
394  if (!s6.values_bound.empty()) {
395  matplotlibcpp::plot(s6.timestamps, s6.values_bound, params_bound);
396  for (size_t i = 0; i < s6.timestamps.size(); i++) {
397  s6.values_bound.at(i) *= -1;
398  }
399  matplotlibcpp::plot(s6.timestamps, s6.values_bound, params_bound);
400  }
401  matplotlibcpp::xlim(0.0, endtime6 - starttime6);
402  }
403 
418  void plot_9errors(ov_eval::Statistics s1, ov_eval::Statistics s2, ov_eval::Statistics s3, ov_eval::Statistics s4, ov_eval::Statistics s5,
419  ov_eval::Statistics s6, ov_eval::Statistics s7, ov_eval::Statistics s8, ov_eval::Statistics s9, std::string color_err,
420  std::string color_std) {
421 
422  // Zero our time arrays
423  double starttime1 = (s1.timestamps.empty()) ? 0 : s1.timestamps.at(0);
424  double endtime1 = (s1.timestamps.empty()) ? 0 : s1.timestamps.at(s1.timestamps.size() - 1);
425  for (size_t i = 0; i < s1.timestamps.size(); i++) {
426  s1.timestamps.at(i) -= starttime1;
427  }
428  double starttime2 = (s2.timestamps.empty()) ? 0 : s2.timestamps.at(0);
429  double endtime2 = (s2.timestamps.empty()) ? 0 : s2.timestamps.at(s2.timestamps.size() - 1);
430  for (size_t i = 0; i < s2.timestamps.size(); i++) {
431  s2.timestamps.at(i) -= starttime2;
432  }
433  double starttime3 = (s3.timestamps.empty()) ? 0 : s3.timestamps.at(0);
434  double endtime3 = (s3.timestamps.empty()) ? 0 : s3.timestamps.at(s3.timestamps.size() - 1);
435  for (size_t i = 0; i < s3.timestamps.size(); i++) {
436  s3.timestamps.at(i) -= starttime3;
437  }
438  double starttime4 = (s4.timestamps.empty()) ? 0 : s4.timestamps.at(0);
439  double endtime4 = (s4.timestamps.empty()) ? 0 : s4.timestamps.at(s4.timestamps.size() - 1);
440  for (size_t i = 0; i < s4.timestamps.size(); i++) {
441  s4.timestamps.at(i) -= starttime4;
442  }
443  double starttime5 = (s5.timestamps.empty()) ? 0 : s5.timestamps.at(0);
444  double endtime5 = (s5.timestamps.empty()) ? 0 : s5.timestamps.at(s5.timestamps.size() - 1);
445  for (size_t i = 0; i < s5.timestamps.size(); i++) {
446  s5.timestamps.at(i) -= starttime5;
447  }
448  double starttime6 = (s6.timestamps.empty()) ? 0 : s6.timestamps.at(0);
449  double endtime6 = (s6.timestamps.empty()) ? 0 : s6.timestamps.at(s6.timestamps.size() - 1);
450  for (size_t i = 0; i < s6.timestamps.size(); i++) {
451  s6.timestamps.at(i) -= starttime6;
452  }
453  double starttime7 = (s7.timestamps.empty()) ? 0 : s7.timestamps.at(0);
454  double endtime7 = (s7.timestamps.empty()) ? 0 : s7.timestamps.at(s7.timestamps.size() - 1);
455  for (size_t i = 0; i < s7.timestamps.size(); i++) {
456  s7.timestamps.at(i) -= starttime7;
457  }
458  double starttime8 = (s8.timestamps.empty()) ? 0 : s8.timestamps.at(0);
459  double endtime8 = (s8.timestamps.empty()) ? 0 : s8.timestamps.at(s8.timestamps.size() - 1);
460  for (size_t i = 0; i < s8.timestamps.size(); i++) {
461  s8.timestamps.at(i) -= starttime8;
462  }
463  double starttime9 = (s9.timestamps.empty()) ? 0 : s9.timestamps.at(0);
464  double endtime9 = (s9.timestamps.empty()) ? 0 : s9.timestamps.at(s9.timestamps.size() - 1);
465  for (size_t i = 0; i < s9.timestamps.size(); i++) {
466  s9.timestamps.at(i) -= starttime9;
467  }
468 
469  // Parameters that define the line styles
470  std::map<std::string, std::string> params_value, params_bound;
471  // params_value.insert({"label","error"});
472  params_value.insert({"linestyle", "-"});
473  params_value.insert({"color", color_err});
474  // params_bound.insert({"label","3 sigma bound"});
475  params_bound.insert({"linestyle", "--"});
476  params_bound.insert({"color", color_std});
477 
478  // Plot our error value
479  matplotlibcpp::subplot(3, 3, 1);
480  matplotlibcpp::plot(s1.timestamps, s1.values, params_value);
481  if (!s1.values_bound.empty()) {
482  matplotlibcpp::plot(s1.timestamps, s1.values_bound, params_bound);
483  for (size_t i = 0; i < s1.timestamps.size(); i++) {
484  s1.values_bound.at(i) *= -1;
485  }
486  matplotlibcpp::plot(s1.timestamps, s1.values_bound, params_bound);
487  }
488  matplotlibcpp::xlim(0.0, endtime1 - starttime1);
489 
490  // Plot our error value
491  matplotlibcpp::subplot(3, 3, 2);
492  matplotlibcpp::plot(s2.timestamps, s2.values, params_value);
493  if (!s2.values_bound.empty()) {
494  matplotlibcpp::plot(s2.timestamps, s2.values_bound, params_bound);
495  for (size_t i = 0; i < s2.timestamps.size(); i++) {
496  s2.values_bound.at(i) *= -1;
497  }
498  matplotlibcpp::plot(s2.timestamps, s2.values_bound, params_bound);
499  }
500  matplotlibcpp::xlim(0.0, endtime2 - starttime2);
501 
502  // Plot our error value
503  matplotlibcpp::subplot(3, 3, 3);
504  matplotlibcpp::plot(s3.timestamps, s3.values, params_value);
505  if (!s3.values_bound.empty()) {
506  matplotlibcpp::plot(s3.timestamps, s3.values_bound, params_bound);
507  for (size_t i = 0; i < s3.timestamps.size(); i++) {
508  s3.values_bound.at(i) *= -1;
509  }
510  matplotlibcpp::plot(s3.timestamps, s3.values_bound, params_bound);
511  }
512  matplotlibcpp::xlim(0.0, endtime3 - starttime3);
513 
514  // Plot our error value
515  matplotlibcpp::subplot(3, 3, 4);
516  matplotlibcpp::plot(s4.timestamps, s4.values, params_value);
517  if (!s4.values_bound.empty()) {
518  matplotlibcpp::plot(s4.timestamps, s4.values_bound, params_bound);
519  for (size_t i = 0; i < s4.timestamps.size(); i++) {
520  s4.values_bound.at(i) *= -1;
521  }
522  matplotlibcpp::plot(s4.timestamps, s4.values_bound, params_bound);
523  }
524  matplotlibcpp::xlim(0.0, endtime4 - starttime4);
525 
526  // Plot our error value
527  matplotlibcpp::subplot(3, 3, 5);
528  matplotlibcpp::plot(s5.timestamps, s5.values, params_value);
529  if (!s5.values_bound.empty()) {
530  matplotlibcpp::plot(s5.timestamps, s5.values_bound, params_bound);
531  for (size_t i = 0; i < s5.timestamps.size(); i++) {
532  s5.values_bound.at(i) *= -1;
533  }
534  matplotlibcpp::plot(s5.timestamps, s5.values_bound, params_bound);
535  }
536  matplotlibcpp::xlim(0.0, endtime5 - starttime5);
537 
538  // Plot our error value
539  matplotlibcpp::subplot(3, 3, 6);
540  matplotlibcpp::plot(s6.timestamps, s6.values, params_value);
541  if (!s6.values_bound.empty()) {
542  matplotlibcpp::plot(s6.timestamps, s6.values_bound, params_bound);
543  for (size_t i = 0; i < s6.timestamps.size(); i++) {
544  s6.values_bound.at(i) *= -1;
545  }
546  matplotlibcpp::plot(s6.timestamps, s6.values_bound, params_bound);
547  }
548  matplotlibcpp::xlim(0.0, endtime6 - starttime6);
549 
550  // Plot our error value
551  matplotlibcpp::subplot(3, 3, 7);
552  matplotlibcpp::plot(s7.timestamps, s7.values, params_value);
553  if (!s7.values_bound.empty()) {
554  matplotlibcpp::plot(s7.timestamps, s7.values_bound, params_bound);
555  for (size_t i = 0; i < s7.timestamps.size(); i++) {
556  s7.values_bound.at(i) *= -1;
557  }
558  matplotlibcpp::plot(s7.timestamps, s7.values_bound, params_bound);
559  }
560  matplotlibcpp::xlim(0.0, endtime7 - starttime7);
561 
562  // Plot our error value
563  matplotlibcpp::subplot(3, 3, 8);
564  matplotlibcpp::plot(s8.timestamps, s8.values, params_value);
565  if (!s8.values_bound.empty()) {
566  matplotlibcpp::plot(s8.timestamps, s8.values_bound, params_bound);
567  for (size_t i = 0; i < s8.timestamps.size(); i++) {
568  s8.values_bound.at(i) *= -1;
569  }
570  matplotlibcpp::plot(s8.timestamps, s8.values_bound, params_bound);
571  }
572  matplotlibcpp::xlim(0.0, endtime8 - starttime8);
573 
574  // Plot our error value
575  matplotlibcpp::subplot(3, 3, 9);
576  matplotlibcpp::plot(s9.timestamps, s9.values, params_value);
577  if (!s9.values_bound.empty()) {
578  matplotlibcpp::plot(s9.timestamps, s9.values_bound, params_bound);
579  for (size_t i = 0; i < s9.timestamps.size(); i++) {
580  s9.values_bound.at(i) *= -1;
581  }
582  matplotlibcpp::plot(s9.timestamps, s9.values_bound, params_bound);
583  }
584  matplotlibcpp::xlim(0.0, endtime9 - starttime9);
585  }
586 
587 #endif
588 };
589 
590 } // namespace ov_eval
591 
592 #endif // OV_EVAL_SIMULATION_H
ov_eval::ResultSimulation::est_state
std::vector< Eigen::VectorXd > est_state
Definition: ResultSimulation.h:107
ov_eval::Statistics::timestamps
std::vector< double > timestamps
Timestamp when these values occured at.
Definition: Statistics.h:64
matplotlibcpp::xlim
double * xlim()
ov_eval::ResultSimulation::state_cov
std::vector< Eigen::VectorXd > state_cov
Definition: ResultSimulation.h:108
quat_ops.h
ov_eval
Evaluation and recording utilities.
Definition: AlignTrajectory.h:36
Loader.h
ov_eval::Statistics::values
std::vector< double > values
Values (e.g. error or nees at a given time)
Definition: Statistics.h:67
ov_eval::ResultSimulation::plot_cam_instrinsics
void plot_cam_instrinsics(bool doplotting, double max_time=INFINITY)
Will plot the camera calibration intrinsics.
Definition: ResultSimulation.cpp:280
ov_eval::ResultSimulation::plot_cam_extrinsics
void plot_cam_extrinsics(bool doplotting, double max_time=INFINITY)
Will plot the camera calibration extrinsic transform.
Definition: ResultSimulation.cpp:390
ov_eval::ResultSimulation::plot_imu_intrinsics
void plot_imu_intrinsics(bool doplotting, double max_time=INFINITY)
Will plot the imu intrinsic errors.
Definition: ResultSimulation.cpp:502
ov_eval::ResultSimulation::ResultSimulation
ResultSimulation(std::string path_est, std::string path_std, std::string path_gt)
Default constructor that will load our data from file.
Definition: ResultSimulation.cpp:26
matplotlibcpp::plot
bool plot()
ov_eval::ResultSimulation::gt_state
std::vector< Eigen::VectorXd > gt_state
Definition: ResultSimulation.h:107
ov_eval::Statistics::values_bound
std::vector< double > values_bound
Bound of these values (e.g. our expected covariance bound)
Definition: Statistics.h:70
matplotlibcpp.h
print.h
colors.h
matplotlibcpp::subplot
void subplot(long nrows, long ncols, long plot_number)
Statistics.h
ov_eval::Statistics
Statistics object for a given set scalar time series values.
Definition: Statistics.h:39
ov_eval::ResultSimulation::plot_state
void plot_state(bool doplotting, double max_time=INFINITY)
Will plot the state error and its three sigma bounds.
Definition: ResultSimulation.cpp:42
ov_eval::ResultSimulation
A single simulation run (the full state not just pose).
Definition: ResultSimulation.h:59
ov_eval::ResultSimulation::plot_timeoff
void plot_timeoff(bool doplotting, double max_time=INFINITY)
Will plot the state imu camera offset and its sigma bound.
Definition: ResultSimulation.cpp:204

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