Testing on rosbag

[cmakelabs]

Thank you for sharing ugpm, I wanted to test it out on data that I have recorded from my xsens IMU, I have written a ROS wrapper for ugpm, and it worked, however when I run it on my data, I got this warning followed by an error and termination:

Preintegration demonstration with UGPM4   Duration: 0.412694 / 293.778134               
WARNING: Accelerometer or gyroscope data is not sampled at a constant frequency         
 [RUNNINMin \ max delta time for acc 1e+10 \ -1e+10 0.675815 / 293.778134               
	Min \ max delta time for gyr 1e+10 \ -1e+10
WARNING: There might be an issue with the IMU data timestamps. This is not handled in the current version (can lead to undefined behavior or alter the performance of the preintegration).
terminate called after throwing an instance of 'std::invalid_argument'
  what():  The argument of ImuData::Get are not consistent82 / 293.778134  

I would really appreciate it, if you could guide me to get a workaround this issue, as we are really excited about ugpm and would really want to try it out on our data.

[clegenti]

Hi @cmakelabs ,
Thanks for the interest in our work :)

There seem to be at least two issues in the printouts but both are likely originating from the same major issue.

One is that the timestamps of the data are not at a constant frequency. It can happen with non-synchronised data collection. I would advise making sure the timestamps are looking good and that the samples in the imu data structure are sorted by time. Also, be careful with the fact that time is stored as a double: you might need to offset the timestamps so as not to lose accuracy due to the numerical precision of the double.
The error is likely coming from erroneous timestamps.
Regarding the non-constant frequency nature of the ROS-provided timestamps on non-real-time OS to remove the warning, if you are processing data batches you can pre-process the timestamps to be linearly distributed between the first and last timestamps (not the best but in most cases that does the job :) )

[cmakelabs]

Thank you very much for your answer,

1. It is true that my data are not synchronized between sensors, but since I am running only on the the IMU topic why synchronization should be an issue, could you please tell If I understood you right?

2. I agree that the error is coming from the timestamps, but I have just recorded the rosbag as usual, not sure whether ROS uses double as default for timestamps, but as far as I know it is just one number in milliseconds. Could you please tell, why would the time needs to be stored in double from the design point of view, and how can I make everything fit together now?

3. Do I understand correct that you suggest me to preprocess timestamps manually, and override the deafult one, by creating a linear interpolation from the first timestamp to the last timestamp with a constant step?

[clegenti]

1. The synchronisation is not only an inter-sensor problem but also a problem between the computer clock and the IMU clock. In such a case a 100Hz IMU might seem to provide data at non-regular deltas of time (typically varying between 3ms and 20ms instead of the expected 10ms). This phenomenon associated with a too-low noise parameter can hinder the performance of the GP (it is less problematic with the LPMs). To hack this you can linearise the timestamps (aka, giving the same delta t between each measurement while keeping the first and last timestamps unchanged) based on the assumption that the IMU clock period is close to constant during data collection.

2. ROS messages have seconds and nanoseconds counters on unsigned integers generally based on the epoch time of the machine. The problem with converting that to a double (seconds) directly is that you can lose accuracy due to the number of significant digits possible in a double. If we are dealing with ms it might be fine but lower accuracy can be lost. The easy way is to subtract the first timestamp of your dataset from every sample in the ROS format before converting to double.

3. C.f. answer to 1. If you cannot get accurate regular timestamps from your hardware and are processing the dataset offline, it might be better to "override" the default timestamps to provide a constant period.

[cmakelabs]

Thank you very much, it is clear now. I would like to ask, how can I try it out online(without recorded ROS bag, but directly getting measurements from the IMU and preintegrate them)? From your experience how a proper sync between CPU clock and IMU can be achieved in that case?

[clegenti]

The fact that the code crashes when the timestamps are not well-spaced is a safeguard to bring awareness to the user that the data is not optimal (thus preventing unfair benchmarking of the algorithm, and/or catching other "bugs" in the interfacing as it is the case here). If the motion of your system is not crazily dynamic it will likely be fine even with the fluctuation of timestamp spacing :). If your timestamps are good enough you can just comment out the error-throwing line of code. But make sure that the timestamps are expressed in seconds and that they are sorted by increasing order (one thing that is worrying in the original printout you posted is the 1e10. This should be in the order of magnitude of the IMU period like 0.0XX)

For the actual synchronisation, you'll likely need hardware synchronisation or an IMU that allows some sort of synchronisation protocol. It will be hardware-dependent and out of the scope of what I can advise here.