NavState and SE_2(3)

[dellaert]

Feature

We want NavState, a manifold, to also get Lie group operations so it becomes SE2(3).

Note: NavState was a perfectly fine manifold, which sometimes is all you need. We could add a class SE_2(3), but we will follow the current GTSAM convention and add the group operations in the same class.

Motivation

Many new VIO filters and smoothers now use SE2(3) for improved consistency.

TODO:

Each of these should be a separate PR.

• Basic Group operations, done in Endow NavState with group operations #1930
• Add Functor to streamline Jacobians, done in Left and Right SO(3) Jacobians #1932
• Simplify Computation of Q blocks, done in Better SE(3) and SE2(3) Jacobians #1938
• Investigate nearZero behavior and selection of thresholds - chose an ok threshold
• Fix coriolis terms according to Brossard22tro
• transpose group operations, unit tested
• Switch retract and localCoordinates to exp/log. If we remove the custom retract and localCoordinates, lots of unit tests fail, so it is important to check this too.
• Use in "Brossard's code"

[dellaert]

FYI @varunagrawal @ProfFan @stefangachter
Let me still play with this for a bit, no action need from y'all except review PRs.