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

@@ -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

config/euroc_mav/kalibr_imu_chain.yaml

@@ -10,7 +10,27 @@ 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
+  model: "kalibr" # calibrated (same as kalibr), kalibr, rpng
+  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

config/kaist/kalibr_imu_chain.yaml

@@ -18,7 +18,27 @@ 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
+  model: "kalibr" # calibrated (same as kalibr), kalibr, rpng
+  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

config/kaist_vio/kalibr_imu_chain.yaml

@@ -14,7 +14,27 @@ 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
+  model: "kalibr" # calibrated (same as kalibr), kalibr, rpng
+  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

config/rpng_aruco/kalibr_imu_chain.yaml

@@ -10,7 +10,27 @@ 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
+  model: "kalibr" # calibrated (same as kalibr), kalibr, rpng
+  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

config/rpng_ironsides/kalibr_imu_chain.yaml

@@ -10,7 +10,27 @@ 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
+  model: "kalibr" # calibrated (same as kalibr), kalibr, rpng
+  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_plane/estimator_config.yaml

@@ -12,6 +12,8 @@ max_cameras: 1 # how many cameras we have 1 = mono, 2 = stereo, >2 = binocular (
 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: 25 # number of features in our state vector

config/rpng_plane/kalibr_imu_chain.yaml

@@ -10,7 +10,27 @@ imu0:
   accelerometer_random_walk: 0.00041327852
   gyroscope_noise_density: 0.00020544166
   gyroscope_random_walk: 1.110622e-05
-  model: calibrated
   rostopic: /d455/imu
   time_offset: 0.0
   update_rate: 400
+  model: "kalibr" # calibrated (same as kalibr), kalibr, rpng
+  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_sim/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_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: true # if imu intrinsics should be calibrated
+calib_imu_g_sensitivity: true # if gyroscope gravity sensitivity should be calibrated
 
 max_clones: 11
 max_slam: 50
@@ -122,7 +122,7 @@ sim_seed_preturb: 0
 sim_seed_measurements: 0
 sim_do_perturbation: false
 sim_traj_path: "src/open_vins/ov_data/sim/tum_corridor1_512_16_okvis.txt"
-sim_distance_threshold: 1.2
+sim_distance_threshold: 1.1
 sim_freq_cam: 10
 sim_freq_imu: 400
 sim_min_feature_gen_dist: 5.0

config/rpng_sim/kalibr_imu_chain.yaml

@@ -10,7 +10,27 @@ 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
+  model: "kalibr" # calibrated (same as kalibr), kalibr, rpng
+  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 ]