Realsense-ros: IMU samples using unite_imu_method are not periodic

[araujokth]

Hi

I was trying to captured the IMU data in a single message by using the unite_imu_method:=linear_interpolation method and I was expecting that the messages would be periodic with a period of 2 ms (500 Hz). However, I noticed that they are being published with an average period of 2 ms but the inter-sampling period oscillates between [0,4] ms. Here is a picture of the inter-sampling time

I was wondering if this is the expected behavior and I have misunderstood that the measurements should come out with a 2 ms period?

I checked what is the period of the gyro and accelerometer messages and they have very low jitter and are published with a 3.97 ms and 2.5 ms period, respectively, with the gyro sometimes missing a measurement. So the problem is only on the unite_imu_method and not on the individual accel and gyro measurements.

I experienced this behavior with the latest SDK 2.25, using Ubuntu 18.04, kernel 4.15.0-58-generic and also with building SDK 2.23, 2.22 and 2.21 from source. I have firmware 5.11.11 and realsense2_camera 2.2.8.

Thanks for the help!

[araujokth]

Anyone?

[doronhi]

please check my answer at #729 .
Does that explain the results you get?

[araujokth]

From what I understand from your message is that the method does not try to provide measurements at a fixed rate or did I misunderstand your reply? However, in order to make use of the IMU measurements for doing SLAM having a fixed rate is very important. For example, one could interpolate the measurements taking the sampling frequency of the accelerometer or the gyroscope, or interpolate the measurements at another base time. Is this something that you guys are considering for a future release or we should not expect it?

[doronhi]

I don't remember SLAM algorithm to be dependent on a fixed rate of inputs. In any case, this is not something that is on the table right now.
I guess that for maximum accuracy you should subscribe for both gyro and accel topics separately, put them directly to your kalman filter and avoid any kind of interpolations and delays caused by the uniting algorithms.
It could be, however, that my assumptions here are wrong.
If you find out that by using your own uniting method, one that interpolate the gyro and accel at a given rate, you get better results from the SLAM algorithm then by using the current linear-interpolation method, I'll be more then happy to discuss and merge that option to the wrapper here.

[araujokth]

One of the reasons for most of the SLAM algorithms requiring a fixed rate is because the model of the IMU is obtained for that fixed rate and then used to compute the motion, so if you change the time interval between measurements you have to change the model.

In this repository https://github.com/chutsu/rs4se, @chutsu has implemented the linear interpolation method with a fixed rate for the realsense, and he also changed how the frame timestamp is performed to be done at mid-exposure time which is also recommended for SLAM, see https://github.com/chutsu/rs4se/issues/1#issuecomment-515069714. However, its unclear if @chutsu would be interested in merging his implementation with this one.

I will probably look into implementing my own interpolation function with a fixed rate for this repo. I will get back to you when I have an update. In case you guys prioritize this in the meantime please do let me know.

Thanks again!

[chutsu]

Can confirm, most state of the art state-estimation algorithms (SLAM / VIO) such as ROVIO, MSCKF, OKVIS, VINS-MONO that fuse IMU data assume both the gyro and the accel are "synchronized" (or lerped) at a fixed rate, it just makes the overall maths and book keeping simpler.

Additionally, most if not all of the state of the art algorithms above assume the imu-camera delay is constant over time. Even the most popular robotics camera-imu calibration toolbox Kalibr assumes that. Its not desirable to estimate a variable time delay in the state vector, it just increases the complexity of the algorithm AFAIK.

However, its unclear if @chutsu would be interested in merging his implementation with this one.

Not at this point. But feel free to copy it.

[doronhi]

Added #1010 to create a fixed frame rate using the "unite_imu_method:=copy" option.
Regarding the nice interpolation solution suggested by @chutsu , I can see why it may be better then the current linear interpolation implementation and I would like to adopt it if it's alright with you @chutsu .
I have only this concern: Although The messages keep their correct timestamps, they come out in bursts after every Accel message. Are the SLAM algorithms you use (@araujokth ) OK with the fact that the messages are not transmitted in equal intervals?
Can you suggest a launch file to demonstrate that? Something along the lines of opensource_tracking.launch maybe?

