VioManagerOptions.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_MSCKF_VIOMANAGEROPTIONS_H
23 #define OV_MSCKF_VIOMANAGEROPTIONS_H
24 
25 #include <Eigen/Eigen>
26 #include <iostream>
27 #include <memory>
28 #include <sstream>
29 #include <string>
30 #include <vector>
31 
32 #include "state/StateOptions.h"
33 #include "update/UpdaterOptions.h"
34 #include "utils/NoiseManager.h"
35 
37 
38 #include "cam/CamEqui.h"
39 #include "cam/CamRadtan.h"
41 #include "track/TrackBase.h"
42 #include "utils/colors.h"
44 #include "utils/print.h"
45 #include "utils/quat_ops.h"
46 
47 namespace ov_msckf {
48 
57 
62  void print_and_load(const std::shared_ptr<ov_core::YamlParser> &parser = nullptr) {
65  print_and_load_noise(parser);
66 
67  // needs to be called last
68  print_and_load_state(parser);
69  }
70 
71  // ESTIMATOR ===============================
72 
75 
78 
80  double dt_slam_delay = 2.0;
81 
83  bool try_zupt = false;
84 
86  double zupt_max_velocity = 1.0;
87 
89  double zupt_noise_multiplier = 1.0;
90 
92  double zupt_max_disparity = 1.0;
93 
95  bool zupt_only_at_beginning = false;
96 
99 
101  std::string record_timing_filepath = "ov_msckf_timing.txt";
102 
109  void print_and_load_estimator(const std::shared_ptr<ov_core::YamlParser> &parser = nullptr) {
110  PRINT_DEBUG("ESTIMATOR PARAMETERS:\n");
111  state_options.print(parser);
113  if (parser != nullptr) {
114  parser->parse_config("dt_slam_delay", dt_slam_delay);
115  parser->parse_config("try_zupt", try_zupt);
116  parser->parse_config("zupt_max_velocity", zupt_max_velocity);
117  parser->parse_config("zupt_noise_multiplier", zupt_noise_multiplier);
118  parser->parse_config("zupt_max_disparity", zupt_max_disparity);
119  parser->parse_config("zupt_only_at_beginning", zupt_only_at_beginning);
120  parser->parse_config("record_timing_information", record_timing_information);
121  parser->parse_config("record_timing_filepath", record_timing_filepath);
122  }
123  PRINT_DEBUG(" - dt_slam_delay: %.1f\n", dt_slam_delay);
124  PRINT_DEBUG(" - zero_velocity_update: %d\n", try_zupt);
125  PRINT_DEBUG(" - zupt_max_velocity: %.2f\n", zupt_max_velocity);
126  PRINT_DEBUG(" - zupt_noise_multiplier: %.2f\n", zupt_noise_multiplier);
127  PRINT_DEBUG(" - zupt_max_disparity: %.4f\n", zupt_max_disparity);
128  PRINT_DEBUG(" - zupt_only_at_beginning?: %d\n", zupt_only_at_beginning);
129  PRINT_DEBUG(" - record timing?: %d\n", (int)record_timing_information);
130  PRINT_DEBUG(" - record timing filepath: %s\n", record_timing_filepath.c_str());
131  }
132 
133  // NOISE / CHI2 ============================
134 
137 
140 
143 
146 
149 
156  void print_and_load_noise(const std::shared_ptr<ov_core::YamlParser> &parser = nullptr) {
157  PRINT_DEBUG("NOISE PARAMETERS:\n");
158  if (parser != nullptr) {
159  parser->parse_external("relative_config_imu", "imu0", "gyroscope_noise_density", imu_noises.sigma_w);
160  parser->parse_external("relative_config_imu", "imu0", "gyroscope_random_walk", imu_noises.sigma_wb);
161  parser->parse_external("relative_config_imu", "imu0", "accelerometer_noise_density", imu_noises.sigma_a);
162  parser->parse_external("relative_config_imu", "imu0", "accelerometer_random_walk", imu_noises.sigma_ab);
163  }
164  imu_noises.print();
165  if (parser != nullptr) {
166  parser->parse_config("up_msckf_sigma_px", msckf_options.sigma_pix);
167  parser->parse_config("up_msckf_chi2_multipler", msckf_options.chi2_multipler);
168  parser->parse_config("up_slam_sigma_px", slam_options.sigma_pix);
169  parser->parse_config("up_slam_chi2_multipler", slam_options.chi2_multipler);
170  parser->parse_config("up_aruco_sigma_px", aruco_options.sigma_pix);
171  parser->parse_config("up_aruco_chi2_multipler", aruco_options.chi2_multipler);
175  parser->parse_config("zupt_chi2_multipler", zupt_options.chi2_multipler);
176  }
177  PRINT_DEBUG(" Updater MSCKF Feats:\n");
179  PRINT_DEBUG(" Updater SLAM Feats:\n");
181  PRINT_DEBUG(" Updater ARUCO Tags:\n");
183  PRINT_DEBUG(" Updater ZUPT:\n");
185  }
186 
187  // STATE DEFAULTS ==========================
188 
190  double gravity_mag = 9.81;
191 
193  Eigen::Matrix<double, 6, 1> vec_dw;
194 
196  Eigen::Matrix<double, 6, 1> vec_da;
197 
199  Eigen::Matrix<double, 9, 1> vec_tg;
200 
202  Eigen::Matrix<double, 4, 1> q_ACCtoIMU;
203 
205  Eigen::Matrix<double, 4, 1> q_GYROtoIMU;
206 
208  double calib_camimu_dt = 0.0;
209 
211  std::unordered_map<size_t, std::shared_ptr<ov_core::CamBase>> camera_intrinsics;
212 
214  std::map<size_t, Eigen::VectorXd> camera_extrinsics;
215 
217  bool use_mask = false;
218 
220  std::map<size_t, cv::Mat> masks;
221 
228  void print_and_load_state(const std::shared_ptr<ov_core::YamlParser> &parser = nullptr) {
229  if (parser != nullptr) {
230  parser->parse_config("gravity_mag", gravity_mag);
231  for (int i = 0; i < state_options.num_cameras; i++) {
232 
233  // Time offset (use the first one)
234  // TODO: support multiple time offsets between cameras
235  if (i == 0) {
236  parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "timeshift_cam_imu", calib_camimu_dt, false);
237  }
238 
239  // Distortion model
240  std::string dist_model = "radtan";
241  parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "distortion_model", dist_model);
242 
243  // Distortion parameters
244  std::vector<double> cam_calib1 = {1, 1, 0, 0};
245  std::vector<double> cam_calib2 = {0, 0, 0, 0};
246  parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "intrinsics", cam_calib1);
247  parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "distortion_coeffs", cam_calib2);
248  Eigen::VectorXd cam_calib = Eigen::VectorXd::Zero(8);
249  cam_calib << cam_calib1.at(0), cam_calib1.at(1), cam_calib1.at(2), cam_calib1.at(3), cam_calib2.at(0), cam_calib2.at(1),
250  cam_calib2.at(2), cam_calib2.at(3);
251  cam_calib(0) /= (downsample_cameras) ? 2.0 : 1.0;
252  cam_calib(1) /= (downsample_cameras) ? 2.0 : 1.0;
253  cam_calib(2) /= (downsample_cameras) ? 2.0 : 1.0;
254  cam_calib(3) /= (downsample_cameras) ? 2.0 : 1.0;
255 
256  // FOV / resolution
257  std::vector<int> matrix_wh = {1, 1};
258  parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "resolution", matrix_wh);
259  matrix_wh.at(0) /= (downsample_cameras) ? 2.0 : 1.0;
260  matrix_wh.at(1) /= (downsample_cameras) ? 2.0 : 1.0;
261 
262  // Extrinsics
263  Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity();
264  parser->parse_external("relative_config_imucam", "cam" + std::to_string(i), "T_imu_cam", T_CtoI);
265 
266  // Load these into our state
267  Eigen::Matrix<double, 7, 1> cam_eigen;
268  cam_eigen.block(0, 0, 4, 1) = ov_core::rot_2_quat(T_CtoI.block(0, 0, 3, 3).transpose());
269  cam_eigen.block(4, 0, 3, 1) = -T_CtoI.block(0, 0, 3, 3).transpose() * T_CtoI.block(0, 3, 3, 1);
270 
271  // Create intrinsics model
272  if (dist_model == "equidistant") {
273  camera_intrinsics.insert({i, std::make_shared<ov_core::CamEqui>(matrix_wh.at(0), matrix_wh.at(1))});
274  camera_intrinsics.at(i)->set_value(cam_calib);
275  } else {
276  camera_intrinsics.insert({i, std::make_shared<ov_core::CamRadtan>(matrix_wh.at(0), matrix_wh.at(1))});
277  camera_intrinsics.at(i)->set_value(cam_calib);
278  }
279  camera_extrinsics.insert({i, cam_eigen});
280  }
281  parser->parse_config("use_mask", use_mask);
282  if (use_mask) {
283  for (int i = 0; i < state_options.num_cameras; i++) {
284  std::string mask_path;
285  std::string mask_node = "mask" + std::to_string(i);
286  parser->parse_config(mask_node, mask_path);
287  std::string total_mask_path = parser->get_config_folder() + mask_path;
288  if (!boost::filesystem::exists(total_mask_path)) {
289  PRINT_ERROR(RED "VioManager(): invalid mask path:\n" RESET);
290  PRINT_ERROR(RED "\t- mask%d - %s\n" RESET, i, total_mask_path.c_str());
291  std::exit(EXIT_FAILURE);
292  }
293  cv::Mat mask = cv::imread(total_mask_path, cv::IMREAD_GRAYSCALE);
294  masks.insert({i, mask});
295  if (mask.cols != camera_intrinsics.at(i)->w() || mask.rows != camera_intrinsics.at(i)->h()) {
296  PRINT_ERROR(RED "VioManager(): mask size does not match camera!\n" RESET);
297  PRINT_ERROR(RED "\t- mask%d - %s\n" RESET, i, total_mask_path.c_str());
298  PRINT_ERROR(RED "\t- mask%d - %d x %d\n" RESET, mask.cols, mask.rows);
299  PRINT_ERROR(RED "\t- cam%d - %d x %d\n" RESET, camera_intrinsics.at(i)->w(), camera_intrinsics.at(i)->h());
300  std::exit(EXIT_FAILURE);
301  }
302  }
303  }
304 
305  // IMU intrinsics
306  Eigen::Matrix3d Tw = Eigen::Matrix3d::Identity();
307  parser->parse_external("relative_config_imu", "imu0", "Tw", Tw);
308  Eigen::Matrix3d Ta = Eigen::Matrix3d::Identity();
309  parser->parse_external("relative_config_imu", "imu0", "Ta", Ta);
310  Eigen::Matrix3d R_IMUtoACC = Eigen::Matrix3d::Identity();
311  parser->parse_external("relative_config_imu", "imu0", "R_IMUtoACC", R_IMUtoACC);
312  Eigen::Matrix3d R_IMUtoGYRO = Eigen::Matrix3d::Identity();
313  parser->parse_external("relative_config_imu", "imu0", "R_IMUtoGYRO", R_IMUtoGYRO);
314  Eigen::Matrix3d Tg = Eigen::Matrix3d::Zero();
315  parser->parse_external("relative_config_imu", "imu0", "Tg", Tg);
316 
317  // Generate the parameters we need
318  // TODO: error here if this returns a NaN value (i.e. invalid matrix specified)
319  Eigen::Matrix3d Dw = Tw.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity());
320  Eigen::Matrix3d Da = Ta.colPivHouseholderQr().solve(Eigen::Matrix3d::Identity());
321  Eigen::Matrix3d R_ACCtoIMU = R_IMUtoACC.transpose();
322  Eigen::Matrix3d R_GYROtoIMU = R_IMUtoGYRO.transpose();
323  if (std::isnan(Tw.norm()) || std::isnan(Dw.norm())) {
324  std::stringstream ss;
325  ss << "gyroscope has bad intrinsic values!" << std::endl;
326  ss << "Tw - " << std::endl << Tw << std::endl << std::endl;
327  ss << "Dw - " << std::endl << Dw << std::endl << std::endl;
328  PRINT_DEBUG(RED "" RESET, ss.str().c_str());
329  std::exit(EXIT_FAILURE);
330  }
331  if (std::isnan(Ta.norm()) || std::isnan(Da.norm())) {
332  std::stringstream ss;
333  ss << "accelerometer has bad intrinsic values!" << std::endl;
334  ss << "Ta - " << std::endl << Ta << std::endl << std::endl;
335  ss << "Da - " << std::endl << Da << std::endl << std::endl;
336  PRINT_DEBUG(RED "" RESET, ss.str().c_str());
337  std::exit(EXIT_FAILURE);
338  }
339 
340  // kalibr model: lower triangular of the matrix and R_GYROtoI
341  // rpng model: upper triangular of the matrix and R_ACCtoI
342  if (state_options.imu_model == StateOptions::ImuModel::KALIBR) {
343  vec_dw << Dw.block<3, 1>(0, 0), Dw.block<2, 1>(1, 1), Dw(2, 2);
344  vec_da << Da.block<3, 1>(0, 0), Da.block<2, 1>(1, 1), Da(2, 2);
345  } else {
346  vec_dw << Dw(0, 0), Dw.block<2, 1>(0, 1), Dw.block<3, 1>(0, 2);
347  vec_da << Da(0, 0), Da.block<2, 1>(0, 1), Da.block<3, 1>(0, 2);
348  }
349  vec_tg << Tg.block<3, 1>(0, 0), Tg.block<3, 1>(0, 1), Tg.block<3, 1>(0, 2);
350  q_GYROtoIMU = ov_core::rot_2_quat(R_GYROtoIMU);
351  q_ACCtoIMU = ov_core::rot_2_quat(R_ACCtoIMU);
352  }
353  PRINT_DEBUG("STATE PARAMETERS:\n");
354  PRINT_DEBUG(" - gravity_mag: %.4f\n", gravity_mag);
355  PRINT_DEBUG(" - gravity: %.3f, %.3f, %.3f\n", 0.0, 0.0, gravity_mag);
356  PRINT_DEBUG(" - camera masks?: %d\n", use_mask);
358  PRINT_ERROR(RED "[SIM]: camera calib size does not match max cameras...\n" RESET);
359  PRINT_ERROR(RED "[SIM]: got %d but expected %d max cameras (camera_intrinsics)\n" RESET, (int)camera_intrinsics.size(),
361  PRINT_ERROR(RED "[SIM]: got %d but expected %d max cameras (camera_extrinsics)\n" RESET, (int)camera_extrinsics.size(),
363  std::exit(EXIT_FAILURE);
364  }
365  PRINT_DEBUG(" - calib_camimu_dt: %.4f\n", calib_camimu_dt);
366  PRINT_DEBUG("CAMERA PARAMETERS:\n");
367  for (int n = 0; n < state_options.num_cameras; n++) {
368  std::stringstream ss;
369  ss << "cam_" << n << "_fisheye:" << (std::dynamic_pointer_cast<ov_core::CamEqui>(camera_intrinsics.at(n)) != nullptr) << std::endl;
370  ss << "cam_" << n << "_wh:" << std::endl << camera_intrinsics.at(n)->w() << " x " << camera_intrinsics.at(n)->h() << std::endl;
371  ss << "cam_" << n << "_intrinsic(0:3):" << std::endl
372  << camera_intrinsics.at(n)->get_value().block(0, 0, 4, 1).transpose() << std::endl;
373  ss << "cam_" << n << "_intrinsic(4:7):" << std::endl
374  << camera_intrinsics.at(n)->get_value().block(4, 0, 4, 1).transpose() << std::endl;
375  ss << "cam_" << n << "_extrinsic(0:3):" << std::endl << camera_extrinsics.at(n).block(0, 0, 4, 1).transpose() << std::endl;
376  ss << "cam_" << n << "_extrinsic(4:6):" << std::endl << camera_extrinsics.at(n).block(4, 0, 3, 1).transpose() << std::endl;
377  Eigen::Matrix4d T_CtoI = Eigen::Matrix4d::Identity();
378  T_CtoI.block(0, 0, 3, 3) = ov_core::quat_2_Rot(camera_extrinsics.at(n).block(0, 0, 4, 1)).transpose();
379  T_CtoI.block(0, 3, 3, 1) = -T_CtoI.block(0, 0, 3, 3) * camera_extrinsics.at(n).block(4, 0, 3, 1);
380  ss << "T_C" << n << "toI:" << std::endl << T_CtoI << std::endl << std::endl;
381  PRINT_DEBUG(ss.str().c_str());
382  }
383  PRINT_DEBUG("IMU PARAMETERS:\n");
384  std::stringstream ss;
385  ss << "imu model:" << ((state_options.imu_model == StateOptions::ImuModel::KALIBR) ? "kalibr" : "rpng") << std::endl;
386  ss << "Dw (columnwise):" << vec_dw.transpose() << std::endl;
387  ss << "Da (columnwise):" << vec_da.transpose() << std::endl;
388  ss << "Tg (columnwise):" << vec_tg.transpose() << std::endl;
389  ss << "q_GYROtoI: " << q_GYROtoIMU.transpose() << std::endl;
390  ss << "q_ACCtoI: " << q_ACCtoIMU.transpose() << std::endl;
391  PRINT_DEBUG(ss.str().c_str());
392  }
393 
394  // TRACKERS ===============================
395 
397  bool use_stereo = true;
398 
400  bool use_klt = true;
401 
403  bool use_aruco = true;
404 
406  bool downsize_aruco = true;
407 
409  bool downsample_cameras = false;
410 
413 
416 
419 
421  int num_pts = 150;
422 
424  int fast_threshold = 20;
425 
427  int grid_x = 5;
428 
430  int grid_y = 5;
431 
433  int min_px_dist = 10;
434 
436  ov_core::TrackBase::HistogramMethod histogram_method = ov_core::TrackBase::HistogramMethod::HISTOGRAM;
437 
439  double knn_ratio = 0.85;
440 
442  double track_frequency = 20.0;
443 
446 
453  void print_and_load_trackers(const std::shared_ptr<ov_core::YamlParser> &parser = nullptr) {
454  if (parser != nullptr) {
455  parser->parse_config("use_stereo", use_stereo);
456  parser->parse_config("use_klt", use_klt);
457  parser->parse_config("use_aruco", use_aruco);
458  parser->parse_config("downsize_aruco", downsize_aruco);
459  parser->parse_config("downsample_cameras", downsample_cameras);
460  parser->parse_config("num_opencv_threads", num_opencv_threads);
461  parser->parse_config("multi_threading_pubs", use_multi_threading_pubs, false);
462  parser->parse_config("multi_threading_subs", use_multi_threading_subs, false);
463  parser->parse_config("num_pts", num_pts);
464  parser->parse_config("fast_threshold", fast_threshold);
465  parser->parse_config("grid_x", grid_x);
466  parser->parse_config("grid_y", grid_y);
467  parser->parse_config("min_px_dist", min_px_dist);
468  std::string histogram_method_str = "HISTOGRAM";
469  parser->parse_config("histogram_method", histogram_method_str);
470  if (histogram_method_str == "NONE") {
472  } else if (histogram_method_str == "HISTOGRAM") {
474  } else if (histogram_method_str == "CLAHE") {
476  } else {
477  printf(RED "VioManager(): invalid feature histogram specified:\n" RESET);
478  printf(RED "\t- NONE\n" RESET);
479  printf(RED "\t- HISTOGRAM\n" RESET);
480  printf(RED "\t- CLAHE\n" RESET);
481  std::exit(EXIT_FAILURE);
482  }
483  parser->parse_config("knn_ratio", knn_ratio);
484  parser->parse_config("track_frequency", track_frequency);
485  }
486  PRINT_DEBUG("FEATURE TRACKING PARAMETERS:\n");
487  PRINT_DEBUG(" - use_stereo: %d\n", use_stereo);
488  PRINT_DEBUG(" - use_klt: %d\n", use_klt);
489  PRINT_DEBUG(" - use_aruco: %d\n", use_aruco);
490  PRINT_DEBUG(" - downsize aruco: %d\n", downsize_aruco);
491  PRINT_DEBUG(" - downsize cameras: %d\n", downsample_cameras);
492  PRINT_DEBUG(" - num opencv threads: %d\n", num_opencv_threads);
493  PRINT_DEBUG(" - use multi-threading pubs: %d\n", use_multi_threading_pubs);
494  PRINT_DEBUG(" - use multi-threading subs: %d\n", use_multi_threading_subs);
495  PRINT_DEBUG(" - num_pts: %d\n", num_pts);
496  PRINT_DEBUG(" - fast threshold: %d\n", fast_threshold);
497  PRINT_DEBUG(" - grid X by Y: %d by %d\n", grid_x, grid_y);
498  PRINT_DEBUG(" - min px dist: %d\n", min_px_dist);
499  PRINT_DEBUG(" - hist method: %d\n", (int)histogram_method);
500  PRINT_DEBUG(" - knn ratio: %.3f\n", knn_ratio);
501  PRINT_DEBUG(" - track frequency: %.1f\n", track_frequency);
502  featinit_options.print(parser);
503  }
504 
505  // SIMULATOR ===============================
506 
509 
512 
516 
518  bool sim_do_perturbation = false;
519 
521  std::string sim_traj_path = "src/open_vins/ov_data/sim/udel_gore.txt";
522 
526 
528  double sim_freq_cam = 10.0;
529 
531  double sim_freq_imu = 400.0;
532 
535 
538 
545  void print_and_load_simulation(const std::shared_ptr<ov_core::YamlParser> &parser = nullptr) {
546  if (parser != nullptr) {
547  parser->parse_config("sim_seed_state_init", sim_seed_state_init);
548  parser->parse_config("sim_seed_preturb", sim_seed_preturb);
549  parser->parse_config("sim_seed_measurements", sim_seed_measurements);
550  parser->parse_config("sim_do_perturbation", sim_do_perturbation);
551  parser->parse_config("sim_traj_path", sim_traj_path);
552  parser->parse_config("sim_distance_threshold", sim_distance_threshold);
553  parser->parse_config("sim_freq_cam", sim_freq_cam);
554  parser->parse_config("sim_freq_imu", sim_freq_imu);
555  parser->parse_config("sim_min_feature_gen_dist", sim_min_feature_gen_distance);
556  parser->parse_config("sim_max_feature_gen_dist", sim_max_feature_gen_distance);
557  }
558  PRINT_DEBUG("SIMULATION PARAMETERS:\n");
559  PRINT_WARNING(BOLDRED " - state init seed: %d \n" RESET, sim_seed_state_init);
560  PRINT_WARNING(BOLDRED " - perturb seed: %d \n" RESET, sim_seed_preturb);
561  PRINT_WARNING(BOLDRED " - measurement seed: %d \n" RESET, sim_seed_measurements);
562  PRINT_WARNING(BOLDRED " - do perturb?: %d\n" RESET, sim_do_perturbation);
563  PRINT_DEBUG(" - traj path: %s\n", sim_traj_path.c_str());
564  PRINT_DEBUG(" - dist thresh: %.2f\n", sim_distance_threshold);
565  PRINT_DEBUG(" - cam feq: %.2f\n", sim_freq_cam);
566  PRINT_DEBUG(" - imu feq: %.2f\n", sim_freq_imu);
567  PRINT_DEBUG(" - min feat dist: %.2f\n", sim_min_feature_gen_distance);
568  PRINT_DEBUG(" - max feat dist: %.2f\n", sim_max_feature_gen_distance);
569  }
570 };
571 
572 } // namespace ov_msckf
573 
574 #endif // OV_MSCKF_VIOMANAGEROPTIONS_H
ov_msckf::VioManagerOptions::camera_extrinsics
std::map< size_t, Eigen::VectorXd > camera_extrinsics
Map between camid and camera extrinsics (q_ItoC, p_IinC).
Definition: VioManagerOptions.h:214
ov_msckf::VioManagerOptions::min_px_dist
int min_px_dist
Will check after doing KLT track and remove any features closer than this.
Definition: VioManagerOptions.h:433
ov_msckf::VioManagerOptions::msckf_options
UpdaterOptions msckf_options
Update options for MSCKF features (pixel noise and chi2 multiplier)
Definition: VioManagerOptions.h:139
ov_msckf::VioManagerOptions::sim_freq_cam
double sim_freq_cam
Frequency (Hz) that we will simulate our cameras.
Definition: VioManagerOptions.h:528
ov_msckf::VioManagerOptions::zupt_max_velocity
double zupt_max_velocity
Max velocity we will consider to try to do a zupt (i.e. if above this, don't do zupt)
Definition: VioManagerOptions.h:86
ov_msckf::NoiseManager::sigma_a
double sigma_a
Accelerometer white noise (m/s^2/sqrt(hz))
Definition: NoiseManager.h:49
StateOptions.h
quat_ops.h
ov_core::TrackBase::HistogramMethod
HistogramMethod
ov_core::TrackBase::NONE
NONE
ov_init::InertialInitializerOptions
ov_msckf::VioManagerOptions::use_stereo
bool use_stereo
If we should process two cameras are being stereo or binocular. If binocular, we do monocular feature...
Definition: VioManagerOptions.h:397
ov_msckf::VioManagerOptions::sim_traj_path
std::string sim_traj_path
Path to the trajectory we will b-spline and simulate on. Should be time(s),pos(xyz),...
Definition: VioManagerOptions.h:521
ov_msckf::UpdaterOptions
Struct which stores general updater options.
Definition: UpdaterOptions.h:32
ov_msckf::VioManagerOptions::dt_slam_delay
double dt_slam_delay
Delay, in seconds, that we should wait from init before we start estimating SLAM features.
Definition: VioManagerOptions.h:80
FeatureInitializerOptions.h
ov_msckf::VioManagerOptions::camera_intrinsics
std::unordered_map< size_t, std::shared_ptr< ov_core::CamBase > > camera_intrinsics
Map between camid and camera intrinsics (fx, fy, cx, cy, d1...d4, cam_w, cam_h)
Definition: VioManagerOptions.h:211
ov_msckf::NoiseManager::sigma_wb
double sigma_wb
Gyroscope random walk (rad/s^2/sqrt(hz))
Definition: NoiseManager.h:43
PRINT_DEBUG
#define PRINT_DEBUG(x...)
ov_msckf::VioManagerOptions::histogram_method
ov_core::TrackBase::HistogramMethod histogram_method
What type of pre-processing histogram method should be applied to images.
Definition: VioManagerOptions.h:436
ov_msckf::VioManagerOptions::print_and_load_trackers
void print_and_load_trackers(const std::shared_ptr< ov_core::YamlParser > &parser=nullptr)
This function will load print out all parameters related to visual tracking This allows for visual ch...
Definition: VioManagerOptions.h:453
ov_msckf
Extended Kalman Filter estimator.
Definition: VioManager.h:46
ov_msckf::VioManagerOptions::zupt_max_disparity
double zupt_max_disparity
Max disparity we will consider to try to do a zupt (i.e. if above this, don't do zupt)
Definition: VioManagerOptions.h:92
ov_msckf::VioManagerOptions::vec_dw
Eigen::Matrix< double, 6, 1 > vec_dw
Gyroscope IMU intrinsics (scale imperfection and axis misalignment, column-wise, inverse)
Definition: VioManagerOptions.h:193
ov_msckf::VioManagerOptions::print_and_load
void print_and_load(const std::shared_ptr< ov_core::YamlParser > &parser=nullptr)
This function will load the non-simulation parameters of the system and print.
Definition: VioManagerOptions.h:62
ov_msckf::NoiseManager::sigma_ab
double sigma_ab
Accelerometer random walk (m/s^3/sqrt(hz))
Definition: NoiseManager.h:55
ov_msckf::VioManagerOptions::downsample_cameras
bool downsample_cameras
Will half the resolution all tracking image (aruco will be 1/4 instead of halved if dowsize_aruoc als...
Definition: VioManagerOptions.h:409
opencv_yaml_parse.h
ov_msckf::VioManagerOptions::fast_threshold
int fast_threshold
Fast extraction threshold.
Definition: VioManagerOptions.h:424
InertialInitializerOptions.h
ov_msckf::VioManagerOptions::sim_do_perturbation
bool sim_do_perturbation
If we should perturb the calibration that the estimator starts with.
Definition: VioManagerOptions.h:518
ov_msckf::UpdaterOptions::print
void print()
Nice print function of what parameters we have loaded.
Definition: UpdaterOptions.h:44
PRINT_ERROR
#define PRINT_ERROR(x...)
ov_msckf::VioManagerOptions::use_multi_threading_pubs
bool use_multi_threading_pubs
If our ROS image publisher should be async (if sim this should be no!)
Definition: VioManagerOptions.h:415
ov_msckf::VioManagerOptions::record_timing_information
bool record_timing_information
If we should record the timing performance to file.
Definition: VioManagerOptions.h:98
ov_msckf::NoiseManager::print
void print()
Nice print function of what parameters we have loaded.
Definition: NoiseManager.h:61
ov_core::FeatureInitializerOptions
ov_msckf::VioManagerOptions::sim_max_feature_gen_distance
double sim_max_feature_gen_distance
Feature distance we generate features from (maximum)
Definition: VioManagerOptions.h:537
ov_msckf::VioManagerOptions::use_mask
bool use_mask
If we should try to load a mask and use it to reject invalid features.
Definition: VioManagerOptions.h:217
ov_msckf::VioManagerOptions::masks
std::map< size_t, cv::Mat > masks
Mask images for each camera.
Definition: VioManagerOptions.h:220
ov_msckf::VioManagerOptions::grid_x
int grid_x
Number of grids we should split column-wise to do feature extraction in.
Definition: VioManagerOptions.h:427
ov_msckf::VioManagerOptions::slam_options
UpdaterOptions slam_options
Update options for SLAM features (pixel noise and chi2 multiplier)
Definition: VioManagerOptions.h:142
ov_msckf::VioManagerOptions::sim_seed_preturb
int sim_seed_preturb
Seed for calibration perturbations. Change this to perturb by different random values if perturbation...
Definition: VioManagerOptions.h:511
ov_msckf::VioManagerOptions::sim_seed_state_init
int sim_seed_state_init
Seed for initial states (i.e. random feature 3d positions in the generated map)
Definition: VioManagerOptions.h:508
ov_msckf::NoiseManager::sigma_w
double sigma_w
Gyroscope white noise (rad/s/sqrt(hz))
Definition: NoiseManager.h:37
ov_msckf::VioManagerOptions::try_zupt
bool try_zupt
If we should try to use zero velocity update.
Definition: VioManagerOptions.h:83
ov_msckf::VioManagerOptions::sim_min_feature_gen_distance
double sim_min_feature_gen_distance
Feature distance we generate features from (minimum)
Definition: VioManagerOptions.h:534
ov_msckf::VioManagerOptions::zupt_only_at_beginning
bool zupt_only_at_beginning
If we should only use the zupt at the very beginning static initialization phase.
Definition: VioManagerOptions.h:95
ov_init::InertialInitializerOptions::print_and_load
void print_and_load(const std::shared_ptr< ov_core::YamlParser > &parser=nullptr)
ov_msckf::VioManagerOptions::grid_y
int grid_y
Number of grids we should split row-wise to do feature extraction in.
Definition: VioManagerOptions.h:430
ov_msckf::VioManagerOptions::zupt_noise_multiplier
double zupt_noise_multiplier
Multiplier of our zupt measurement IMU noise matrix (default should be 1.0)
Definition: VioManagerOptions.h:89
ov_msckf::StateOptions::print
void print(const std::shared_ptr< ov_core::YamlParser > &parser=nullptr)
Nice print function of what parameters we have loaded.
Definition: StateOptions.h:95
ov_msckf::VioManagerOptions::aruco_options
UpdaterOptions aruco_options
Update options for ARUCO features (pixel noise and chi2 multiplier)
Definition: VioManagerOptions.h:145
CamRadtan.h
ov_msckf::VioManagerOptions::featinit_options
ov_core::FeatureInitializerOptions featinit_options
Parameters used by our feature initialize / triangulator.
Definition: VioManagerOptions.h:445
ov_msckf::VioManagerOptions::use_aruco
bool use_aruco
If should extract aruco tags and estimate them.
Definition: VioManagerOptions.h:403
ov_msckf::VioManagerOptions::downsize_aruco
bool downsize_aruco
Will half the resolution of the aruco tag image (will be faster)
Definition: VioManagerOptions.h:406
ov_core::TrackBase::CLAHE
CLAHE
ov_msckf::VioManagerOptions::calib_camimu_dt
double calib_camimu_dt
Time offset between camera and IMU.
Definition: VioManagerOptions.h:208
ov_msckf::VioManagerOptions::init_options
ov_init::InertialInitializerOptions init_options
Our state initialization options (e.g. window size, num features, if we should get the calibration)
Definition: VioManagerOptions.h:77
ov_msckf::VioManagerOptions::use_klt
bool use_klt
If we should use KLT tracking, or descriptor matcher.
Definition: VioManagerOptions.h:400
ov_msckf::VioManagerOptions::sim_distance_threshold
double sim_distance_threshold
Definition: VioManagerOptions.h:525
ov_core::rot_2_quat
Eigen::Matrix< double, 4, 1 > rot_2_quat(const Eigen::Matrix< double, 3, 3 > &rot)
ov_msckf::VioManagerOptions::gravity_mag
double gravity_mag
Gravity magnitude in the global frame (i.e. should be 9.81 typically)
Definition: VioManagerOptions.h:190
ov_core::quat_2_Rot
Eigen::Matrix< double, 3, 3 > quat_2_Rot(const Eigen::Matrix< double, 4, 1 > &q)
ov_msckf::VioManagerOptions::num_opencv_threads
int num_opencv_threads
Threads our front-end should try to use (opencv uses this also)
Definition: VioManagerOptions.h:412
ov_msckf::VioManagerOptions::use_multi_threading_subs
bool use_multi_threading_subs
If our ROS subscriber callbacks should be async (if sim and serial then this should be no!...
Definition: VioManagerOptions.h:418
ov_msckf::VioManagerOptions::imu_noises
NoiseManager imu_noises
Continuous-time IMU noise (gyroscope and accelerometer)
Definition: VioManagerOptions.h:136
ov_msckf::VioManagerOptions::q_ACCtoIMU
Eigen::Matrix< double, 4, 1 > q_ACCtoIMU
Rotation from gyroscope frame to the "IMU" accelerometer frame.
Definition: VioManagerOptions.h:202
CamEqui.h
ov_msckf::VioManagerOptions::sim_seed_measurements
int sim_seed_measurements
Definition: VioManagerOptions.h:515
ov_core::TrackBase::HISTOGRAM
HISTOGRAM
print.h
BOLDRED
#define BOLDRED
ov_msckf::StateOptions
Struct which stores all our filter options.
Definition: StateOptions.h:35
colors.h
ov_msckf::VioManagerOptions::vec_tg
Eigen::Matrix< double, 9, 1 > vec_tg
Gyroscope gravity sensitivity (scale imperfection and axis misalignment, column-wise)
Definition: VioManagerOptions.h:199
ov_msckf::StateOptions::imu_model
ImuModel imu_model
What model our IMU intrinsics are.
Definition: StateOptions.h:65
ov_msckf::VioManagerOptions::q_GYROtoIMU
Eigen::Matrix< double, 4, 1 > q_GYROtoIMU
Rotation from accelerometer to the "IMU" gyroscope frame frame.
Definition: VioManagerOptions.h:205
ov_msckf::VioManagerOptions::num_pts
int num_pts
The number of points we should extract and track in each image frame. This highly effects the computa...
Definition: VioManagerOptions.h:421
RESET
#define RESET
NoiseManager.h
ov_msckf::VioManagerOptions::print_and_load_estimator
void print_and_load_estimator(const std::shared_ptr< ov_core::YamlParser > &parser=nullptr)
This function will load print out all estimator settings loaded. This allows for visual checking that...
Definition: VioManagerOptions.h:109
ov_msckf::VioManagerOptions::state_options
StateOptions state_options
Core state options (e.g. number of cameras, use fej, stereo, what calibration to enable etc)
Definition: VioManagerOptions.h:74
ov_msckf::VioManagerOptions::print_and_load_simulation
void print_and_load_simulation(const std::shared_ptr< ov_core::YamlParser > &parser=nullptr)
This function will load print out all simulated parameters. This allows for visual checking that ever...
Definition: VioManagerOptions.h:545
ov_msckf::VioManagerOptions
Struct which stores all options needed for state estimation.
Definition: VioManagerOptions.h:56
TrackBase.h
ov_msckf::UpdaterOptions::sigma_pix
double sigma_pix
Noise sigma for our raw pixel measurements.
Definition: UpdaterOptions.h:38
ov_msckf::StateOptions::num_cameras
int num_cameras
Number of distinct cameras that we will observe features in.
Definition: StateOptions.h:83
ov_msckf::VioManagerOptions::track_frequency
double track_frequency
Frequency we want to track images at (higher freq ones will be dropped)
Definition: VioManagerOptions.h:442
ov_msckf::VioManagerOptions::zupt_options
UpdaterOptions zupt_options
Update options for zero velocity (chi2 multiplier)
Definition: VioManagerOptions.h:148
ov_msckf::VioManagerOptions::vec_da
Eigen::Matrix< double, 6, 1 > vec_da
Accelerometer IMU intrinsics (scale imperfection and axis misalignment, column-wise,...
Definition: VioManagerOptions.h:196
ov_msckf::VioManagerOptions::knn_ratio
double knn_ratio
KNN ration between top two descriptor matcher which is required to be a good match.
Definition: VioManagerOptions.h:439
ov_msckf::UpdaterOptions::sigma_pix_sq
double sigma_pix_sq
Covariance for our raw pixel measurements.
Definition: UpdaterOptions.h:41
UpdaterOptions.h
ov_core::FeatureInitializerOptions::print
void print(const std::shared_ptr< ov_core::YamlParser > &parser=nullptr)
ov_msckf::VioManagerOptions::print_and_load_noise
void print_and_load_noise(const std::shared_ptr< ov_core::YamlParser > &parser=nullptr)
This function will load print out all noise parameters loaded. This allows for visual checking that e...
Definition: VioManagerOptions.h:156
ov_msckf::NoiseManager
Struct of our imu noise parameters.
Definition: NoiseManager.h:34
ov_msckf::VioManagerOptions::record_timing_filepath
std::string record_timing_filepath
The path to the file we will record the timing information into.
Definition: VioManagerOptions.h:101
ov_msckf::VioManagerOptions::print_and_load_state
void print_and_load_state(const std::shared_ptr< ov_core::YamlParser > &parser=nullptr)
This function will load and print all state parameters (e.g. sensor extrinsics) This allows for visua...
Definition: VioManagerOptions.h:228
ov_msckf::VioManagerOptions::sim_freq_imu
double sim_freq_imu
Frequency (Hz) that we will simulate our inertial measurement unit.
Definition: VioManagerOptions.h:531
RED
RED
PRINT_WARNING
#define PRINT_WARNING(x...)
ov_msckf::UpdaterOptions::chi2_multipler
double chi2_multipler
What chi-squared multipler we should apply.
Definition: UpdaterOptions.h:35


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