Add Lie group capabilities to NavState

[varunagrawal]

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

Frank: this PR needs to just finish the transpose group operations.

[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.

[dellaert]

Given your PR to bring in Brossard’s changes, @stefangachter , it would be good to finish this PR, but @varunagrawal is very focused on other things. As Varun remarked, there are some things left:

• the older retract/local I think should be left unchanged unless there is good reason to change them.
• Unit tests in this PR are not complete. Especially exp/log derivatives need to be validated I think.

Re the other PR:

• ExtendedPose3 should be removed In favor of the Lie NavState I think, no need to have two redundant classes.
• Coriolis and earth rotation correction might have to be added in a separate PR
• Focus (IMHO) should be on the “invariant” advantages
  I’ll also paste these comments there.

[dellaert]

Why was retract unchanged but local coordinates changed? And, what’s the difference with example and logmap?

[dellaert]

@varunagrawal ?

[dellaert]

I merged two PRs, #1930 and #1932. #1930 was based on this PR but also introduced ExpmapTranslation. #1932 simplified the Expmap jacobians of Pose3 and hence also of NavState::Expmap.

I propose yet another PR takes care of retract and localCoordinates, and this one does the more advanced transpose operations...