Fix ego motion extrapolation when using PGO
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.
Edited by Nicolas Cadart