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.