A new IMU manager deriving from Pose manager is created
IMU raw data are preintegrated to get poses that are processed as before
Preintegration is performed with gtsam
The IMU bias is refined with a SLAM pose feed back
Bias refinement is performed with gtsam graph
The graph also allows to get a more accurate start pose for preintegration. Indeed, a small rotation error in the start pose can lead to a bad gravity projection and big acceleration mistakes, so the start pose must be clean.
The optimized poses by the graph are not directly used as SLAM outputs by default but one can call the optimizeGraph function to replace all the SLAM poses by the optimized poses and to recompute the maps.