Add dynamic masking support and VIODE dataset configuration

config/viode_config/estimator_config.yaml

@@ -0,0 +1,124 @@
+%YAML:1.0 # need to specify the file type at the top!
+
+verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
+
+use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
+integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used)
+use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints between pairs
+max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)
+
+calib_cam_extrinsics: false # if the transform between camera and IMU should be optimized R_ItoC, p_CinI
+calib_cam_intrinsics: false # if camera intrinsics should be optimized (focal, center, distortion)
+calib_cam_timeoffset: false # if timeoffset between camera and IMU should be optimized
+calib_imu_intrinsics: false # if imu intrinsics should be calibrated (rotation and skew-scale matrix)
+calib_imu_g_sensitivity: false # if gyroscope gravity sensitivity (Tg) should be calibrated
+
+max_clones: 11 # how many clones in the sliding window
+max_slam: 50 # number of features in our state vector
+max_slam_in_update: 25 # update can be split into sequential updates of batches, how many in a batch
+max_msckf_in_update: 40 # how many MSCKF features to use in the update
+dt_slam_delay: 1 # delay before initializing (helps with stability from bad initialization...)
+
+gravity_mag: 9.81007 # magnitude of gravity in this location
+
+feat_rep_msckf: "GLOBAL_3D"
+feat_rep_slam: "ANCHORED_MSCKF_INVERSE_DEPTH"
+feat_rep_aruco: "ANCHORED_MSCKF_INVERSE_DEPTH"
+
+# zero velocity update parameters we can use
+# we support either IMU-based or disparity detection.
+try_zupt: true
+zupt_chi2_multipler: 0 # set to 0 for only disp-based
+zupt_max_velocity: 0.1
+zupt_noise_multiplier: 10
+zupt_max_disparity: 0.5 # set to 0 for only imu-based
+zupt_only_at_beginning: false
+
+# ==================================================================
+# ==================================================================
+
+init_window_time: 1.2 # how many seconds to collect initialization information
+init_imu_thresh: 0.5 # threshold for variance of the accelerometer to detect a "jerk" in motion
+init_max_disparity: 20.0 # max disparity to consider the platform stationary (dependent on resolution)
+init_max_features: 75 # how many features to track during initialization (saves on computation)
+
+init_dyn_use: true # if dynamic initialization should be used
+init_dyn_mle_opt_calib: false # if we should optimize calibration during intialization (not recommended)
+init_dyn_mle_max_iter: 50 # how many iterations the MLE refinement should use (zero to skip the MLE)
+init_dyn_mle_max_time: 0.05 # how many seconds the MLE should be completed in
+init_dyn_mle_max_threads: 6 # how many threads the MLE should use
+init_dyn_num_pose: 6 # number of poses to use within our window time (evenly spaced)
+init_dyn_min_deg: 5.0 # orientation change needed to try to init
+
+init_dyn_inflation_ori: 10 # what to inflate the recovered q_GtoI covariance by
+init_dyn_inflation_vel: 100 # what to inflate the recovered v_IinG covariance by
+init_dyn_inflation_bg: 10 # what to inflate the recovered bias_g covariance by
+init_dyn_inflation_ba: 100 # what to inflate the recovered bias_a covariance by
+init_dyn_min_rec_cond: 1e-12 # reciprocal condition number thresh for info inversion
+
+init_dyn_bias_g: [ 0.0, 0.0, 0.0 ] # initial gyroscope bias guess
+init_dyn_bias_a: [ 0.0, 0.0, 0.0 ] # initial accelerometer bias guess
+
+# ==================================================================
+# ==================================================================
+
+record_timing_information: false # if we want to record timing information of the method
+record_timing_filepath: "/tmp/traj_timing.txt" # https://docs.openvins.com/eval-timing.html#eval-ov-timing-flame
+
+# if we want to save the simulation state and its diagional covariance
+# use this with rosrun ov_eval error_simulation
+save_total_state: false
+filepath_est: "/tmp/ov_estimate.txt"
+filepath_std: "/tmp/ov_estimate_std.txt"
+filepath_gt: "/tmp/ov_groundtruth.txt"
+
+# ==================================================================
+# ==================================================================
+
+# our front-end feature tracking parameters
+# we have a KLT and descriptor based (KLT is better implemented...)
+use_klt: true # if true we will use KLT, otherwise use a ORB descriptor + robust matching
+num_pts: 600 # number of points (per camera) we will extract and try to track
+fast_threshold: 10 # threshold for fast extraction (warning: lower threshs can be expensive)
+grid_x: 6 # extraction sub-grid count for horizontal direction (uniform tracking)
+grid_y: 4 # extraction sub-grid count for vertical direction (uniform tracking)
+min_px_dist: 15 # distance between features (features near each other provide less information)
+knn_ratio: 0.90 # descriptor knn threshold for the top two descriptor matches
+track_frequency: 21.0 # frequency we will perform feature tracking at (in frames per second / hertz)
+downsample_cameras: false # will downsample image in half if true
+num_opencv_threads: 1 # -1: auto, 0-1: serial, >1: number of threads
+histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
+
+# aruco tag tracker for the system
+# DICT_6X6_1000 from https://chev.me/arucogen/
+use_aruco: false
+num_aruco: 1024
+downsize_aruco: true
+
+# ==================================================================
+# ==================================================================
+
+# camera noises and chi-squared threshold multipliers
+up_msckf_sigma_px: 1.5
+up_msckf_chi2_multipler: 1
+up_slam_sigma_px: 1
+up_slam_chi2_multipler: 1
+up_aruco_sigma_px: 1
+up_aruco_chi2_multipler: 1
+
+# masks for our images
+use_mask: false
+# No need when use mask is false
+mask0: "dummy_mask.png"
+mask1: "dummy_mask.png"
+
+# imu and camera spacial-temporal
+# imu config should also have the correct noise values
+relative_config_imu: "kalibr_imu_chain.yaml"
+relative_config_imucam: "kalibr_imucam_chain.yaml"
+
+# Custom Dynamic Mask
+use_dynamic_mask: true
+dynamic_mask_topic0: "/cam0/masked"
+dynamic_mask_topic1: "/cam1/masked"
+

