Skip to content

[feat][refact] Add interpolation models choice

Arthur Bourbousson requested to merge feat/BSplineInterpolation into master

Current SLAM uses a constant linear velocity model for every part that needs interpolation. This merge aims at adapting the SLAM to use other models that can lead to more realistic interpolations.

First, a class for interpolation models is created. Models are separated into Translation interpolation and Rotation interpolation, a trajectory model combines one translation and one rotation to cover a full 6D pose model of interpolation. Idea from here

Interpolation Models are :

  • Linear (current model)
  • Quadratic Spline
  • Cubic Spline

Those interpolation models replace the current one in the SLAM and can be chosen with the parameter InterpoModel. Wrappers for ROS and LidarView have been added.

Notes :

  • Extrapolation has been tested and is not usable with the current spline models. So extrapolation is kept linear. But later an extrapolation model can be found and applied exactly as interpolation ones.

  • The changes of this MR reduce the drift, it is normal that it doesn't pass the ROS tests, results are not equivalent. But MR has been tested on LidarView and ROS, new results are still valid.

  • Interpolation modes have not been included in Ceres constraint based on interpolation (two poses optimization).

Timings : (evaluated on 2022-02-22_13-28-21.pcap from 0 to 400 frames)

Test time (ms)
master 34
MR linear 34
MR quadratic 38
MR cubic 40
master + ext poses 295
MR linear + ext poses 284
MR quadratic + ext poses 293
MR cubic + ext poses 312

See details here.

Cubic interpolation seems to add instability to the trajectory with big oscillations.

Edited by Julia Sanchez

Merge request reports