Skip to content

[fix] Integrate IMU measurement AFTER its previous lidar state

Tong Fu requested to merge fix/FixTimeForIMUIntegrateMeasurement into master

The integration of IMU measurement starts from the measurement corresponding to the one just after previous lidar state timestamp.

The GTSAM function PreintegratedImuMeasurements::integrateMeasurement check whether dt<=0. If dt<=0, it aborts the current process.

dt represents measTime-preLidarTime in this case

So we should avoid using the measurement which its timestamp is equal to lidar state timestamp (which causes dt=0).

An example of aborted case

itRawMeasStartUpdate->Time = 1659364917.820329904556

timeIcurr = 1659364917.820329904556

terminate called after throwing an instance of 'std::runtime_error'

  what():  PreintegratedImuMeasurements::integrateMeasurement: dt <=0
Edited by Julia Sanchez

Merge request reports