[chutsu]

I would like to adopt it if it's alright with you @chutsu

@dorodnic Feel free to copy the code. :+1:

The messages keep their correct timestamps, they come out in bursts after every Accel message.

This shouldn't be a problem with most VIO algorithms I have come across, since inorder to do a prediction step in an EKF or IMU factor in a non-linear optimization SLAM algo, they assume the gyro and accel measurements are aligned with each other. In short the algos have to wait for both gyro and accel (if they use IMU) in anycase, so the bursty behaviour shouldn't pose a problem.

[araujokth]

Added #1010 to create a fixed frame rate using the "unite_imu_method:=copy" option.
Regarding the nice interpolation solution suggested by @chutsu , I can see why it may be better then the current linear interpolation implementation and I would like to adopt it if it's alright with you @chutsu .

This is great news @doronhi . Thanks!

I have only this concern: Although The messages keep their correct timestamps, they come out in bursts after every Accel message. Are the SLAM algorithms you use (@araujokth ) OK with the fact that the messages are not transmitted in equal intervals?

In the majority of the cases people create datasets out of this data in order to run their experiments, so the burstiness will not affect this since the timestamps are correct inside the rosbag.

If trying to run this online, then one could add a fixed delay to wait for IMU measurements, which should not be longer than one or two acceleratometer samples? Then these measurements will still be fed into the SLAM framework in a periodic manner.

Can you suggest a launch file to demonstrate that? Something along the lines of opensource_tracking.launch maybe?

Sure, I think it would be great to try this out and see the difference in performance between having periodic vs aperiodic IMU measurements.

[araujokth]

Hi @doronhi, was wondering if you had any update regarding the implementation of the linear_interpolation? It would be really valuable as the copy method lowers the performance since even though the IMU measurements are now periodic, their values are not the best they could be for the corresponding time instance. Thanks again for the help!

[doronhi]

The linear_interpolation method was re-implemented. Would you like to suggest a different algorithm then the current one?
If so, I think we should close this issue and start a new one with the new suggestion.

[RealSenseCustomerSupport]

---

Hi @doronhi, If we don't hear from you in 7 days. this issue will be closed.

[araujokth]

@doronhi sorry for the late reply and sorry for the misunderstanding. In my original post, the intention was that any "unite_imu_method" should make sure that the IMU measurements are periodic, and not aperiodic as they are now. Hence, for linear_interpolation to be useful, it should output the measurements periodically. Was this fixed in its re-implementation? I have tried it and was still aperiodic, but I could have done something wrong.

[doronhi]

As far as I know and checked it is periodic in the last version. I would appreciate if you could check report back if the problem persists.

[araujokth]

Sorry, only had time to verify this now. I did the test with the copy method, and it is always periodic, but the linear_interpolation method has several samples which are not. I gathered data over different periods and the behavior is the same. The IMU period was set to 400 Hz, so we should get the intersampling time of 2.5 ms, but sometimes it is 5 ms. Here is a plot to show what happens:

@doronhi could you check if this is the same that happens to you?

[doronhi]

Sorry for the late reply.
Time difference of 5ms instead of 2.5ms suggests a frame drop.
I checked with my NUC and found 1 case at the beginning of a 2 minutes test.
I'll have to check the spec what is the allowed rate, if any at all, of frame drops in IMU.
How much was your computer overloaded at the time of the test?

[araujokth]

No problem! It was not overloaded, the only thing running was the ros application to get the data from the realsense. I tried several times to run with unite_imu_method:=copy and there were never any frame drops and the period was always 2.5 ms. However, whenever I tried with unite_imu_method:=linear_interpolation there were always the sporadic drops as you see above.

[doronhi]

That is interesting indeed. I'll look into it.

[RealSenseSupport]

@doronhi Any finding from your side?