Skip to content

Fix ego motion extrapolation when using PGO

Nicolas Cadart requested to merge fix/EgoMotionExtrapolationWithPGO into master

When using Pose Graph Optimization, last Tworld is beeing updated after optimization succeded. However, PreviousTWorld was not updated. When extrapolating motion from PreviousTWorld to TWorld, this lead to very important motion. This resulted in a wrong big jump for next TWorld estimation.

This commit also updates PreviousTWorld after PGO to repair Ego Motion extrapolation.

@nick.laurenson @lea.vauchier PTAL

Edited by Nicolas Cadart

Merge request reports