Realsense-ros: Is RGB data has sync with IMU topic in D435i?

[gajena]

I am testing IMU sync with RGB image for VIO application, and I have done calibration using Kalibr, and it shows time difference around 30 ms and ROVIO is diverging fast.
I have also checked timestamp data it shows a delay of around 1 sec between timestamps of IMU and Camera topics (both RGB and depth).
I am also enabling param hold_back_imu_for_frames and tested with both methods for imu copy and linear_interpolation.

[gajena]

I was testing that which camera is global shutter and found that RGB is rolling shutter and infrared camera is global shutter.
Calibrated using Kalibr and found that with Infrared frames, IMU has delay around only 5ms (that is good for ROVIO) .
Video can be find here:
https://drive.google.com/open?id=1dnXHpL3JaceyRbHA8mvL8_ZBExDUs5_7

[doronhi]

Hi @gajena,
There seems to be a delay between the infra, color and IMU, according to some preliminary tests and it seems, at least right now, to be a firmware error. However, I did see a delay both in RGB and IR sensors.
I'll keep this issue posted as this unravel.
About the ROVIO's divergence, is it possible to give a reference to your launch files and installation procedures, so it'll be easier to check the phenomena in a use-case scenario?

[gajena]

Hi! @doronhi,
I have done calibration using Kalibr using launch file from here. After this, I have updated ROVIO's launch files here.

[doronhi]

Thanks! Seriously starting to check on the ROVIO might take me even a couple of weeks, I’m afraid, but I updates regarding delays I hope will be available much sooner.

[andre-nguyen]

@gajena Can you share your yaml configuration for the calibration?

[gajena]

@andre-nguyen, Are you asking about files required by Kalibr or the result produced by Kalibr?

[andre-nguyen]

The files required for it. I'm interested in knowing the noise values you used for the cam-imu calibration. I suspect we can also put the distortion coefficients to 0 since the Realsense outputs pre rectified images.

[gajena]

I used the traditional approach

