Merge pull request #259 from rpng/develop_v2.6.1

Develop v2.6.1

.gitignore

@@ -7,3 +7,4 @@ doxgen_generated
 *.swp
 *.swo
 doc
+.vscode

ReadMe.md

@@ -17,10 +17,11 @@ details on what the system supports.
 * Github project page - https://github.com/rpng/open_vins
 * Documentation - https://docs.openvins.com/
 * Getting started guide - https://docs.openvins.com/getting-started.html
-* Publication reference - http://udel.edu/~pgeneva/downloads/papers/c10.pdf
+* Publication reference - https://pgeneva.com/downloads/papers/Geneva2020ICRA.pdf
 
 ## News / Events
 
+* **July 14, 2022** - Improved feature extraction logic for >100hz tracking, some bug fixes and updated scripts. See v2.6.1 [PR#259](https://github.com/rpng/open_vins/pull/259).
 * **March 14, 2022** - Initial dynamic initialization open sourcing, asynchronous subscription to inertial readings and publishing of odometry, support for lower frequency feature tracking. See v2.6 [PR#232](https://github.com/rpng/open_vins/pull/232) for details.
 * **December 13, 2021** - New YAML configuration system, ROS2 support, Docker images, robust static initialization based on disparity, internal logging system to reduce verbosity, image transport publishers, dynamic number of features support, and other small fixes. See
   v2.5 [PR#209](https://github.com/rpng/open_vins/pull/209) for details.

config/euroc_mav/estimator_config.yaml

@@ -27,7 +27,7 @@ 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
+try_zupt: false
 zupt_chi2_multipler: 0 # set to 0 for only disp-based
 zupt_max_velocity: 0.1
 zupt_noise_multiplier: 50
@@ -39,24 +39,25 @@ zupt_only_at_beginning: true
 
 init_window_time: 2.0 # how many seconds to collect initialization information
 init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion
-init_max_disparity: 1.5 # max disparity to consider the platform stationary (dependent on resolution)
-init_max_features: 25 # how many features to track during initialization (saves on computation)
-
-init_dyn_mle_opt_calib: false
-init_dyn_mle_max_iter: 50
-init_dyn_mle_max_time: 0.05
-init_dyn_mle_max_threads: 6
-init_dyn_num_pose: 6
-init_dyn_min_deg: 10.0
-
-init_dyn_inflation_ori: 10
-init_dyn_inflation_vel: 100
-init_dyn_inflation_bg: 10
-init_dyn_inflation_ba: 100
-init_dyn_min_rec_cond: 1e-12
-
-init_dyn_bias_g: [0.0, 0.0, 0.0]
-init_dyn_bias_a: [0.0, 0.0, 0.0]
+init_max_disparity: 10.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: false # 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: 10.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
 
 # ==================================================================
 # ==================================================================
@@ -76,14 +77,14 @@ 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
-num_pts: 200
-fast_threshold: 20
-grid_x: 20
-grid_y: 20
-min_px_dist: 15
-knn_ratio: 0.70
-track_frequency: 21.0
+use_klt: true # if true we will use KLT, otherwise use a ORB descriptor + robust matching
+num_pts: 200 # number of points (per camera) we will extract and try to track
+fast_threshold: 20 # threshold for fast extraction (warning: lower threshs can be expensive)
+grid_x: 5 # extraction sub-grid count for horizontal direction (uniform tracking)
+grid_y: 5 # extraction sub-grid count for vertical direction (uniform tracking)
+min_px_dist: 10 # distance between features (features near each other provide less information)
+knn_ratio: 0.70 # 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
 multi_threading: true # if should enable opencv multi threading
 histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE

config/kaist/estimator_config.yaml

@@ -9,17 +9,16 @@ use_rk4int: true # if rk4 integration should be used (overrides imu averaging)
 use_stereo: true # if we have more than 1 camera, if we should try to track stereo constraints
 max_cameras: 2
 
-calib_cam_extrinsics: false
+calib_cam_extrinsics: false # degenerate motion
 calib_cam_intrinsics: true
-calib_cam_timeoffset: true
+calib_cam_timeoffset: true # degenerate motion
 
-max_clones: 12
+max_clones: 8
 max_slam: 50
 max_slam_in_update: 25
 max_msckf_in_update: 50
 dt_slam_delay: 1
 
-#gravity_mag: 9.79858
 gravity_mag: 9.81
 
 feat_rep_msckf: "ANCHORED_MSCKF_INVERSE_DEPTH"
@@ -29,7 +28,7 @@ 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: 1 # set to 0 for only disp-based
+zupt_chi2_multipler: 0.25 # set to 0 for only disp-based
 zupt_max_velocity: 0.1
 zupt_noise_multiplier: 5
 zupt_max_disparity: 0.4 # set to 0 for only imu-based
@@ -39,16 +38,17 @@ zupt_only_at_beginning: false
 # ==================================================================
 
 init_window_time: 2.0
-init_imu_thresh: 0.5 #0.5
+init_imu_thresh: 0.5
 init_max_disparity: 1.5
-init_max_features: 25
+init_max_features: 75
 
+init_dyn_use: true
 init_dyn_mle_opt_calib: false
 init_dyn_mle_max_iter: 50
 init_dyn_mle_max_time: 0.05
 init_dyn_mle_max_threads: 6
 init_dyn_num_pose: 6
-init_dyn_min_deg: 0.0 # traj is mostly straight line motion
+init_dyn_min_deg: 5.0 # traj is mostly straight line motion
 
 init_dyn_inflation_ori: 10
 init_dyn_inflation_vel: 100
@@ -78,18 +78,19 @@ filepath_gt: "/tmp/ov_groundtruth.txt"
 # we have a KLT and descriptor based (KLT is better implemented...)
 use_klt: true
 num_pts: 200
-fast_threshold: 50
-grid_x: 20
-grid_y: 15
-min_px_dist: 30
+fast_threshold: 30
+grid_x: 5
+grid_y: 5
+min_px_dist: 20
 knn_ratio: 0.65
-track_frequency: 21.0
+track_frequency: 31.0
 downsample_cameras: false
 multi_threading: true
 histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE
 
-fi_max_dist: 500
-fi_max_baseline: 800
+fi_min_dist: 0.25
+fi_max_dist: 150.0
+fi_max_baseline: 200
 fi_max_cond_number: 20000
 fi_triangulate_1d: false
 
@@ -103,9 +104,9 @@ downsize_aruco: true
 # ==================================================================
 
 # camera noises and chi-squared threshold multipliers
-up_msckf_sigma_px: 1
+up_msckf_sigma_px: 1.5
 up_msckf_chi2_multipler: 1
-up_slam_sigma_px: 1
+up_slam_sigma_px: 1.5
 up_slam_chi2_multipler: 1
 up_aruco_sigma_px: 1
 up_aruco_chi2_multipler: 1

config/kaist/kalibr_imu_chain.yaml

@@ -1,22 +1,23 @@
 %YAML:1.0 # need to specify the file type at the top!
 
 
-# MTI-100 series converted from data sheet, guess on bias random walk
+# MTI-100 series converted from datasheet, guess on bias random walk
 # https://www.xsens.com/hubfs/Downloads/usermanual/MTi_usermanual.pdf
+
 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: 2.0000e-03  # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
-  accelerometer_random_walk: 3.0000e-03   # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
-  gyroscope_noise_density: 1.6968e-04     # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
-  gyroscope_random_walk: 1.9393e-05       # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
-  # <param name="gyroscope_noise_density"      type="double"   value="1.7453e-04" /> <!-- 1.7453e-04 -->
-  # <param name="gyroscope_random_walk"        type="double"   value="1.0000e-05" />
-  # <param name="accelerometer_noise_density"  type="double"   value="5.8860e-03" /> <!-- 5.8860e-04 -->
-  # <param name="accelerometer_random_walk"    type="double"   value="1.0000e-04" />
+#  accelerometer_noise_density: 2.0000e-03  # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
+#  accelerometer_random_walk: 3.0000e-03    # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
+#  gyroscope_noise_density: 1.6968e-04      # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
+#  gyroscope_random_walk: 1.9393e-05        # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
+  accelerometer_noise_density: 5.8860e-03  # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
+  accelerometer_random_walk: 1.0000e-04    # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
+  gyroscope_noise_density: 1.7453e-04      # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
+  gyroscope_random_walk:  1.0000e-05      # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
   model: calibrated
   rostopic: /imu/data_raw
   time_offset: 0.0

config/kaist/kalibr_imucam_chain.yaml

@@ -13,6 +13,7 @@ cam0:
   intrinsics: [8.1690378992770002e+02,8.1156803828490001e+02,6.0850726281690004e+02,2.6347599764440002e+02]
   resolution: [1280, 560]
   rostopic: /stereo/left/image_raw
+
 cam1:
   T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
     - [-0.00768,-0.01509,0.99986,1.73376]
@@ -25,4 +26,4 @@ cam1:
   distortion_model: radtan
   intrinsics: [8.1378205539589999e+02,8.0852165574269998e+02,6.1386419539320002e+02,2.4941049348650000e+02]
   resolution: [1280, 560]
-  rostopic: /stereo/right/image_raw
+  rostopic: /stereo/right/image_raw

config/kaist_vio/estimator_config.yaml

@@ -16,18 +16,18 @@ calib_cam_timeoffset: false # disable since this is a degenerate motion
 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
+max_msckf_in_update: 50 # 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.81 # magnitude of gravity in this location
 
-feat_rep_msckf: "GLOBAL_3D"
+feat_rep_msckf: "ANCHORED_MSCKF_INVERSE_DEPTH"
 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
+try_zupt: false
 zupt_chi2_multipler: 0 # set to 0 for only disp-based
 zupt_max_velocity: 0.02
 zupt_noise_multiplier: 10
@@ -38,10 +38,11 @@ zupt_only_at_beginning: false
 # ==================================================================
 
 init_window_time: 2.0 # how many seconds to collect initialization information
-init_imu_thresh: 1.5 # threshold for variance of the accelerometer to detect a "jerk" in motion
-init_max_disparity: 0.20 # max disparity to consider the platform stationary (dependent on resolution)
-init_max_features: 25 # how many features to track during initialization (saves on computation)
+init_imu_thresh: 0.60 # threshold for variance of the accelerometer to detect a "jerk" in motion
+init_max_disparity: 5.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: false
 init_dyn_mle_opt_calib: false
 init_dyn_mle_max_iter: 50
 init_dyn_mle_max_time: 0.05
@@ -79,11 +80,11 @@ filepath_gt: "/tmp/ov_groundtruth.txt"
 use_klt: true
 num_pts: 200
 fast_threshold: 30
-grid_x: 20
-grid_y: 20
+grid_x: 5
+grid_y: 5
 min_px_dist: 15
 knn_ratio: 0.70
-track_frequency: 10.0
+track_frequency: 31.0
 downsample_cameras: false # will downsample image in half if true
 multi_threading: true # if should enable opencv multi threading
 histogram_method: "HISTOGRAM" # NONE, HISTOGRAM, CLAHE

config/kaist_vio/kalibr_imu_chain.yaml

@@ -6,14 +6,14 @@ imu0:
     - [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.00333388 # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
-  accelerometer_random_walk: 0.00047402   # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
-  gyroscope_noise_density: 0.00005770     # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
-  gyroscope_random_walk: 0.00001565       # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
-#  accelerometer_noise_density: 0.07   # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
-#  accelerometer_random_walk: 0.009   # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
-#  gyroscope_noise_density: 0.001      # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
-#  gyroscope_random_walk: 0.0003      # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
+#  accelerometer_noise_density: 0.00333388 # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
+#  accelerometer_random_walk: 0.00047402   # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
+#  gyroscope_noise_density: 0.00005770     # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
+#  gyroscope_random_walk: 0.00001565       # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
+  accelerometer_noise_density: 0.07   # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
+  accelerometer_random_walk: 0.009   # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
+  gyroscope_noise_density: 0.001      # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
+  gyroscope_random_walk: 0.0003      # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
   model: calibrated
   rostopic: /mavros/imu/data
   time_offset: 0.0

config/rpng_aruco/estimator_config.yaml

@@ -27,7 +27,7 @@ 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
+try_zupt: false
 zupt_chi2_multipler: 0 # set to 0 for only disp-based
 zupt_max_velocity: 0.1
 zupt_noise_multiplier: 50
@@ -39,9 +39,10 @@ zupt_only_at_beginning: true
 
 init_window_time: 2.0
 init_imu_thresh: 1.2
-init_max_disparity: 1.5
-init_max_features: 25
+init_max_disparity: 2.0
+init_max_features: 75
 
+init_dyn_use: false
 init_dyn_mle_opt_calib: false
 init_dyn_mle_max_iter: 50
 init_dyn_mle_max_time: 0.05
@@ -77,9 +78,9 @@ filepath_gt: "/tmp/ov_groundtruth.txt"
 use_klt: true
 num_pts: 150
 fast_threshold: 30
-grid_x: 20
-grid_y: 20
-min_px_dist: 30
+grid_x: 5
+grid_y: 5
+min_px_dist: 20
 knn_ratio: 0.85
 track_frequency: 21.0
 downsample_cameras: false
@@ -96,12 +97,12 @@ downsize_aruco: true
 # ==================================================================
 
 # camera noises and chi-squared threshold multipliers
-up_msckf_sigma_px: 1
+up_msckf_sigma_px: 1.5
 up_msckf_chi2_multipler: 1
-up_slam_sigma_px: 1
+up_slam_sigma_px: 1.5
 up_slam_chi2_multipler: 1
-up_aruco_sigma_px: 1
-up_aruco_chi2_multipler: 8
+up_aruco_sigma_px: 2.0
+up_aruco_chi2_multipler: 10
 
 # masks for our images
 use_mask: false