Drifting odometry in ign gazebo simulation (subt simulation)

[cjpurackal]

Thanks for the project! I'm facing some trouble running it. I'm getting very serious drifts in odometry when running on the subt simulator(ignition gazebo).

https://user-images.githubusercontent.com/12036454/179759456-e7f908fb-fdaa-4122-89d7-2a0b7ea85c90.mp4

Here's how my configs look like: estimator_config:

%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)
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)

use_stereo: false # 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: 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

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.81 # 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: 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: 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]

# ==================================================================
# ==================================================================

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
num_pts: 200
fast_threshold: 20
grid_x: 20
grid_y: 20
min_px_dist: 15
knn_ratio: 0.70
track_frequency: 21.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

# 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: "subt_imu_chain.yaml"
relative_config_imucam: "subt_imucam_chain.yaml"

imu_chain.yaml:

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: 1.0e-2  # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
  accelerometer_random_walk: 1.0e-3    # [ m / s^3 / sqrt(Hz) ].  ( accel bias diffusion )
  gyroscope_noise_density:  2.0e-4     # [ rad / s / sqrt(Hz) ]   ( gyro "white noise" )
  gyroscope_random_walk: 8.0e-7       # [ rad / s^2 / sqrt(Hz) ] ( gyro bias diffusion )
  model: calibrated
  rostopic: /X1/imu/data
  time_offset: 0.0
  update_rate: 50.0

imu_cam_yaml:

%YAML:1.0

cam0:
  T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
    - [1, 0, 0, 0.438]
    - [0, 1, 0, 0.0]
    - [0, 0, 1, 0.272]
    - [0.0, 0.0, 0.0, 1.0]
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [0, 0, 0, 0]
  distortion_model: plumb_bob
  intrinsics: [277.1, 277.1, 160.5, 120.5] #fu, fv, cu, cv
  resolution: [320, 240]
  rostopic: /X1/front/image_raw

I have taken the noise values from the imu from the following sdf file.

        <imu>
          <angular_velocity>
            <x>
              <noise type="gaussian">
                <mean>0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
                <dynamic_bias_stddev>0.0000004</dynamic_bias_stddev>
                <dynamic_bias_correlation_time>1000.0</dynamic_bias_correlation_time>
              </noise>
            </x>
            <y>
              <noise type="gaussian">
                <mean>0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
                <dynamic_bias_stddev>0.0000004</dynamic_bias_stddev>
                <dynamic_bias_correlation_time>1000.0</dynamic_bias_correlation_time>
              </noise>
            </y>
            <z>
              <noise type="gaussian">
                <mean>0</mean>
                <stddev>2e-4</stddev>
                <bias_mean>0.0000075</bias_mean>
                <bias_stddev>0.0000008</bias_stddev>
                <dynamic_bias_stddev>0.0000004</dynamic_bias_stddev>
                <dynamic_bias_correlation_time>1000.0</dynamic_bias_correlation_time>
              </noise>
            </z>
          </angular_velocity>
          <linear_acceleration>
            <x>
              <noise type="gaussian">
                <mean>0</mean>
                <stddev>1e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
                <dynamic_bias_stddev>0.002</dynamic_bias_stddev>
                <dynamic_bias_correlation_time>300.0</dynamic_bias_correlation_time>
              </noise>
            </x>
            <y>
              <noise type="gaussian">
                <mean>0</mean>
                <stddev>1e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
                <dynamic_bias_stddev>0.002</dynamic_bias_stddev>
                <dynamic_bias_correlation_time>300.0</dynamic_bias_correlation_time>
              </noise>
            </y>
            <z>
              <noise type="gaussian">
                <mean>0</mean>
                <stddev>1e-2</stddev>
                <bias_mean>0.1</bias_mean>
                <bias_stddev>0.001</bias_stddev>
                <dynamic_bias_stddev>0.002</dynamic_bias_stddev>
                <dynamic_bias_correlation_time>300.0</dynamic_bias_correlation_time>
              </noise>
            </z>
          </linear_acceleration>
        </imu>

I have taken the camera to imu transform by echoing it from tf. I've also tried running the system when the robot was already moving. What could possible the issue here?

[goldbattle]

Likely this is either the camera-IMU transformation or the initialization is bad (look at the imu thresh and zvupt configs). I would also make sure your IMU frequency is >100hz.