config/viode_config/kalibr_imu_chain.yaml

@@ -0,0 +1,44 @@
+%YAML:1.0
+
+imu0:
+  T_i_b:
+    - [1.0, 0.0, 0.0, 0.0]
+    - [0.0, 1.0, 0.0, 0.0]
+    - [0.0, 0.0, 1.0, 0.0]
+    - [0.0, 0.0, 0.0, 1.0]
+  accelerometer_noise_density: 0.2  # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
+  accelerometer_random_walk: 0.02    # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
+  gyroscope_noise_density: 0.05     # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
+  gyroscope_random_walk: 4.0e-5       # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
+  rostopic: /imu0
+  time_offset: 0.0
+  update_rate: 200.0
+  # three different modes supported:
+  # "calibrated" (same as "kalibr"), "kalibr", "rpng"
+  model: "kalibr"
+  # how to get from Kalibr imu.yaml result file:
+  #   - Tw is imu0:gyroscopes:M:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:C_gyro_i:
+  #   - Ta is imu0:accelerometers:M:
+  #   - R_IMUtoACC not used by Kalibr
+  #   - Tg is imu0:gyroscopes:A:
+  Tw:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  R_IMUtoGYRO:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  Ta:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  R_IMUtoACC:
+    - [ 1.0, 0.0, 0.0 ]
+    - [ 0.0, 1.0, 0.0 ]
+    - [ 0.0, 0.0, 1.0 ]
+  Tg:
+    - [ 0.0, 0.0, 0.0 ]
+    - [ 0.0, 0.0, 0.0 ]
+    - [ 0.0, 0.0, 0.0 ]

config/viode_config/kalibr_imucam_chain.yaml