• Collected Infrared cam data with IR disable
• Obtained cam param
  I was not able to find a noise model for the IMU, so I used a noise model of ADIS IMU(it's not a good way to do it, but somehow it worked!!)
• After that just ran the Kalibr for infra image and IMU data.

By the way, have you tried with zero distortion coefficients?

[andre-nguyen]

Here is my calibration result. As you can see my reprojection errors are decent. Key points:

• I did not zero out the distortion coefficients. But as you can see they are estimated to be very small which is good.
• For the noise values I used the ones someone else used for the ZR300 which has the same BMI055 IMU.
• I didn't have a big april grid with me so I used a small one printed on normal A4 paper.
• I ran everything at a "low" resolution of 640x480. Which is closer to the resolution of the machine vision cameras the Rovio authors used.

I noticed that the initial cam-imu time offset was initialized somewhere to 4ms but after optimization it is determined to be 0.1 ms which is excellent. Having to turn off IR using the realsense viewer is annoying though. I tried rovio and the result seemed OK (no divergence) but I was at my desk as I didn't have a mobile setup.

Conclusion: IMU to infrared cams timestamping should be good. There's still a few open questions on how to use D435i for VIO such as the best IMU data merging method, use of distortion coefficients

Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 0.173943353672, median 0.147643722052, std: 0.119621573992
Reprojection error (cam1):     mean 0.16247533893, median 0.138554530845, std: 0.10923907102
Gyroscope error (imu0):        mean 0.32260865406, median 0.252569401111, std: 0.233390189106
Accelerometer error (imu0):    mean 0.778813662592, median 0.641485282384, std: 0.72379960621

Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 0.173943353672, median 0.147643722052, std: 0.119621573992
Reprojection error (cam1) [px]:     mean 0.16247533893, median 0.138554530845, std: 0.10923907102
Gyroscope error (imu0) [rad/s]:     mean 0.0134788896442, median 0.0105525845083, std: 0.00975125919095
Accelerometer error (imu0) [m/s^2]: mean 0.0324786074339, median 0.0267516476173, std: 0.0301843745173

Transformation (cam0):
-----------------------
T_ci:  (imu0 to cam0): 
[[ 0.0110382  -0.99991198  0.00736134  0.00624933]
 [-0.00558312 -0.00742331 -0.99995686 -0.00097011]
 [ 0.99992349  0.01099662 -0.00566457 -0.01621383]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam0 to imu0): 
[[ 0.0110382  -0.00558312  0.99992349  0.01613819]
 [-0.99991198 -0.00742331  0.01099662  0.00641987]
 [ 0.00736134 -0.99995686 -0.00566457 -0.00110792]
 [ 0.          0.          0.          1.        ]]

timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
-9.29860761026e-05


Transformation (cam1):
-----------------------
T_ci:  (imu0 to cam1): 
[[ 0.00805359 -0.99993973  0.00746128 -0.04341998]
 [-0.00547116 -0.00750548 -0.99995687 -0.00099582]
 [ 0.9999526   0.00801242 -0.00553128 -0.01652654]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam1 to imu0): 
[[ 0.00805359 -0.00547116  0.9999526   0.01686999]
 [-0.99993973 -0.00750548  0.00801242 -0.04329242]
 [ 0.00746128 -0.99995687 -0.00553128 -0.00076322]
 [ 0.          0.          0.          1.        ]]

timeshift cam1 to imu0: [s] (t_imu = t_cam + shift)
-0.00011329653378

Baselines:
----------
Baseline (cam0 to cam1): 
[[ 0.99999554 -0.00008307 -0.00298525 -0.04971776]
 [ 0.0000834   0.99999999  0.00011104 -0.00002442]
 [ 0.00298524 -0.00011129  0.99999554 -0.00033155]
 [ 0.          0.          0.          1.        ]]
baseline norm:  0.0497188743763 [m]


Gravity vector in target coords: [m/s^2]
[-0.0534996  -8.66839068 -4.58525503]


Calibration configuration
=========================

cam0
-----
  Camera model: pinhole
  Focal length: [373.6504302988745, 375.75763034879947]
  Principal point: [320.4363427220866, 239.39529301144844]
  Distortion model: radtan
  Distortion coefficients: [-0.004020373096063456, -0.005547787897453452, 0.0003561157710439325, 0.0003653801898754835]
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.0235 [m]
    Spacing 0.00705 [m]


cam1
-----
  Camera model: pinhole
  Focal length: [373.40636529301906, 375.49353064695407]
  Principal point: [321.520615553491, 239.36084632157363]
  Distortion model: radtan
  Distortion coefficients: [-0.0024021276332198294, -0.0078016361176795074, 0.0002687660056814919, 0.0004465628578118777]
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.0235 [m]
    Spacing 0.00705 [m]



IMU configuration
=================

IMU0:
----------------------------
  Model: calibrated
  Update rate: 500.0
  Accelerometer:
    Noise density: 0.001865 
    Noise density (discrete): 0.0417026677804 
    Random walk: 0.0002
  Gyroscope:
    Noise density: 0.0018685
    Noise density (discrete): 0.0417809301596 
    Random walk: 4e-06
  T_i_b
    [[ 1.  0.  0.  0.]
     [ 0.  1.  0.  0.]
     [ 0.  0.  1.  0.]
     [ 0.  0.  0.  1.]]
  time offset with respect to IMU0: 0.0 [s]

[gajena]

Hey! @andre-nguyen is your configuration performing consistently with VIO application?
Mine one is sometimes giving the problem in the initialization of IMU, So now I am trying HW sync with GPIO pins!

[andre-nguyen]

Unfortunately I'm not working full time with the RealSense, so I have little hands on time. I've had a case where Rovio diverged but I can't say much more than that. I didn't have initialization problems but I haven't worked enough with the camera to say for sure I won't run into that problem in the future.

[SteveMacenski]

Is this using the united IMU data? I haven't been able to get rovio working with the un-united IMU outputs (the separated gyro and accel samples). For some reason my 435i's don't publish out on the camera/imu channel when united is enabled. With just running on the gyro samples, rovio falls through the floor fairly quickly.

[andre-nguyen]

Yes I think it was the united but it's been a long time now.

[gajena]

@andre-nguyen This will help in the modeling of IMU noise model: https://github.com/gaowenliang/imu_utils

[doronhi]

@SteveMacenski , can you publish the log file when camera/Imu is not published? Maybe under a new issue?

[SteveMacenski]

@doronhi done in https://github.com/intel-ros/realsense/issues/598 -- I'll follow up there

[RealSenseCustomerSupport]

[Please Ignore - RealSense system comment]

[GITSHOHOKU]

@gajena
hi how can I disable the IR do you mean depth param?
Can d435i get both infra image and calibrate them with rosnode

[gajena]

@GITSHOHOKU
Hi,
I can not able to understand, what are you asking!?
I think you are asking about IR projector, you can disable it by using param depth emitter in dynamic reconfigure.

[GITSHOHOKU]

@ gajena
That's what I mean,It worked,Thank you！

[0Jiahao]

Hi @gajena! I am currently using the same setup as yours (d435i + ROVIO), it works well when motion is slow but will drift when the motion is aggressive. Is yours the same? And would it be possible to share your rovio.info with me?
Kind regards,

[gajena]

I tried this thing and had good results for two or three test-runs, after that It was inconsistent in time-sync.
So I have moved to a new setup: Monocular camera with PX4 IMU trigger.
Or
Try approach for hwsync given in #586.
Config are given here

[GITSHOHOKU]

@andre-nguyen
I use Kalibr on D435i and the result shows the imu frame a little bit outside the camera model.
Accroding to using imu_interpolation can this happened usually?

[andre-nguyen]

Your calibration dataset is probably just bad. Try again and make sure you excite all axes. My guess is that both linear interpolation and copy should work.

Personally I'm now using the camera with an external IMU so I don't really know what is up with the internal one.

[GITSHOHOKU]

@andre-nguyen
Thanks! I will try it again with new dataset

[RealSenseCustomerSupport]

---

@gajena Any other questions about this ticket? Looking forward to your update. Thanks!

[MoBaT]

Hey @andre-nguyen, Can you post your BAG file of your calibration routine? I was able to replicate a similar intrinsics file but my extrinsics were not great at all.

Another question, did you have trouble getting the intrinsics? What were your parameters you passed into the kalibr_calibrate_cameras python script?

[andre-nguyen]

Sorry I no longer have that data.

did you have trouble getting the intrinsics

No, that is the easy part.

What were your parameters you passed into the kalibr_calibrate_cameras python script?

I used the radial tangential distortion model. For the rest I manually measured the squared on my april grid.

[MoBaT]

@andre-nguyen Alright, thanks for the info. There's something wrong with what I'm doing. My errors look far too big. Do you have any pointers on what I may be doing wrong? I'm pulling the data straight from the SDK and lerping the accel and gyro data together into one message.

Calibration results
===================
Normalized Residuals
----------------------------
Reprojection error (cam0):     mean 70.5714838714, median 56.5866966872, std: 51.7444852315
Reprojection error (cam1):     mean 70.0255735319, median 56.5559415449, std: 50.5282309034
Gyroscope error (imu0):        mean 60.121572858, median 54.7564204545, std: 32.9379516753
Accelerometer error (imu0):    mean 69.3391841036, median 44.1101192421, std: 61.6188212839

Residuals
----------------------------
Reprojection error (cam0) [px]:     mean 70.5714838714, median 56.5866966872, std: 51.7444852315
Reprojection error (cam1) [px]:     mean 70.0255735319, median 56.5559415449, std: 50.5282309034
Gyroscope error (imu0) [rad/s]:     mean 2.50722997933, median 2.28348881106, std: 1.37360045608
Accelerometer error (imu0) [m/s^2]: mean 2.89162895884, median 1.83950964851, std: 2.56966923302

Transformation (cam0):
-----------------------
T_ci:  (imu0 to cam0): 
[[ 0.84165441 -0.04340753  0.53826912 -2.5437008 ]
 [-0.04312531  0.98817785  0.14712152  0.48581055]
 [-0.5382918  -0.1470385   0.82983228 -5.08874476]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam0 to imu0): 
[[ 0.84165441 -0.04312531 -0.5382918  -0.57736186]
 [-0.04340753  0.98817785 -0.1470385  -1.3387244 ]
 [ 0.53826912  0.14712152  0.82983228  5.52052707]
 [ 0.          0.          0.          1.        ]]

timeshift cam0 to imu0: [s] (t_imu = t_cam + shift)
-0.011101760183208957


Transformation (cam1):
-----------------------
T_ci:  (imu0 to cam1): 
[[ 0.84173661 -0.04371255  0.53811587 -2.59355357]
 [-0.04288884  0.98815288  0.1473582   0.48458343]
 [-0.53818215 -0.14711596  0.82988968 -5.08908858]
 [ 0.          0.          0.          1.        ]]

T_ic:  (cam1 to imu0): 
[[ 0.84173661 -0.04288884 -0.53818215 -0.5349844 ]
 [-0.04371255  0.98815288 -0.14711596 -1.34089949]
 [ 0.53811587  0.1473582   0.82988968  5.54760705]
 [ 0.          0.          0.          1.        ]]

timeshift cam1 to imu0: [s] (t_imu = t_cam + shift)
-0.011140242513692195

Baselines:
----------
Baseline (cam0 to cam1): 
[[ 0.99999994 -0.0003275  -0.00012657 -0.05033791]
 [ 0.00032751  0.99999994  0.00007278 -0.00002362]
 [ 0.00012655 -0.00007282  0.99999999  0.0000134 ]
 [ 0.          0.          0.          1.        ]]
baseline norm:  0.050337919270404825 [m]


Gravity vector in target coords: [m/s^2]
[-0.31738785 -3.49002701 -9.15900646]


Calibration configuration
=========================

cam0
-----
  Camera model: pinhole
  Focal length: [382.50863246682906, 383.1081527073867]
  Principal point: [319.3868289751252, 240.75497655494152]
  Distortion model: radtan
  Distortion coefficients: [-0.0003035746446133223, -0.0037745189818182046, 0.0006963241011780466, 0.000728908616387402]
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.048 [m]
    Spacing 0.0144 [m]


cam1
-----
  Camera model: pinhole
  Focal length: [382.6717496306011, 383.2565172725056]
  Principal point: [319.51637471594046, 240.75321517420156]
  Distortion model: radtan
  Distortion coefficients: [-0.0010694866969111472, -0.0024761388220480244, 0.0008121390400336279, 0.00030537253728065154]
  Type: aprilgrid
  Tags: 
    Rows: 6
    Cols: 6
    Size: 0.048 [m]
    Spacing 0.0144 [m]



IMU configuration
=================

IMU0:
----------------------------
  Model: calibrated
  Update rate: 500.0
  Accelerometer:
    Noise density: 0.001865 
    Noise density (discrete): 0.0417026677804 
    Random walk: 0.002
  Gyroscope:
    Noise density: 0.001865
    Noise density (discrete): 0.0417026677804 
    Random walk: 4e-06
  T_i_b
    [[1. 0. 0. 0.]
     [0. 1. 0. 0.]
     [0. 0. 1. 0.]
     [0. 0. 0. 1.]]
  time offset with respect to IMU0: 0.0 [s ]

[andre-nguyen]

Yes 70 pixel reprojection error means the whole thing is rubbish. You should at the very least get subpixel reprojection (less than 1) if not less than 0.2.

I would advise to post your bag file and all the configs you used. I remember I had a lot of bad calibrations because I would use data with a certain camera resolution and my configs would be in another resolution.

[MoBaT]

@andre-nguyen I tared up my configurations, outputs, bag file, etc. I also have a COMMANDS file which has the parameters I ran for the camera calibration and imu_camera calibration. https://www.dropbox.com/s/xj83tk7p90u6d2v/output12.tar.gz?dl=0

What do you mean by you would "use data with a certain camera resolution and my configs would be in another resolution"?

[andre-nguyen]

Sorry but your rosbag is broken

$  rosbag info output.bag 
path:        output.bag
version:     2.0
duration:    0.0s
start:       Jan 18 1970 21:25:04.78 (1563904.78)
end:         Jan 18 1970 21:25:04.80 (1563904.80)
size:        832.9 MB
messages:    12300
compression: none [948/948 chunks]
types:       sensor_msgs/Image [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/Imu   [6a62c6daae103f4ff57a132d6f95cec2]
topics:      /cam0/image_raw   1415 msgs    : sensor_msgs/Image
             /cam1/image_raw   1415 msgs    : sensor_msgs/Image
             /imu0             9470 msgs    : sensor_msgs/Imu

Also you seem to be using the same bag for intrinsic and extrinsic calibration. It is clear to me you should go through the kalibr tutorial more carefully. They even give the commands to run. Also you shouldn't be turning on approx-sync.

I also suggest you download the Euroc calibration datasets to get an idea of the movement you are supposed to do to get things working.

Edit: I managed to open your bag with rqt instead of trying to play it. You aren't exciting the system enough. Try 3 back and forth movements on all 6 degrees of freedom and finish with a figure 8 + angular motion to excite all 6 DoF at the same time. Also make two datasets 1 for intrinsics and 1 for extrinsics as explained in the tutorial.