Wrong timestamp in state propagation

[pritzvac]

Hi, I think I found a bug in the propagation of IMU measurements.
The problematic code:

open_vins/ov_msckf/src/state/Propagator.cpp

Line 154 in 17b73cf

double time1 = timestamp + cache_t_off;

As far as I understand it, time1 is the stamp into which the state should be propagated, timestamp is the latest IMU stamp (the method is essentially called in the ROS IMU callback) and cache_t_off is the calibrated IMU-camera time offset. But if the time offset is defined as t_imu = t_cam + cache_t_off, i.e., the delay of camera messages, then it shouldn't be added to the IMU timestamps. It causes that the state is propagated much further into future than the latest available IMU measurement, which requires linear extrapolation using

open_vins/ov_msckf/src/state/Propagator.h

Line 154 in 17b73cf

static ov_core::ImuData interpolate_data(const ov_core::ImuData &imu_1, const ov_core::ImuData &imu_2, double timestamp) {

In my case the state is extrapolated into the future by about 0.04 s each time, producing quite jerky behavior, which is mostly visible in the twist values. Sometimes it even causes large outliers because the IMU that I'm using sometimes produces data with incorrect timestamps. See the image - the red data is the original output, the blue data is output when i changed the code to be just double time1 = timestamp;.