@@ -0,0 +1,28 @@
+%YAML:1.0
+
+cam0:
+  T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
+    - [0.0, 0.0, 1.0, 0.0]
+    - [1.0, 0.0, 0.0, 0.0]
+    - [0.0, 1.0, 0.0, 0.0]
+    - [0.0, 0.0, 0.0, 1.0]
+  cam_overlaps: [1]
+  camera_model: pinhole
+  distortion_coeffs: [0.0, 0.0, 0.0, 0.0]
+  distortion_model: radtan
+  intrinsics: [376.0, 376.0, 376.0, 240.0] #fu, fv, cu, cv
+  resolution: [752, 480]
+  rostopic: /cam0/image_raw
+cam1:
+  T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
+    - [0.0, 0.0, 1.0, 0.0]
+    - [1.0, 0.0, 0.0, 0.05]
+    - [0.0, 1.0, 0.0, 0.0]
+    - [0.0, 0.0, 0.0, 1.0]
+  cam_overlaps: [0]
+  camera_model: pinhole
+  distortion_coeffs: [0.0, 0.0, 0.0, 0.0]
+  distortion_model: radtan
+  intrinsics: [376.0, 376.0, 376.0, 240.0] #fu, fv, cu, cv
+  resolution: [752, 480]
+  rostopic: /cam1/image_raw

ov_core/src/plot/matplotlibcpp.h

