Add Lie group capabilities to NavState

[varunagrawal]

This PR makes NavState the $SE_2(3)$ Lie Group as defined in Barrau20icra.

There are some things that need to be completed before this PR can land:

• Jacobians for AdjointTranspose.
• Verify computation of retract and localCoordinates. Specifically, need to derive the jacobians and verify they are the same as Barrau20icra.
• If we remove the custom retract and localCoordinates, lots of unit tests fail, so it is important to check this too.

[ProfFan]

As a quick note, unit tests from #1582 should be added and pass

[dellaert]

CI also fails for Cayley variants....

[dellaert]

CI fails. Can you comment on state of this PR?

[r4hul77]

Hi, is there any plan for this ? I want to add NavState Between Factor for dynamics constraint between 2 states. My state is NavState as I want to conveniently use ImuFactor2

[varunagrawal]

@dellaert @r4hul77 this paper and this paper have detailed descriptions of the various $SE_2(3)$ Lie algebra operations which I plan to implement. However, this will take a while since I am currently occupied with ICRA and PhD stuff.

[r4hul77]

Gotcha, thanks for this amazing library btw. I kinda found a work around by setting up a hard constraint between a pose3 and NavStates Pose3 to be equal and I'm doing the between factors between the pose3. Is it an acceptable work around ?

[stefangachter]

Hi @varunagrawal, there is an older SE2(3) implementation for GTSAM available from Bonnabel and Barrau's team: https://github.com/mbrossar/gtsam see, https://github.com/mbrossar/gtsam/blob/develop/gtsam/geometry/ExtendedPose3.h and https://github.com/mbrossar/gtsam/blob/develop/gtsam/geometry/ExtendedPose3.cpp
This implementation was used to improve IMU pre-integration as described in the paper authored by Brossard: https://ieeexplore.ieee.org/document/9519152 Maybe you can crosscheck or leverage your implementation based on Brossard's implementation.

[stefangachter]

Note that there is an error with coriolis term according to the paper cited above (https://ieeexplore.ieee.org/document/9519152):

As far as I can see, this is still not corrected:
https://github.com/mbrossar/gtsam/blob/a5e8d5f7f9ed77032f521b75c728176a42f2766c/gtsam/navigation/NavState.cpp#L222
Because it is part of NavState, it could be corrected within this PR.