Gazebo Simulation problems with IMU and Camera.

[anastaga]

Hello, I have encountered the "classic" drift in a simulation environment using your code. After some modification, i managed to have it initialize correctly and depending on the way the quadrotor moves, inside the simulation, it might actually have mediocre results. By that, i mean it does not drift instantly but it has a small window of 2-8 seconds that it works as expected. Then it starts drifting and the path deviates hugely from where it should be.

I used the calib_cam_extrinsics,intrinsics and timeoffset on a multiple rosbags I recorded inside Gazebo and took the average values of the IMU R_CtoI and IMU p_CinI matrix. Those i put inside my kalibr_imucam_chain file, as seen below.

kalibr_imucam_chain_cam.yaml

%YAML:1.0

cam0:
  T_imu_cam: #rotation from camera to IMU R_CtoI, position of camera in IMU p_CinI
    - [-0.183,  0.164,  0.969,  0.027]
    - [-0.982, -0.073,  -0.174, 0.013] 
    - [0.042,  -0.984,  0.175, 0.082] 
    - [0.0, 0.0, 0.0, 1.0]
  timeshift_cam_imu: 0.00512
  cam_overlaps: [1]
  camera_model: pinhole
  distortion_coeffs: [0.00379, 0.00910, 0.00443, 0.00674]
  distortion_model: plumb_bob
  intrinsics: [464.154, 461.709, 320, 244.399] #fu, fv, cu, cv
  resolution: [640, 480]
  rostopic: /camera/color/image_raw

I used two different imu plugins, the one that comes with hector_quadrotor and the gazebo one with varied frequencies (ranging from 50-350hz).

Kalibr_imu_chain.yaml

%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: 2.0000e-3  # [ m / s^2 / sqrt(Hz) ]   ( accel "white noise" )
  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: /raw_imu
  time_offset: 0.00512
  update_rate: 200.0

One thing i want to note is, i do not know how to get the T_i_b matrix, so any help with that would be greatly appreciated. Secondly, i tried reducing the noise and random walk from inside gazebo but it didn't really do anything Third, does the mode: calibrated (as seen below the gyroscope_random_walk) do? I searched the code but didn't find anything particular. Can it be changed to, let's say, non-calibrated or such?

Any help will be greatly appreciated!

[goldbattle]

Hi, do you have an example launch file for gazebo and an environment I could test in?

[goldbattle]

Feel free to reopen if you have a bag or launch file to reproduce, thanks!