@@ -257,18 +257,42 @@ inline bool annotate(std::string annotation, double x, double y) {
 
 #ifndef WITHOUT_NUMPY
 // Type selector for numpy array conversion
-template <typename T> struct select_npy_type { const static NPY_TYPES type = NPY_NOTYPE; }; // Default
-template <> struct select_npy_type<double> { const static NPY_TYPES type = NPY_DOUBLE; };
-template <> struct select_npy_type<float> { const static NPY_TYPES type = NPY_FLOAT; };
-template <> struct select_npy_type<bool> { const static NPY_TYPES type = NPY_BOOL; };
-template <> struct select_npy_type<int8_t> { const static NPY_TYPES type = NPY_INT8; };
-template <> struct select_npy_type<int16_t> { const static NPY_TYPES type = NPY_SHORT; };
-template <> struct select_npy_type<int32_t> { const static NPY_TYPES type = NPY_INT; };
-template <> struct select_npy_type<int64_t> { const static NPY_TYPES type = NPY_INT64; };
-template <> struct select_npy_type<uint8_t> { const static NPY_TYPES type = NPY_UINT8; };
-template <> struct select_npy_type<uint16_t> { const static NPY_TYPES type = NPY_USHORT; };
-template <> struct select_npy_type<uint32_t> { const static NPY_TYPES type = NPY_ULONG; };
-template <> struct select_npy_type<uint64_t> { const static NPY_TYPES type = NPY_UINT64; };
+template <typename T> struct select_npy_type {
+  const static NPY_TYPES type = NPY_NOTYPE;
+}; // Default
+template <> struct select_npy_type<double> {
+  const static NPY_TYPES type = NPY_DOUBLE;
+};
+template <> struct select_npy_type<float> {
+  const static NPY_TYPES type = NPY_FLOAT;
+};
+template <> struct select_npy_type<bool> {
+  const static NPY_TYPES type = NPY_BOOL;
+};
+template <> struct select_npy_type<int8_t> {
+  const static NPY_TYPES type = NPY_INT8;
+};
+template <> struct select_npy_type<int16_t> {
+  const static NPY_TYPES type = NPY_SHORT;
+};
+template <> struct select_npy_type<int32_t> {
+  const static NPY_TYPES type = NPY_INT;
+};
+template <> struct select_npy_type<int64_t> {
+  const static NPY_TYPES type = NPY_INT64;
+};
+template <> struct select_npy_type<uint8_t> {
+  const static NPY_TYPES type = NPY_UINT8;
+};
+template <> struct select_npy_type<uint16_t> {
+  const static NPY_TYPES type = NPY_USHORT;
+};
+template <> struct select_npy_type<uint32_t> {
+  const static NPY_TYPES type = NPY_ULONG;
+};
+template <> struct select_npy_type<uint64_t> {
+  const static NPY_TYPES type = NPY_UINT64;
+};
 
 template <typename Numeric> PyObject *get_array(const std::vector<Numeric> &v) {
   detail::_interpreter::get(); // interpreter needs to be initialized for the numpy commands to work
@@ -1659,7 +1683,9 @@ template <typename T> using is_function = typename std::is_function<std::remove_
 
 template <bool obj, typename T> struct is_callable_impl;
 
-template <typename T> struct is_callable_impl<false, T> { typedef is_function<T> type; }; // a non-object is callable iff it is a function
+template <typename T> struct is_callable_impl<false, T> {
+  typedef is_function<T> type;
+}; // a non-object is callable iff it is a function
 
 template <typename T> struct is_callable_impl<true, T> {
   struct Fallback {

ov_core/src/track/TrackAruco.cpp

@@ -94,7 +94,7 @@ void TrackAruco::perform_tracking(double timestamp, const cv::Mat &imgin, size_t
   //===================================================================================
 
   // Perform extraction
-#if CV_MAJOR_VERSION > 4 || ( CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
   aruco_detector.detectMarkers(img0, corners[cam_id], ids_aruco[cam_id], rejects[cam_id]);
 #else
   cv::aruco::detectMarkers(img0, aruco_dict, corners[cam_id], ids_aruco[cam_id], aruco_params, rejects[cam_id]);

ov_core/src/track/TrackAruco.h

@@ -55,7 +55,7 @@ class TrackAruco : public TrackBase {
                       bool downsize)
       : TrackBase(cameras, 0, numaruco, stereo, histmethod), max_tag_id(numaruco), do_downsizing(downsize) {
 #if ENABLE_ARUCO_TAGS
-#if CV_MAJOR_VERSION > 4 || ( CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
     aruco_dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_6X6_1000);
     aruco_params.cornerRefinementMethod = cv::aruco::CORNER_REFINE_SUBPIX;
     aruco_detector = cv::aruco::ArucoDetector(aruco_dict, aruco_params);
@@ -107,7 +107,7 @@ class TrackAruco : public TrackBase {
   bool do_downsizing;
 
 #if ENABLE_ARUCO_TAGS
-#if CV_MAJOR_VERSION > 4 || ( CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION >= 7)
   // Our dictionary that we will extract aruco tags with
   cv::aruco::Dictionary aruco_dict;
   // Parameters the opencv extractor uses
@@ -121,7 +121,6 @@ class TrackAruco : public TrackBase {
   cv::Ptr<cv::aruco::DetectorParameters> aruco_params;
 #endif
 
-
   // Our tag IDs and corner we will get from the extractor
   std::unordered_map<size_t, std::vector<int>> ids_aruco;
   std::unordered_map<size_t, std::vector<std::vector<cv::Point2f>>> corners, rejects;

ov_init/src/ceres/State_JPLQuatLocal.h

@@ -56,10 +56,10 @@ class State_JPLQuatLocal : public ceres::LocalParameterization {
   bool PlusJacobian(const double *x, double *jacobian) const override;
 
   // Inverse update: delta = Log(q2 ⊗ inv(q1))
-  bool Minus(const double* y, const double* x, double* delta) const override;
+  bool Minus(const double *y, const double *x, double *delta) const override;
 
   // Jacobian of Minus
-  bool MinusJacobian(const double* x, double* jacobian) const override;
+  bool MinusJacobian(const double *x, double *jacobian) const override;
 
   int AmbientSize() const override { return 4; }
   int TangentSize() const override { return 3; }
@@ -84,7 +84,6 @@ class State_JPLQuatLocal : public ceres::LocalParameterization {
   int LocalSize() const override { return 3; };
 
 #endif
-
 };
 
 } // namespace ov_init

ov_init/src/dynamic/DynamicInitializer.cpp

@@ -371,8 +371,8 @@ bool DynamicInitializer::initialize(double &timestamp, Eigen::MatrixXd &covarian
         } else {
           H_i.block(0, size_feature * A_index_features.at(feat.first), 2, 3) = Y; // feat
         }
-        H_i.block(0, size_feature * num_features + 0, 2, 3) = -DT * Y;            // vel
-        H_i.block(0, size_feature * num_features + 3, 2, 3) = 0.5 * DT * DT * Y;  // grav
+        H_i.block(0, size_feature * num_features + 0, 2, 3) = -DT * Y;           // vel
+        H_i.block(0, size_feature * num_features + 3, 2, 3) = 0.5 * DT * DT * Y; // grav
 
         // Else lets append this to our system!
         A.block(index_meas, 0, 2, A.cols()) = H_i;