ResultSimulation.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 "ResultSimulation.h"
23 
24 using namespace ov_eval;
25 
26 ResultSimulation::ResultSimulation(std::string path_est, std::string path_std, std::string path_gt) {
27 
28  // Load from disk
29  Loader::load_simulation(path_est, est_state);
30  Loader::load_simulation(path_std, state_cov);
31  Loader::load_simulation(path_gt, gt_state);
32 
34  assert(est_state.size() == state_cov.size());
35  assert(est_state.size() == gt_state.size());
36 
37  // Debug print
38  PRINT_DEBUG("[SIM]: loaded %d timestamps from file!!\n", (int)est_state.size());
39  PRINT_DEBUG("[SIM]: we have %d cameras in total!!\n", (int)est_state.at(0)(18));
40 }
41 
42 void ResultSimulation::plot_state(bool doplotting, double max_time) {
43 
44  // Errors for each xyz direction
45  Statistics error_ori[3], error_pos[3], error_vel[3], error_bg[3], error_ba[3];
46 
47  // Calculate the position and orientation error at every timestep
48  double start_time = est_state.at(0)(0);
49  for (size_t i = 0; i < est_state.size(); i++) {
50 
51  // Exit if we have reached our max time
52  if ((est_state.at(i)(0) - start_time) > max_time)
53  break;
54 
55  // Assert our times are the same
56  assert(est_state.at(i)(0) == gt_state.at(i)(0));
57 
58  // Calculate orientation error
59  // NOTE: we define our error as e_R = -Log(R*Rhat^T)
60  Eigen::Matrix3d e_R =
61  ov_core::quat_2_Rot(gt_state.at(i).block(1, 0, 4, 1)) * ov_core::quat_2_Rot(est_state.at(i).block(1, 0, 4, 1)).transpose();
62  Eigen::Vector3d ori_err = -180.0 / M_PI * ov_core::log_so3(e_R);
63  for (int j = 0; j < 3; j++) {
64  error_ori[j].timestamps.push_back(est_state.at(i)(0));
65  error_ori[j].values.push_back(ori_err(j));
66  error_ori[j].values_bound.push_back(3 * 180.0 / M_PI * state_cov.at(i)(1 + j));
67  error_ori[j].calculate();
68  }
69 
70  // Calculate position error
71  Eigen::Vector3d pos_err = gt_state.at(i).block(5, 0, 3, 1) - est_state.at(i).block(5, 0, 3, 1);
72  for (int j = 0; j < 3; j++) {
73  error_pos[j].timestamps.push_back(est_state.at(i)(0));
74  error_pos[j].values.push_back(pos_err(j));
75  error_pos[j].values_bound.push_back(3 * state_cov.at(i)(4 + j));
76  error_pos[j].calculate();
77  }
78 
79  // Calculate velocity error
80  Eigen::Vector3d vel_err = gt_state.at(i).block(8, 0, 3, 1) - est_state.at(i).block(8, 0, 3, 1);
81  for (int j = 0; j < 3; j++) {
82  error_vel[j].timestamps.push_back(est_state.at(i)(0));
83  error_vel[j].values.push_back(vel_err(j));
84  error_vel[j].values_bound.push_back(3 * state_cov.at(i)(7 + j));
85  error_vel[j].calculate();
86  }
87 
88  // Calculate gyro bias error
89  Eigen::Vector3d bg_err = gt_state.at(i).block(11, 0, 3, 1) - est_state.at(i).block(11, 0, 3, 1);
90  for (int j = 0; j < 3; j++) {
91  error_bg[j].timestamps.push_back(est_state.at(i)(0));
92  error_bg[j].values.push_back(bg_err(j));
93  error_bg[j].values_bound.push_back(3 * state_cov.at(i)(10 + j));
94  error_bg[j].calculate();
95  }
96 
97  // Calculate accel bias error
98  Eigen::Vector3d ba_err = gt_state.at(i).block(14, 0, 3, 1) - est_state.at(i).block(14, 0, 3, 1);
99  for (int j = 0; j < 3; j++) {
100  error_ba[j].timestamps.push_back(est_state.at(i)(0));
101  error_ba[j].values.push_back(ba_err(j));
102  error_ba[j].values_bound.push_back(3 * state_cov.at(i)(13 + j));
103  error_ba[j].calculate();
104  }
105  }
106 
107  // return if we don't want to plot
108  if (!doplotting)
109  return;
110 
111 #ifndef HAVE_PYTHONLIBS
112  PRINT_ERROR(RED "Unable to plot the state error, just returning..\n" RESET);
113  return;
114 #else
115 
116  //=====================================================
117  // Plot this figure
118  matplotlibcpp::figure_size(1000, 500);
119  plot_3errors(error_ori[0], error_ori[1], error_ori[2], "blue", "red");
120  // Update the title and axis labels
121  matplotlibcpp::subplot(3, 1, 1);
122  matplotlibcpp::title("IMU Orientation Error");
123  matplotlibcpp::ylabel("x-error (deg)");
124  matplotlibcpp::subplot(3, 1, 2);
125  matplotlibcpp::ylabel("y-error (deg)");
126  matplotlibcpp::subplot(3, 1, 3);
127  matplotlibcpp::ylabel("z-error (deg)");
128  matplotlibcpp::xlabel("dataset time (s)");
129  matplotlibcpp::tight_layout();
130  matplotlibcpp::show(false);
131  //=====================================================
132 
133  //=====================================================
134  // Plot this figure
135  matplotlibcpp::figure_size(1000, 500);
136  plot_3errors(error_pos[0], error_pos[1], error_pos[2], "blue", "red");
137  // Update the title and axis labels
138  matplotlibcpp::subplot(3, 1, 1);
139  matplotlibcpp::title("IMU Position Error");
140  matplotlibcpp::ylabel("x-error (m)");
141  matplotlibcpp::subplot(3, 1, 2);
142  matplotlibcpp::ylabel("y-error (m)");
143  matplotlibcpp::subplot(3, 1, 3);
144  matplotlibcpp::ylabel("z-error (m)");
145  matplotlibcpp::xlabel("dataset time (s)");
146  matplotlibcpp::tight_layout();
147  matplotlibcpp::show(false);
148  //=====================================================
149 
150  //=====================================================
151  // Plot this figure
152  matplotlibcpp::figure_size(1000, 500);
153  plot_3errors(error_vel[0], error_vel[1], error_vel[2], "blue", "red");
154  // Update the title and axis labels
155  matplotlibcpp::subplot(3, 1, 1);
156  matplotlibcpp::title("IMU Velocity Error");
157  matplotlibcpp::ylabel("x-error (m/s)");
158  matplotlibcpp::subplot(3, 1, 2);
159  matplotlibcpp::ylabel("y-error (m/s)");
160  matplotlibcpp::subplot(3, 1, 3);
161  matplotlibcpp::ylabel("z-error (m/s)");
162  matplotlibcpp::xlabel("dataset time (s)");
163  matplotlibcpp::tight_layout();
164  matplotlibcpp::show(false);
165  //=====================================================
166 
167  //=====================================================
168  // Plot this figure
169  matplotlibcpp::figure_size(1000, 500);
170  plot_3errors(error_bg[0], error_bg[1], error_bg[2], "blue", "red");
171  // Update the title and axis labels
172  matplotlibcpp::subplot(3, 1, 1);
173  matplotlibcpp::title("IMU Gyroscope Bias Error");
174  matplotlibcpp::ylabel("x-error (rad/s)");
175  matplotlibcpp::subplot(3, 1, 2);
176  matplotlibcpp::ylabel("y-error (rad/s)");
177  matplotlibcpp::subplot(3, 1, 3);
178  matplotlibcpp::ylabel("z-error (rad/s)");
179  matplotlibcpp::xlabel("dataset time (s)");
180  matplotlibcpp::tight_layout();
181  matplotlibcpp::show(false);
182  //=====================================================
183 
184  //=====================================================
185  // Plot this figure
186  matplotlibcpp::figure_size(1000, 500);
187  plot_3errors(error_ba[0], error_ba[1], error_ba[2], "blue", "red");
188  // Update the title and axis labels
189  matplotlibcpp::subplot(3, 1, 1);
190  matplotlibcpp::title("IMU Accelerometer Bias Error");
191  matplotlibcpp::ylabel("x-error (m/s^2)");
192  matplotlibcpp::subplot(3, 1, 2);
193  matplotlibcpp::ylabel("y-error (m/s^2)");
194  matplotlibcpp::subplot(3, 1, 3);
195  matplotlibcpp::ylabel("z-error (m/s^2)");
196  matplotlibcpp::xlabel("dataset time (s)");
197  matplotlibcpp::tight_layout();
198  matplotlibcpp::show(false);
199  //=====================================================
200 
201 #endif
202 }
203 
204 void ResultSimulation::plot_timeoff(bool doplotting, double max_time) {
205 
206  // Calculate the time offset error at every timestep
207  Statistics error_time;
208  double start_time = est_state.at(0)(0);
209  for (size_t i = 0; i < est_state.size(); i++) {
210 
211  // Exit if we have reached our max time
212  if ((est_state.at(i)(0) - start_time) > max_time)
213  break;
214 
215  // Assert our times are the same
216  assert(est_state.at(i)(0) == gt_state.at(i)(0));
217 
218  // If we are not calibrating then don't plot it!
219  if (state_cov.at(i)(16) == 0.0) {
220  PRINT_WARNING(YELLOW "Time offset was not calibrated online, so will not plot...\n" RESET);
221  return;
222  }
223 
224  // Calculate time difference
225  error_time.timestamps.push_back(est_state.at(i)(0));
226  error_time.values.push_back(est_state.at(i)(17) - gt_state.at(i)(17));
227  error_time.values_bound.push_back(3 * state_cov.at(i)(16));
228  error_time.calculate();
229  }
230 
231  // return if we don't want to plot
232  if (!doplotting)
233  return;
234 
235 #ifndef HAVE_PYTHONLIBS
236  PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET);
237  return;
238 #else
239 
240  // Plot this figure
241  matplotlibcpp::figure_size(800, 250);
242 
243  // Zero our time array
244  double starttime = (error_time.timestamps.empty()) ? 0 : error_time.timestamps.at(0);
245  double endtime = (error_time.timestamps.empty()) ? 0 : error_time.timestamps.at(error_time.timestamps.size() - 1);
246  for (size_t i = 0; i < error_time.timestamps.size(); i++) {
247  error_time.timestamps.at(i) -= starttime;
248  }
249 
250  // Parameters that define the line styles
251  std::map<std::string, std::string> params_value, params_bound;
252  params_value.insert({"label", "error"});
253  params_value.insert({"linestyle", "-"});
254  params_value.insert({"color", "blue"});
255  params_bound.insert({"label", "3 sigma bound"});
256  params_bound.insert({"linestyle", "--"});
257  params_bound.insert({"color", "red"});
258 
259  // Plot our error value
260  matplotlibcpp::plot(error_time.timestamps, error_time.values, params_value);
261  if (!error_time.values_bound.empty()) {
262  matplotlibcpp::plot(error_time.timestamps, error_time.values_bound, params_bound);
263  for (size_t i = 0; i < error_time.timestamps.size(); i++) {
264  error_time.values_bound.at(i) *= -1;
265  }
266  matplotlibcpp::plot(error_time.timestamps, error_time.values_bound, "r--");
267  }
268  matplotlibcpp::xlim(0.0, endtime - starttime);
269 
270  // Update the title and axis labels
271  matplotlibcpp::title("Camera IMU Time Offset Error");
272  matplotlibcpp::ylabel("error (sec)");
273  matplotlibcpp::xlabel("dataset time (s)");
274  matplotlibcpp::tight_layout();
275  matplotlibcpp::show(false);
276 
277 #endif
278 }
279 
280 void ResultSimulation::plot_cam_instrinsics(bool doplotting, double max_time) {
281 
282  // Check that we have cameras
283  if ((int)est_state.at(0)(18) < 1) {
284  PRINT_ERROR(YELLOW "You need at least one camera to plot intrinsics...\n" RESET);
285  return;
286  }
287 
288  // Camera intrinsics statistic storage
289  std::vector<std::vector<Statistics>> error_cam_k, error_cam_d;
290  for (int i = 0; i < (int)est_state.at(0)(18); i++) {
291  std::vector<Statistics> temp1, temp2;
292  for (int j = 0; j < 4; j++) {
293  temp1.push_back(Statistics());
294  temp2.push_back(Statistics());
295  }
296  error_cam_k.push_back(temp1);
297  error_cam_d.push_back(temp2);
298  }
299 
300  // Loop through and calculate error
301  double start_time = est_state.at(0)(0);
302  for (size_t i = 0; i < est_state.size(); i++) {
303 
304  // Exit if we have reached our max time
305  if ((est_state.at(i)(0) - start_time) > max_time)
306  break;
307 
308  // Assert our times are the same
309  assert(est_state.at(i)(0) == gt_state.at(i)(0));
310 
311  // If we are not calibrating then don't plot it!
312  if (state_cov.at(i)(18) == 0.0) {
313  PRINT_WARNING(YELLOW "Camera intrinsics not calibrated online, so will not plot...\n" RESET);
314  return;
315  }
316 
317  // Loop through each camera and calculate error
318  for (int n = 0; n < (int)est_state.at(0)(18); n++) {
319  for (int j = 0; j < 4; j++) {
320  error_cam_k.at(n).at(j).timestamps.push_back(est_state.at(i)(0));
321  error_cam_k.at(n).at(j).values.push_back(est_state.at(i)(19 + 15 * n + j) - gt_state.at(i)(19 + 15 * n + j));
322  error_cam_k.at(n).at(j).values_bound.push_back(3 * state_cov.at(i)(18 + 14 * n + j));
323  error_cam_d.at(n).at(j).timestamps.push_back(est_state.at(i)(0));
324  error_cam_d.at(n).at(j).values.push_back(est_state.at(i)(19 + 4 + 15 * n + j) - gt_state.at(i)(19 + 4 + 15 * n + j));
325  error_cam_d.at(n).at(j).values_bound.push_back(3 * state_cov.at(i)(18 + 4 + 14 * n + j));
326  }
327  }
328  }
329 
330  // return if we don't want to plot
331  if (!doplotting)
332  return;
333 
334 #ifndef HAVE_PYTHONLIBS
335  PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET);
336  return;
337 #else
338 
339  // Plot line colors
340  std::vector<std::string> colors = {"blue", "red", "black", "green", "cyan", "magenta"};
341  assert(error_cam_k.size() <= colors.size());
342 
343  // Plot this figure
344  matplotlibcpp::figure_size(800, 600);
345  for (int n = 0; n < (int)est_state.at(0)(18); n++) {
346  std::string estcolor = ((int)est_state.at(0)(18) == 1) ? "blue" : colors.at(n);
347  std::string stdcolor = ((int)est_state.at(0)(18) == 1) ? "red" : colors.at(n);
348  plot_4errors(error_cam_k.at(n)[0], error_cam_k.at(n)[1], error_cam_k.at(n)[2], error_cam_k.at(n)[3], colors.at(n), stdcolor);
349  }
350 
351  // Update the title and axis labels
352  matplotlibcpp::subplot(4, 1, 1);
353  matplotlibcpp::title("Intrinsics Projection Error");
354  matplotlibcpp::ylabel("fx (px)");
355  matplotlibcpp::subplot(4, 1, 2);
356  matplotlibcpp::ylabel("fy (px)");
357  matplotlibcpp::subplot(4, 1, 3);
358  matplotlibcpp::ylabel("cx (px)");
359  matplotlibcpp::subplot(4, 1, 4);
360  matplotlibcpp::ylabel("cy (px)");
361  matplotlibcpp::xlabel("dataset time (s)");
362  matplotlibcpp::tight_layout();
363  matplotlibcpp::show(false);
364 
365  // Plot this figure
366  matplotlibcpp::figure_size(800, 600);
367  for (int n = 0; n < (int)est_state.at(0)(18); n++) {
368  std::string estcolor = ((int)est_state.at(0)(18) == 1) ? "blue" : colors.at(n);
369  std::string stdcolor = ((int)est_state.at(0)(18) == 1) ? "red" : colors.at(n);
370  plot_4errors(error_cam_d.at(n)[0], error_cam_d.at(n)[1], error_cam_d.at(n)[2], error_cam_d.at(n)[3], estcolor, stdcolor);
371  }
372 
373  // Update the title and axis labels
374  matplotlibcpp::subplot(4, 1, 1);
375  matplotlibcpp::title("Intrinsics Distortion Error");
376  matplotlibcpp::ylabel("d1");
377  matplotlibcpp::subplot(4, 1, 2);
378  matplotlibcpp::ylabel("d2");
379  matplotlibcpp::subplot(4, 1, 3);
380  matplotlibcpp::ylabel("d3");
381  matplotlibcpp::subplot(4, 1, 4);
382  matplotlibcpp::ylabel("d4");
383  matplotlibcpp::xlabel("dataset time (s)");
384  matplotlibcpp::tight_layout();
385  matplotlibcpp::show(false);
386 
387 #endif
388 }
389 
390 void ResultSimulation::plot_cam_extrinsics(bool doplotting, double max_time) {
391 
392  // Check that we have cameras
393  if ((int)est_state.at(0)(18) < 1) {
394  PRINT_ERROR(YELLOW "You need at least one camera to plot intrinsics...\n" RESET);
395  return;
396  }
397 
398  // Camera extrinsics statistic storage
399  std::vector<std::vector<Statistics>> error_cam_ori, error_cam_pos;
400  for (int i = 0; i < (int)est_state.at(0)(18); i++) {
401  std::vector<Statistics> temp1, temp2;
402  for (int j = 0; j < 3; j++) {
403  temp1.push_back(Statistics());
404  temp2.push_back(Statistics());
405  }
406  error_cam_ori.push_back(temp1);
407  error_cam_pos.push_back(temp2);
408  }
409 
410  // Loop through and calculate error
411  double start_time = est_state.at(0)(0);
412  for (size_t i = 0; i < est_state.size(); i++) {
413 
414  // Exit if we have reached our max time
415  if ((est_state.at(i)(0) - start_time) > max_time)
416  break;
417 
418  // Assert our times are the same
419  assert(est_state.at(i)(0) == gt_state.at(i)(0));
420 
421  // If we are not calibrating then don't plot it!
422  if (state_cov.at(i)(26) == 0.0) {
423  PRINT_WARNING(YELLOW "Camera extrinsics not calibrated online, so will not plot...\n" RESET);
424  return;
425  }
426 
427  // Loop through each camera and calculate error
428  for (int n = 0; n < (int)est_state.at(0)(18); n++) {
429  // NOTE: we define our error as e_R = -Log(R*Rhat^T)
430  Eigen::Matrix3d e_R = ov_core::quat_2_Rot(gt_state.at(i).block(27 + 15 * n, 0, 4, 1)) *
431  ov_core::quat_2_Rot(est_state.at(i).block(27 + 15 * n, 0, 4, 1)).transpose();
432  Eigen::Vector3d ori_err = -180.0 / M_PI * ov_core::log_so3(e_R);
433  // Eigen::Matrix3d e_R = Math::quat_2_Rot(est_state.at(i).block(27+15*n,0,4,1)).transpose() *
434  // Math::quat_2_Rot(gt_state.at(i).block(27+15*n,0,4,1)); Eigen::Vector3d ori_err = 180.0/M_PI*Math::log_so3(e_R);
435  for (int j = 0; j < 3; j++) {
436  error_cam_ori.at(n).at(j).timestamps.push_back(est_state.at(i)(0));
437  error_cam_ori.at(n).at(j).values.push_back(ori_err(j));
438  error_cam_ori.at(n).at(j).values_bound.push_back(3 * 180.0 / M_PI * state_cov.at(i)(26 + 14 * n + j));
439  error_cam_pos.at(n).at(j).timestamps.push_back(est_state.at(i)(0));
440  error_cam_pos.at(n).at(j).values.push_back(est_state.at(i)(27 + 4 + 15 * n + j) - gt_state.at(i)(27 + 4 + 15 * n + j));
441  error_cam_pos.at(n).at(j).values_bound.push_back(3 * state_cov.at(i)(26 + 3 + 14 * n + j));
442  }
443  }
444  }
445 
446  // return if we don't want to plot
447  if (!doplotting)
448  return;
449 
450 #ifndef HAVE_PYTHONLIBS
451  PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET);
452  return;
453 #else
454 
455  // Plot line colors
456  std::vector<std::string> colors = {"blue", "red", "black", "green", "cyan", "magenta"};
457  assert(error_cam_ori.size() <= colors.size());
458 
459  // Plot this figure
460  matplotlibcpp::figure_size(800, 500);
461  for (int n = 0; n < (int)est_state.at(0)(18); n++) {
462  std::string estcolor = ((int)est_state.at(0)(18) == 1) ? "blue" : colors.at(n);
463  std::string stdcolor = ((int)est_state.at(0)(18) == 1) ? "red" : colors.at(n);
464  plot_3errors(error_cam_ori.at(n)[0], error_cam_ori.at(n)[1], error_cam_ori.at(n)[2], colors.at(n), stdcolor);
465  }
466 
467  // Update the title and axis labels
468  matplotlibcpp::subplot(3, 1, 1);
469  matplotlibcpp::title("Camera Calibration Orientation Error");
470  matplotlibcpp::ylabel("x-error (deg)");
471  matplotlibcpp::subplot(3, 1, 2);
472  matplotlibcpp::ylabel("y-error (deg)");
473  matplotlibcpp::subplot(3, 1, 3);
474  matplotlibcpp::ylabel("z-error (deg)");
475  matplotlibcpp::xlabel("dataset time (s)");
476  matplotlibcpp::tight_layout();
477  matplotlibcpp::show(false);
478 
479  // Plot this figure
480  matplotlibcpp::figure_size(800, 500);
481  for (int n = 0; n < (int)est_state.at(0)(18); n++) {
482  std::string estcolor = ((int)est_state.at(0)(18) == 1) ? "blue" : colors.at(n);
483  std::string stdcolor = ((int)est_state.at(0)(18) == 1) ? "red" : colors.at(n);
484  plot_3errors(error_cam_pos.at(n)[0], error_cam_pos.at(n)[1], error_cam_pos.at(n)[2], estcolor, stdcolor);
485  }
486 
487  // Update the title and axis labels
488  matplotlibcpp::subplot(3, 1, 1);
489  matplotlibcpp::title("Camera Calibration Position Error");
490  matplotlibcpp::ylabel("x-error (m)");
491  matplotlibcpp::subplot(3, 1, 2);
492  matplotlibcpp::ylabel("y-error (m)");
493  matplotlibcpp::subplot(3, 1, 3);
494  matplotlibcpp::ylabel("z-error (m)");
495  matplotlibcpp::xlabel("dataset time (s)");
496  matplotlibcpp::tight_layout();
497  matplotlibcpp::show(false);
498 
499 #endif
500 }
501 
502 void ResultSimulation::plot_imu_intrinsics(bool doplotting, double max_time) {
503 
504  // Check that we have cameras
505  if ((int)est_state.at(0)(18) < 1) {
506  PRINT_ERROR(YELLOW "You need at least one camera to run estimator and plot results...\n" RESET);
507  return;
508  }
509 
510  // get the parameters id
511  int num_cam = (int)est_state.at(0)(18);
512  int imu_model = (int)est_state.at(0)(1 + 16 + 1 + 1 + 15 * num_cam);
513  int dw_id = 1 + 16 + 1 + 1 + 15 * num_cam + 1;
514  int dw_cov_id = 1 + 15 + 1 + 1 + 14 * num_cam + 1;
515  int da_id = dw_id + 6;
516  int da_cov_id = dw_cov_id + 6;
517  int tg_id = da_id + 6;
518  int tg_cov_id = da_cov_id + 6;
519  int wtoI_id = tg_id + 9;
520  int wtoI_cov_id = tg_cov_id + 9;
521  int atoI_id = wtoI_id + 4;
522  int atoI_cov_id = wtoI_cov_id + 3;
523 
524  // IMU intrinsics statistic storage
525  std::vector<Statistics> error_dw, error_da, error_tg, error_wtoI, error_atoI;
526  for (int j = 0; j < 6; j++) {
527  error_dw.emplace_back(Statistics());
528  error_da.emplace_back(Statistics());
529  }
530  for (int j = 0; j < 9; j++) {
531  error_tg.emplace_back(Statistics());
532  }
533  for (int j = 0; j < 3; j++) {
534  error_wtoI.emplace_back(Statistics());
535  error_atoI.emplace_back(Statistics());
536  }
537 
538  // Loop through and calculate error
539  double start_time = est_state.at(0)(0);
540  for (size_t i = 0; i < est_state.size(); i++) {
541 
542  // Exit if we have reached our max time
543  if ((est_state.at(i)(0) - start_time) > max_time)
544  break;
545 
546  // Assert our times are the same
547  assert(est_state.at(i)(0) == gt_state.at(i)(0));
548 
549  // If we are not calibrating then don't plot it!
550  if (state_cov.at(i)(dw_cov_id) == 0.0) {
551  PRINT_WARNING(YELLOW "IMU intrinsics not calibrated online, so will not plot...\n" RESET);
552  return;
553  }
554 
555  // Loop through IMU parameters and calculate error
556  for (int j = 0; j < 6; j++) {
557  error_dw.at(j).timestamps.push_back(est_state.at(i)(0));
558  error_dw.at(j).values.push_back(est_state.at(i)(dw_id + j) - gt_state.at(i)(dw_id + j));
559  error_dw.at(j).values_bound.push_back(3 * state_cov.at(i)(dw_cov_id + j));
560  error_da.at(j).timestamps.push_back(est_state.at(i)(0));
561  error_da.at(j).values.push_back(est_state.at(i)(da_id + j) - gt_state.at(i)(da_id + j));
562  error_da.at(j).values_bound.push_back(3 * state_cov.at(i)(da_cov_id + j));
563  }
564  for (int j = 0; j < 9; j++) {
565  error_tg.at(j).timestamps.push_back(est_state.at(i)(0));
566  error_tg.at(j).values.push_back(est_state.at(i)(tg_id + j) - gt_state.at(i)(tg_id + j));
567  error_tg.at(j).values_bound.push_back(3 * state_cov.at(i)(tg_cov_id + j));
568  }
569 
570  // Calculate orientation errors
571  Eigen::Matrix3d e_R_wtoI = ov_core::quat_2_Rot(gt_state.at(i).block<4, 1>(wtoI_id, 0)) *
572  ov_core::quat_2_Rot(est_state.at(i).block<4, 1>(wtoI_id, 0)).transpose();
573  Eigen::Vector3d ori_wtoI = -180.0 / M_PI * ov_core::log_so3(e_R_wtoI);
574  Eigen::Matrix3d e_R_atoI = ov_core::quat_2_Rot(gt_state.at(i).block<4, 1>(atoI_id, 0)) *
575  ov_core::quat_2_Rot(est_state.at(i).block<4, 1>(atoI_id, 0)).transpose();
576  Eigen::Vector3d ori_atoI = -180.0 / M_PI * ov_core::log_so3(e_R_atoI);
577  for (int j = 0; j < 3; j++) {
578  error_wtoI.at(j).timestamps.push_back(est_state.at(i)(0));
579  error_wtoI.at(j).values.push_back(ori_wtoI(j));
580  error_wtoI.at(j).values_bound.push_back(3 * 180.0 / M_PI * state_cov.at(i)(wtoI_cov_id + j));
581  error_atoI.at(j).timestamps.push_back(est_state.at(i)(0));
582  error_atoI.at(j).values.push_back(ori_atoI(j));
583  error_atoI.at(j).values_bound.push_back(3 * 180.0 / M_PI * state_cov.at(i)(atoI_cov_id + j));
584  }
585  }
586 
587  // return if we don't want to plot
588  if (!doplotting)
589  return;
590 
591 #ifndef HAVE_PYTHONLIBS
592  PRINT_ERROR(RED "Matplotlib not loaded, so will not plot, just returning..\n" RESET);
593  return;
594 #else
595 
596  // Plot line colors
597  std::vector<std::string> colors = {"blue", "red", "black", "green", "cyan", "magenta"};
598  std::string estcolor = ((int)est_state.at(0)(18) == 1) ? "blue" : colors.at(0);
599  std::string stdcolor = ((int)est_state.at(0)(18) == 1) ? "red" : colors.at(1);
600 
601  // Plot this figure
602  matplotlibcpp::figure_size(1000, 500);
603  plot_6errors(error_dw[0], error_dw[3], error_dw[1], error_dw[4], error_dw[2], error_dw[5], colors.at(0), stdcolor);
604 
605  // Update the title and axis labels
606  matplotlibcpp::subplot(3, 2, 1);
607  matplotlibcpp::title("IMU Dw Error (1:3)");
608  matplotlibcpp::ylabel("dw_1");
609  matplotlibcpp::subplot(3, 2, 2);
610  matplotlibcpp::title("IMU Dw Error (4:6)");
611  matplotlibcpp::ylabel("dw_4");
612  matplotlibcpp::subplot(3, 2, 3);
613  matplotlibcpp::ylabel("dw_2");
614  matplotlibcpp::subplot(3, 2, 4);
615  matplotlibcpp::ylabel("dw_5");
616  matplotlibcpp::subplot(3, 2, 5);
617  matplotlibcpp::ylabel("dw_3");
618  matplotlibcpp::xlabel("dataset time (s)");
619  matplotlibcpp::subplot(3, 2, 6);
620  matplotlibcpp::ylabel("dw_6");
621  matplotlibcpp::xlabel("dataset time (s)");
622  matplotlibcpp::tight_layout();
623  matplotlibcpp::show(false);
624 
625  //=====================================================
626  //=====================================================
627 
628  // Plot this figure
629  matplotlibcpp::figure_size(1000, 500);
630  plot_6errors(error_da[0], error_da[3], error_da[1], error_da[4], error_da[2], error_da[5], colors.at(0), stdcolor);
631 
632  // Update the title and axis labels
633  matplotlibcpp::subplot(3, 2, 1);
634  matplotlibcpp::title("IMU Da Error (1:3)");
635  matplotlibcpp::ylabel("da_1");
636  matplotlibcpp::subplot(3, 2, 2);
637  matplotlibcpp::title("IMU Da Error (4:6)");
638  matplotlibcpp::ylabel("da_4");
639  matplotlibcpp::subplot(3, 2, 3);
640  matplotlibcpp::ylabel("da_2");
641  matplotlibcpp::subplot(3, 2, 4);
642  matplotlibcpp::ylabel("da_5");
643  matplotlibcpp::subplot(3, 2, 5);
644  matplotlibcpp::ylabel("da_3");
645  matplotlibcpp::xlabel("dataset time (s)");
646  matplotlibcpp::subplot(3, 2, 6);
647  matplotlibcpp::ylabel("da_6");
648  matplotlibcpp::xlabel("dataset time (s)");
649  matplotlibcpp::tight_layout();
650  matplotlibcpp::show(false);
651 
652  //=====================================================
653  //=====================================================
654 
655  // Plot this figure
656  // NOTE: display is row-based not column-based
657  matplotlibcpp::figure_size(1400, 500);
658  plot_9errors(error_tg[0], error_tg[3], error_tg[6], error_tg[1], error_tg[4], error_tg[7], error_tg[2], error_tg[5], error_tg[8],
659  colors.at(0), stdcolor);
660 
661  // Update the title and axis labels
662  matplotlibcpp::subplot(3, 3, 1);
663  matplotlibcpp::title("IMU Tg Error (1:3)");
664  matplotlibcpp::ylabel("tg_1");
665  matplotlibcpp::subplot(3, 3, 2);
666  matplotlibcpp::title("IMU Tg Error (4:6)");
667  matplotlibcpp::ylabel("tg_4");
668  matplotlibcpp::subplot(3, 3, 3);
669  matplotlibcpp::title("IMU Tg Error (7:9)");
670  matplotlibcpp::ylabel("tg_7");
671  matplotlibcpp::subplot(3, 3, 4);
672  matplotlibcpp::ylabel("tg_2");
673  matplotlibcpp::subplot(3, 3, 5);
674  matplotlibcpp::ylabel("tg_5");
675  matplotlibcpp::subplot(3, 3, 6);
676  matplotlibcpp::ylabel("tg_8");
677  matplotlibcpp::subplot(3, 3, 7);
678  matplotlibcpp::ylabel("tg_3");
679  matplotlibcpp::xlabel("dataset time (s)");
680  matplotlibcpp::subplot(3, 3, 8);
681  matplotlibcpp::ylabel("tg_6");
682  matplotlibcpp::xlabel("dataset time (s)");
683  matplotlibcpp::subplot(3, 3, 9);
684  matplotlibcpp::ylabel("tg_9");
685  matplotlibcpp::xlabel("dataset time (s)");
686  matplotlibcpp::tight_layout();
687  matplotlibcpp::show(false);
688 
689  //=====================================================
690  //=====================================================
691 
692  // Finally plot
693  if (imu_model == 0) {
694 
695  // Kalibr model
696  // Plot this figure
697  matplotlibcpp::figure_size(800, 500);
698  plot_3errors(error_wtoI[0], error_wtoI[1], error_wtoI[2], colors.at(0), stdcolor);
699 
700  // Update the title and axis labels
701  matplotlibcpp::subplot(3, 1, 1);
702  matplotlibcpp::title("IMU R_GYROtoIMU Error");
703  matplotlibcpp::ylabel("x-error (deg)");
704  matplotlibcpp::subplot(3, 1, 2);
705  matplotlibcpp::ylabel("y-error (deg)");
706  matplotlibcpp::subplot(3, 1, 3);
707  matplotlibcpp::ylabel("z-error (deg)");
708  matplotlibcpp::xlabel("dataset time (s)");
709  matplotlibcpp::tight_layout();
710  matplotlibcpp::show(false);
711 
712  } else {
713 
714  // RPNG model
715  // Plot this figure
716  matplotlibcpp::figure_size(800, 500);
717  plot_3errors(error_atoI[0], error_atoI[1], error_atoI[2], colors.at(0), stdcolor);
718 
719  // Update the title and axis labels
720  matplotlibcpp::subplot(3, 1, 1);
721  matplotlibcpp::title("IMU R_ACCtoIMU Error");
722  matplotlibcpp::ylabel("x-error (deg)");
723  matplotlibcpp::subplot(3, 1, 2);
724  matplotlibcpp::ylabel("y-error (deg)");
725  matplotlibcpp::subplot(3, 1, 3);
726  matplotlibcpp::ylabel("z-error (deg)");
727  matplotlibcpp::xlabel("dataset time (s)");
728  matplotlibcpp::tight_layout();
729  matplotlibcpp::show(false);
730  }
731 
732 #endif
733 }
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
ov_eval
Evaluation and recording utilities.
Definition: AlignTrajectory.h:36
YELLOW
YELLOW
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::calculate
void calculate()
Will calculate all values from our vectors of information.
Definition: Statistics.h:73
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::Loader::load_simulation
static void load_simulation(std::string path, std::vector< Eigen::VectorXd > &values)
Load an arbitrary sized row of space separated values, used for our simulation.
Definition: Loader.cpp:179
matplotlibcpp::figure_size
void figure_size(size_t w, size_t h)
PRINT_ERROR
#define PRINT_ERROR(x...)
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
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_core::quat_2_Rot
Eigen::Matrix< double, 3, 3 > quat_2_Rot(const Eigen::Matrix< double, 4, 1 > &q)
matplotlibcpp::tight_layout
void tight_layout()
matplotlibcpp::subplot
void subplot(long nrows, long ncols, long plot_number)
ResultSimulation.h
RESET
#define RESET
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
matplotlibcpp::xlabel
void xlabel(const std::string &str, const std::map< std::string, std::string > &keywords={})
ov_core::log_so3
Eigen::Matrix< double, 3, 1 > log_so3(const Eigen::Matrix< double, 3, 3 > &R)
matplotlibcpp::title
void title(const std::string &titlestr, const std::map< std::string, std::string > &keywords={})
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
RED
RED
PRINT_WARNING
#define PRINT_WARNING(x...)
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