Added support for ROS2 Jazzy and Ubuntu 24.04

ReadMe.md

@@ -1,4 +1,4 @@
-# OpenVINS
+# OpenVINS - Added support for ROS2 Jazzy + Ubuntu 24.04
 
 [![ROS 1 Workflow](https://github.com/rpng/open_vins/actions/workflows/build_ros1.yml/badge.svg)](https://github.com/rpng/open_vins/actions/workflows/build_ros1.yml)
 [![ROS 2 Workflow](https://github.com/rpng/open_vins/actions/workflows/build_ros2.yml/badge.svg)](https://github.com/rpng/open_vins/actions/workflows/build_ros2.yml)

config/rs_d455_mono/estimator_config.yaml

@@ -0,0 +1,119 @@
+%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: 1 # 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: 25 # 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: 20 # how many MSCKF features to use in the update
+dt_slam_delay: 2 # delay before initializing (helps with stability from bad initialization...)
+
+gravity_mag: 9.8065 # magnitude of gravity in this location
+
+feat_rep_msckf: "GLOBAL_3D"
+feat_rep_slam: "GLOBAL_3D"
+feat_rep_aruco: "GLOBAL_3D"
+
+# zero velocity update parameters we can use
+# we support either IMU-based or disparity detection.
+try_zupt: false
+zupt_chi2_multipler: 0 # set to 0 for only disp-based
+zupt_max_velocity: 0.1
+zupt_noise_multiplier: 50
+zupt_max_disparity: 1.5 # set to 0 for only imu-based
+zupt_only_at_beginning: true
+
+# ==================================================================
+# ==================================================================
+
+init_window_time: 2.0 # 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: 4.0 # max disparity to consider the platform stationary (dependent on resolution)
+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)
+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
+
+# ==================================================================
+# ==================================================================
+
+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: 200 # number of points (per camera) we will extract and try to track
+fast_threshold: 30 # threshold for fast extraction (warning: lower threshs can be expensive)
+grid_x: 8 # extraction sub-grid count for horizontal direction (uniform tracking)
+grid_y: 8 # extraction sub-grid count for vertical direction (uniform tracking)
+min_px_dist: 20 # 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: 31.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: 4 # -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
+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
+
+# 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"
+
+
+
+

config/rs_d455_mono/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.00207649074
+  accelerometer_random_walk: 0.00041327852
+  gyroscope_noise_density: 0.00020544166
+  gyroscope_random_walk: 1.110622e-05
+  rostopic: /d455/imu
+  time_offset: 0.0
+  update_rate: 400
+  # 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/rs_d455/estimator_config.yaml → config/rs_d455_stereo/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: false
+try_zupt: true
 zupt_chi2_multipler: 0 # set to 0 for only disp-based
 zupt_max_velocity: 0.1
 zupt_noise_multiplier: 10
@@ -38,7 +38,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_imu_thresh: 2.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: 50 # how many features to track during initialization (saves on computation)
 
@@ -112,8 +112,4 @@ use_mask: false
 # 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"
-
-
-
-
+relative_config_imucam: "kalibr_imucam_chain.yaml"

config/rs_d455/kalibr_imu_chain.yaml → config/rs_d455_stereo/kalibr_imu_chain.yaml

@@ -13,21 +13,23 @@ imu0:
   #gyroscope_noise_density: 0.00010272083263292572
   #gyroscope_random_walk: 1.1106223553679963e-06
   # Inflated values (to account for unmodelled effects)
-  #  - white noise multiplied by 2
-  #  - bias random walk multiplied by 10
-  accelerometer_noise_density: 0.00207649074
-  accelerometer_random_walk: 0.00041327852
-  gyroscope_noise_density: 0.00020544166
-  gyroscope_random_walk: 0.00001110622
-  rostopic: /d455/imu
+  # density x5
+  # walk x10
+  #Accelerometer
+  accelerometer_noise_density: 0.018325460766195713 
+  accelerometer_random_walk: 0.0031526548679969497 
+  #Gyroscope
+  gyroscope_noise_density: 0.005658445654838128
+  gyroscope_random_walk: 4.9244827968800026e-04 
+  rostopic: /camera/imu
   time_offset: 0.0
-  update_rate: 400
+  update_rate: 200
   # 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:
+  #   - R_IMUtoGYRO: is imu0:gyroscopes:M:
   #   - Ta is imu0:accelerometers:M:
   #   - R_IMUtoACC not used by Kalibr
   #   - Tg is imu0:gyroscopes:A:
@@ -50,4 +52,4 @@ imu0:
   Tg:
     - [ 0.0, 0.0, 0.0 ]
     - [ 0.0, 0.0, 0.0 ]
-    - [ 0.0, 0.0, 0.0 ]
+    - [ 0.0, 0.0, 0.0 ]

config/rs_d455_stereo/kalibr_imucam_chain.yaml

@@ -0,0 +1,51 @@
+%YAML:1.0
+
+cam0:
+  T_cam_imu:
+    - [0.9999942992129773, -0.0006837030318090254, 0.003306673813320594, 0.01734357246126995]
+    - [0.0006819183997666188, 0.9999996212593953, 0.000540803438387852, -0.008519633421954392]
+    - [-0.003307042309899398, -0.0005385454736672996, 0.9999943867042107, -0.018723905687986195]
+    - [0.0, 0.0, 0.0, 1.0]
+  cam_overlaps: [1]
+  camera_model: pinhole
+  distortion_coeffs: [0.0019712980218442736, -0.0011090942743057983, -0.0006211171700097682, -0.0034327653409294196]
+  distortion_model: radtan
+  intrinsics: [320.17488748238287, 320.0283628174116, 314.0668795454674, 180.36893447271257]
+  resolution: [640, 360]
+  rostopic: /camera/infra1/image_rect_raw
+  timeshift_cam_imu: 0.004639958649183853
+
+cam1:
+  T_cam_imu:
+    - [0.9999855287596171, -0.0003605813977934117, 0.00536770457504073, -0.07667191595392714]
+    - [0.000365189906339375, 0.9999995655732808, -0.0008576068926449692, -0.008542662389807396]
+    - [-0.005367393006074334, 0.0008595547135404881, 0.9999852260198713, -0.017739927010989854]
+    - [0.0, 0.0, 0.0, 1.0]
+  T_cn_cnm1:
+    - [0.9999978238338454, 0.00032423014342537217, 0.0020608741811117422, -0.09397410073693341]
+    - [-0.00032135073657469737, 0.9999989720658036, -0.0013973550168239824, -4.36282992379309e-05]
+    - [-0.002061325127286215, 0.0013966897125111987, 0.99999690009348, 0.0010315706607051294]
+    - [0.0, 0.0, 0.0, 1.0]
+  cam_overlaps: [0]
+  camera_model: pinhole
+  distortion_coeffs: [0.00415520544025779, -0.003951065114859482, -0.00042589527047545586, -0.00442085250772721]
+  distortion_model: radtan
+  intrinsics: [319.69438067702725, 320.1948319604044, 313.46161305104766, 180.7178241492703]
+  resolution: [640, 360]
+  rostopic: /camera/infra2/image_rect_raw
+  timeshift_cam_imu: 0.005012709485041032
+
+# cam0:
+#   T_cam_imu:
+#     - [0.9999654398038452, 0.007342326779113337, -0.003899927610975742, -0.027534314618518095]
+#     - [-0.0073452195116216765, 0.9999727585590525, -0.0007279355223411334, -0.0030587146933711722]
+#     - [0.0038944766308488753, 0.0007565561891287445, 0.9999921303062861, -0.023605118842939803]
+#     - [0.0, 0.0, 0.0, 1.0]
+#   cam_overlaps: []
+#   camera_model: pinhole
+#   distortion_coeffs: [-0.045761895748285604, 0.03423951132164367, -0.00040139057556727315, 0.000431371425853453]
+#   distortion_model: radtan
+#   intrinsics: [416.85223429743274, 414.92069080087543, 421.02459311003213, 237.76180565241077]
+#   resolution: [848, 480]
+#   rostopic: /d455/color/image_raw
+#   timeshift_cam_imu: 0.002524377913673846