Skip to content

[fix] Integrate IMU measurement AFTER its previous lidar state

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

Loading