Accel and Gyro data syncronization

[GoosebumpsFactory]

---

Required Info
Camera Model	D435i
Firmware Version	05.10.13.00
Operating System & Version	Ubuntu 18.04.2 LTS
Kernel Version (Linux Only)	4.15.0-48-generic
Platform	PC
SDK Version	2.21.0
Language	C++
Segment	Robot

Issue Description

Many applications and algorithms consume IMU data with both accel and gyro readings for a given timestamp (i.e., they expect synchronized, instantaneous accel & gyro data) . The rosbags captured using D435i have separate rostopics for the accel and gyro data. Both these rostopics have a message type of sensor_msgs/Imu. The accel and gyro messages have different time stamps and the sensor_msgs/Imu messages corresponding to the accel does not have gyro values (and vice versa).

I was gonna solve this issue by writing a utility that combines the accel and gyro rostopics into a single IMU topic by interpolating the gyro readings to align with the accel readings. Before I implemented this utility I figured I would ask if the API already provides a workaround/solution for this issue.

[GoosebumpsFactory]

I wrote a utility to align gyro data with accel data in a bag by finding the two nearest gyro frames for a given accel frame and then interpolating the gyro's angular velocities using bilinear interpolation. The combined sensor_msgs/Imu message is then written back into the bag to a new topic. I have confirmed that these new sensor_msgs/Imu messages work well with algorithms that expect a combined gyro & accel readings in a sensor_msgs/Imu message.

This hack is a bit harder to do in a live stream and has performance implications. Before I go implement this hack as a ROS node may I ask if there is a reason why the API doesn't provide a combined accel+gyro data stream (@dorodnic @doronhi)? Is this a feature you folks are willing to consider adding?

Also, related is the question of timestamps for the IR images and their alignment with the gyro/accel frame timestamps. I just did an experiment where I captured three calibration sequences at different fixed exposures for the IR camera (2ms, 4ms, 8ms). Then I used a calibration algorithm to estimate two properties of the sensor stack:

1. IMU transform
2. IMU timeshift (i.e., the time offset for IMU samples with respect to IR camera samples).

As expected the IMU transform was estimated to near identical values for all three datasets and the values where similar to the ones provided by the device in the topic /device_0/sensor_2/Accel_0/tf/0.

However, the estimated IMU timeshift for these three datasets had the following profile:
offset = 8ms - (camera_exposure / 2)

A good policy for camera timestamps in VIO sensor stacks tends to be trigger time + (camera_exposure / 2), which keeps the timeoffset between camera and IMU constant. This policy is especially important in the auto-exposure case. I could correct for the impact of exposure on camera timestamps in post but may I ask if there is a reason why the API doesn't use this policy? Is this a policy you folks are willing to consider implementing?

Also, besides the camera_exposure's impact on the timeshift, does the constant 8ms offset seem correct given the internal hardware configuration of D435i? I think all the users of your API/product would love not have to worry about these offsets if they can be avoided by the hardware/API.

Thank for your excellent product and service.

[doronhi]

Hi,
I'll refer to what I know and let others fill in the rest.
Regarding the ROS interface, the wrapper provides a form of united IMU message. It is described in the README.md. Search for the word "linear_interpolation". It interpolates sometimes the gyro and sometimes the accel value, according to which one just arrived. Does that option works for you?

[GoosebumpsFactory]

@doronhi thanks for that reference. That answers my questions.

[ethanguo0327]

@doronhi thanks for that reference. That answers my questions.
hi,goosbumpfactory,can you please share your synchronization code?will really appreciate it!

[ruffsl]

A good policy for camera timestamps in VIO sensor stacks tends to be trigger time + (camera_exposure / 2), which keeps the timeoffset between camera and IMU constant. This policy is especially important in the auto-exposure case. I could correct for the impact of exposure on camera timestamps in post but may I ask if there is a reason why the API doesn't use this policy? Is this a policy you folks are willing to consider implementing?

Was this resolved by customizing the frame timestamp domain to either SENSOR_TIMESTAMP or FRAME_TIMESTAMP? Looks like one can only acquire the time from the middle of the exposure. Is this also a frequent convention of VIO sensors?

librealsense/include/librealsense2/hpp/rs_frame.hpp

Lines 450 to 454 in 9f99fa9

	* Chronologically the list of timestamps comprises of:
	* SENSOR_TIMESTAMP - Device clock. For video sensors designates the middle of exposure. Requires metadata support.
	* FRAME_TIMESTAMP - Device clock. Stamped at the beginning of frame readout and transfer. Requires metadata support.
	* BACKEND_TIMESTAMP - Host (EPOCH) clock in Kernel space. Frame transfer from USB Controller to the USB Driver.
	* TIME_OF_ARRIVAL - Host (EPOCH) clock in User space. Frame transfer from the USB Driver to Librealsense.

---

I'm also having some issues where I'm getting metadata for my accelerometer, thus the hardware clock, but not for the gyro, where I'm only getting the system time, even though I believe my config has met metadata support requirements.

https://github.com/IntelRealSense/librealsense/blob/master/doc/frame_metadata.md#metadata-support-for-intel-realsense-devices
Perhaps I should open an new ticket for my gyro issue?