Merge pull request #337 from rpng/develop_v2.7

Develop v2.7 - IMU Intrinsic Support

Doxyfile

@@ -1761,7 +1761,7 @@ LATEX_BIB_STYLE        = plainnat
 # The default value is: NO.
 # This tag requires that the tag GENERATE_LATEX is set to YES.
 
-LATEX_TIMESTAMP        = NO
+LATEX_TIMESTAMP        = YES
 
 #---------------------------------------------------------------------------
 # Configuration options related to the RTF output

ReadMe.md

@@ -21,7 +21,7 @@ details on what the system supports.
 
 ## News / Events
 
-
+* **May 11, 2023** - Inertial intrinsic support released as part of v2.7 along with a few bug fixes and improvements to stereo KLT tracking. Please check out the [release page](https://github.com/rpng/open_vins/releases/tag/v2.7) for details.
 * **April 15, 2023** - Minor update to v2.6.3 to support incremental feature triangulation of active features for downstream applications, faster zero-velocity update, small bug fixes, some example realsense configurations, and cached fast state prediction. Please check out the [release page](https://github.com/rpng/open_vins/releases/tag/v2.6.3) for details.
 * **April 3, 2023** - We have released a monocular plane-aided VINS, termed [ov_plane](https://github.com/rpng/ov_plane), which leverages the OpenVINS project. Both now support the released [Indoor AR Table](https://github.com/rpng/ar_table_dataset) dataset.
 * **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) and v2.6.2 [PR#264](https://github.com/rpng/open_vins/pull/264).
@@ -79,6 +79,7 @@ details on what the system supports.
     * Camera to IMU transform
     * Camera to IMU time offset
     * Camera intrinsics
+    * Inertial intrinsics (including g-sensitivity)
 * Environmental SLAM feature
     * OpenCV ARUCO tag SLAM features
     * Sparse feature SLAM features
@@ -90,7 +91,7 @@ details on what the system supports.
     * Masked tracking
 * Static and dynamic state initialization
 * Zero velocity detection and updates
-* Out of the box evaluation on EurocMav, TUM-VI, UZH-FPV, KAIST Urban and VIO datasets
+* Out of the box evaluation on EuRocMav, TUM-VI, UZH-FPV, KAIST Urban and other VIO datasets
 * Extensive evaluation suite (ATE, RPE, NEES, RMSE, etc..)
 
 ## Codebase Extensions

config/euroc_mav/estimator_config.yaml

@@ -3,15 +3,15 @@
 verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
 
 use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
-use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it
-use_rk4int: true # if rk4 integration should be used (overrides imu averaging)
-
+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: true # if the transform between camera and IMU should be optimized R_ItoC, p_CinI
 calib_cam_intrinsics: true # if camera intrinsics should be optimized (focal, center, distortion)
 calib_cam_timeoffset: true # 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
@@ -40,7 +40,7 @@ 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: 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_max_features: 50 # 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)

config/euroc_mav/kalibr_imu_chain.yaml

@@ -10,7 +10,35 @@ imu0:
   accelerometer_random_walk: 3.0000e-3    # [ 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 )
-  model: calibrated
   rostopic: /imu0
   time_offset: 0.0
-  update_rate: 200.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:M:
+  #   - 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/kaist/estimator_config.yaml

@@ -1,17 +1,17 @@
 %YAML:1.0 # need to specify the file type at the top!
 
-verbosity: "DEBUG" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
-
-use_fej: true # if we should use first-estimate Jacobians (enable for good consistency)
-use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it
-use_rk4int: true # if rk4 integration should be used (overrides imu averaging)
+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
-max_cameras: 2
+max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)
 
 calib_cam_extrinsics: false # degenerate motion
 calib_cam_intrinsics: true
 calib_cam_timeoffset: true # degenerate motion
+calib_imu_intrinsics: false
+calib_imu_g_sensitivity: false
 
 max_clones: 11
 max_slam: 50
@@ -40,7 +40,7 @@ zupt_only_at_beginning: false
 init_window_time: 2.0
 init_imu_thresh: 0.5
 init_max_disparity: 1.5
-init_max_features: 75
+init_max_features: 50
 
 init_dyn_use: true
 init_dyn_mle_opt_calib: false

config/kaist/kalibr_imu_chain.yaml

@@ -18,7 +18,35 @@ imu0:
   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
-  update_rate: 500.0
+  update_rate: 500.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:M:
+  #   - 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/kaist_vio/estimator_config.yaml

@@ -3,15 +3,15 @@
 verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
 
 use_fej: true # if first-estimate Jacobians should be used (enable for good consistency)
-use_imuavg: true # if using discrete integration, if we should average sequential IMU measurements to "smooth" it
-use_rk4int: true # if rk4 integration should be used (overrides imu averaging)
-
+integration: "rk4" # discrete, rk4, analytical (if rk4 or analytical used then analytical covariance propagation is used)
 use_stereo: true # NEED TO USE STEREO! OTHERWISE CAN'T RECOVER SCALE!!!!!! DEGENERATE MOTION!!!
 max_cameras: 2 # NEED TO USE STEREO! OTHERWISE CAN'T RECOVER SCALE!!!!!! DEGENERATE MOTION!!!
 
 calib_cam_extrinsics: false # disable since this is a degenerate motion
 calib_cam_intrinsics: true
 calib_cam_timeoffset: false # disable since this is a degenerate motion
+calib_imu_intrinsics: false
+calib_imu_g_sensitivity: false
 
 max_clones: 11 # how many clones in the sliding window
 max_slam: 50 # number of features in our state vector
@@ -40,7 +40,7 @@ zupt_only_at_beginning: false
 init_window_time: 2.0 # how many seconds to collect initialization information
 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_max_features: 50 # how many features to track during initialization (saves on computation)
 
 init_dyn_use: false
 init_dyn_mle_opt_calib: false

config/kaist_vio/kalibr_imu_chain.yaml

@@ -14,7 +14,35 @@ imu0:
   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
-  update_rate: 100.0
+  update_rate: 100.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:M:
+  #   - 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/rpng_aruco/estimator_config.yaml

@@ -2,16 +2,16 @@
 
 verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
 
-use_fej: true # if we should use first-estimate Jacobians (enable for good consistency)
-use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it
-use_rk4int: true # if rk4 integration should be used (overrides imu averaging)
-
+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
-max_cameras: 2
+max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)
 
 calib_cam_extrinsics: true
 calib_cam_intrinsics: true
 calib_cam_timeoffset: true
+calib_imu_intrinsics: false
+calib_imu_g_sensitivity: false
 
 max_clones: 11
 max_slam: 50
@@ -40,7 +40,7 @@ zupt_only_at_beginning: true
 init_window_time: 2.0
 init_imu_thresh: 1.2
 init_max_disparity: 2.0
-init_max_features: 75
+init_max_features: 50
 
 init_dyn_use: false
 init_dyn_mle_opt_calib: false

config/rpng_aruco/kalibr_imu_chain.yaml

@@ -10,7 +10,35 @@ imu0:
   accelerometer_random_walk: 3.0000e-3    # [ 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 )
-  model: calibrated
   rostopic: /imu0
   time_offset: 0.0
-  update_rate: 200.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:M:
+  #   - 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/rpng_ironsides/estimator_config.yaml

@@ -2,16 +2,16 @@
 
 verbosity: "INFO" # ALL, DEBUG, INFO, WARNING, ERROR, SILENT
 
-use_fej: true # if we should use first-estimate Jacobians (enable for good consistency)
-use_imuavg: true # for our discrete integration, if we should average sequential IMU measurements to "smooth" it
-use_rk4int: true # if rk4 integration should be used (overrides imu averaging)
-
+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
-max_cameras: 2
+max_cameras: 2 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (all mono tracking)
 
 calib_cam_extrinsics: false # degenerate motion
 calib_cam_intrinsics: true
 calib_cam_timeoffset: false # degenerate motion
+calib_imu_intrinsics: false
+calib_imu_g_sensitivity: false
 
 max_clones: 11
 max_slam: 50
@@ -40,7 +40,7 @@ zupt_only_at_beginning: false
 init_window_time: 2.0
 init_imu_thresh: 0.5
 init_max_disparity: 1.5
-init_max_features: 75
+init_max_features: 50
 
 init_dyn_use: false
 init_dyn_mle_opt_calib: false

config/rpng_ironsides/kalibr_imu_chain.yaml

@@ -10,7 +10,35 @@ imu0:
   accelerometer_random_walk: 1.3054568211204843e-04   # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
   gyroscope_noise_density: 1.1186830841306218e-04     # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
   gyroscope_random_walk: 8.997530210630026e-07        # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
-  model: calibrated
   rostopic: /imu0
   time_offset: 0.0
-  update_rate: 200.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:M:
+  #   - 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 ]