[feat][refact] Add interpolation models choice
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.