SE2(3) representation and Lie group pre-integration of Brossard et al. #1916

stefangachter · 2024-12-06T16:14:13Z

This pull request is to provide a "cleaned-up" version of Brossard's fork: https://github.com/mbrossar/gtsam
The PR adds two cmake options: GTSAM_LIEGROUP_PREINTEGRATION and GTSAM_EXTENDED_POSE_RETRACT
GTSAM_LIEGROUP_PREINTEGRATION enables the Lie group pre-integration as described in https://ieeexplore.ieee.org/document/9519152 (Note that this implementation differs only in the bias update from on-manifold pre-integration, i.e., integration and uncertainty propagation are the same.)
GTSAM_EXTENDED_POSE_RETRACT enables the extended-pose (SE2(3)) retraction in the Lie group pre-integration. This is disabled by default in the source of Brossard. Note that enabling pose retraction might "break" your optimization. It might need a consistent implementation of the extended pose for all the factors.
The handling of Earth rotation is enabled by default. See the conversation here #1613 as well.
(Note, the original branch is based on release 4.2. This branch is cherry-picked and not yet "tested".)

dellaert

Great! As I said in the NavState PR, I think:

I’d opt for landing that PR, maybe with the extendedPose3 tests added, so we just have the one class that does both manifold and Lie: NavState
Coriolis and earth rotation correction might have to be added in a separate PR, with tests, to make this PR cleaner/smaller
Focus (IMHO) should be on the “invariant” advantages, I.e., LieGroup integration.

dellaert · 2024-12-06T17:20:11Z

gtsam/navigation/NavState.h

@@ -18,6 +18,7 @@

 #pragma once

+#include <gtsam/geometry/ExtendedPose3.h>


Don’t see why

dellaert · 2024-12-06T17:22:52Z

gtsam/navigation/NavState.cpp

@@ -103,6 +103,16 @@ bool NavState::equals(const NavState& other, double tol) const {
      && equal_with_abs_tol(v_, other.v_, tol);
 }

+//------------------------------------------------------------------------------
+#ifdef GTSAM_EXTENDED_POSE_RETRACT


I think this is done for the ImuFactor only, hence I’d prefer a solution that chooses NavState::exmap in the IMUFactor itself, once the other PR lands, if LieGroup integration is selected.

dellaert · 2024-12-06T17:23:49Z

gtsam/navigation/NavState.cpp

-    D_dt_R, -I_3x3, Z_3x3, //
-    D_dv_R, Z_3x3, -I_3x3;
-  }
+      const ExtendedPose3 T0(R_, t_, v_);


Do you know why there are so many changes here?

dellaert · 2024-12-06T17:24:15Z

gtsam/navigation/PreintegrationBase.cpp

-                                             H2 ? &D_biasCorrected_bias : nullptr);
+      H2 ? &D_biasCorrected_bias : nullptr);
+
+  // Correct for Earth rate


Is this tested?

dellaert · 2024-12-06T17:24:59Z

gtsam/navigation/PreintegrationBase.cpp

-  NavState state_j = state_i.retract(xi,
-                                     H1 ? &D_predict_state : nullptr,
-                                     H1 || H2 ? &D_predict_delta : nullptr);
+  NavState state_j = state_ii.retract(xi, D_predict_state, D_predict_delta);


We. An select here to use expmap based on integration scheme, once NavState has Lie

dellaert · 2024-12-06T17:25:16Z

gtsam/navigation/tests/testLieGroupPreintegration.cpp

+/**
+ * @file    testLieGroupPreintegration.cpp
+ * @brief   Unit test for the LieGroupPreintegration
+ * @author  Luca Carlone


Brossard, Gachter?

dellaert · 2024-12-06T17:25:46Z

gtsam/navigation/tests/testNavState.cpp

@@ -53,7 +53,7 @@ TEST(NavState, Constructor) {
          numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
  EXPECT(
      assert_equal(
-          numericalDerivative32(create, kAttitude, kPosition, kVelocity), aH2));
+          numericalDerivative33(create, kAttitude, kPosition, kVelocity), aH3));


dellaert · 2024-12-06T17:26:07Z

gtsam/navigation/tests/testNavState.cpp

@@ -200,10 +200,10 @@ TEST(NavState, Coriolis2) {
 }

 TEST(NavState, Coriolis3) {
-  /** Consider a massless planet with an attached nav frame at 
-   *  n_omega = [0 0 1]', and a body at position n_t = [1 0 0]', travelling with 
+  /** Consider a massless planet with an attached nav frame at


This test did not change?

dellaert · 2024-12-06T17:27:09Z

gtsam/navigation/CombinedImuFactor.h


 namespace gtsam {

 #ifdef GTSAM_TANGENT_PREINTEGRATION
 typedef TangentPreintegration PreintegrationType;
 #else
+#ifdef GTSAM_LIEGROUP_PREINTEGRATION


We might want to consider deprecating using compile-time flags in favor of a run-time mechanism.

varunagrawal · 2024-12-07T06:42:20Z

@stefangachter would it help is #1613 was finished? Your pointers and comments on that PR are immensely helpful.

dellaert · 2024-12-10T21:43:41Z

Ok, since I have not yet heard yet from @stefangachter , I’ll take a crack at just the most basic group operations, making sure @varunagrawal changes and Brossard’s changes agree. I’ll do the PR straight to develop and then we can merge develop into both the PRs.

stefangachter · 2024-12-11T10:09:36Z

I'm sorry for not getting back to you sooner. I was out of the office and busy with other stuff. I wanted to gain an overview of the different implementations before making a "commitment." (And I am setting up gtsam for development purposes under Windows, which takes its time as well.)

dellaert · 2024-12-11T20:33:07Z

I'm sorry for not getting back to you sooner. I was out of the office and busy with other stuff. I wanted to gain an overview of the different implementations before making a "commitment." (And I am setting up gtsam for development purposes under Windows, which takes its time as well.)

No worries! I have been reading a lot of papers, and I’m not yet fully sure about which way to go, as I noticed the NavState manifold uses R,p,v and SE2(3) seems to always use R,v,p order. It’s a conundrum how to maintain backwards compatibility with NavState yet also agree with papers. Possibilities:

a separate class with Rvp. But it’s so much duplication!
One class but clearly document the constructor being Rpv. A bit confusing, but inline with Pose3 being both manifold and group.
Keep NavState as the SE2(3) “torsor” and have ExtendedPose3 inherit, with an Rvp constructor. Confusing as hell, and not inline with “combined manifold/group classes”.

TBH, if backwards compatibility was not an issue I’d probably opt to split all classes, e.g., Pose3/Transform3, etc., but that’s a can of worms.

stefangachter · 2024-12-12T06:58:12Z

From a state estimation point of view, I would like to be able to choose the composition of the state "freely" and not depend on a particular "structure". Currently, two variables are needed to estimate R, p, and v: 'x' representing pose (R,p) and 'v' representing velocity (v). It would be desirable to have a navigation state as a single variable 'x' but that can be composed of <SE(3), R3> or SE2(3), for example. This is my high-level view. -- I recently looked at https://github.com/aau-cns/Lie-plusplus, which allows compositions of groups.( There are other similar templated libraries for "groups". But I do not have an "overview" of them.) Thus, the navigation state NavState could be defined as a composition of groups. -- The biggest impact of the "navigation state representation" is on IMU pre-integration. But as Brossard et al. https://ieeexplore.ieee.org/document/9519152 and Delama et al. https://ieeexplore.ieee.org/document/10777045 show, the pre-integration can be nicely formulated in a "group composition agnostic" way. It would take an effort to adapt the code accordingly but backward "compatibility" might be possible. -- From your suggestion above, Frank, I would go for 2). -- P.S. Probably I am a little bit sloppy in using the term "group" and do not make the distinction to "manifold".

dellaert · 2024-12-12T14:24:36Z

From your suggestion above, Frank, I would go for 2). -- P.S. Probably I am a little bit sloppy in using the term "group" and do not make the distinction to "manifold".

All good points, I appreciate the input. I am actually more and more nudging to 3, though, as the most advanced methods now are based on the Equivariant filter, https://ieeexplore.ieee.org/document/9840886. There the distinction between manifold and group is paramount. It is crucial especially, when the manifold, e.g., Unit3, does not admit group operations - but SO(3) "acts" on the sphere.

varunagrawal · 2024-12-12T16:21:24Z

I'm sorry for not getting back to you sooner. I was out of the office and busy with other stuff. I wanted to gain an overview of the different implementations before making a "commitment." (And I am setting up gtsam for development purposes under Windows, which takes its time as well.)

No worries! I have been reading a lot of papers, and I’m not yet fully sure about which way to go, as I noticed the NavState manifold uses R,p,v and SE2(3) seems to always use R,v,p order. It’s a conundrum how to maintain backwards compatibility with NavState yet also agree with papers. Possibilities:

a separate class with Rvp. But it’s so much duplication!

One class but clearly document the constructor being Rpv. A bit confusing, but inline with Pose3 being both manifold and group.

Keep NavState as the SE2(3) “torsor” and have ExtendedPose3 inherit, with an Rvp constructor. Confusing as hell, and not inline with “combined manifold/group classes”.

TBH, if backwards compatibility was not an issue I’d probably opt to split all classes, e.g., Pose3/Transform3, etc., but that’s a can of worms.

Maybe my $0.02 since my state estimation metric paper uses the NavState as a $SE_2(3)$ group:

I personally think underneath the hood the representation we use doesn't quite matter. In my own paper, I used (R, p, v) and was able to show equivalent group operations. It's not ideal, but similar to how ConstantBias is (a, $\omega$) (which a lot of people have asked me about), it's something we have to live with. I would focus on clean documentation instead aka option 2.
Thanks for all the nice resources @stefangachter! Both you and @dellaert seem to have found a vein of research with the equivariant estimators that I was not familiar with. I have been following work on Invariant filtering (and smoothing) from various groups, and my primary motivation was to have something like this for my own work. PS the first author, Ziwon, is now a PhD student at Georgia Tech.

ProfFan · 2024-12-13T02:49:22Z

2. I have been following work on Invariant filtering (and smoothing) from various groups, and my primary motivation was to have something like this for my own work. PS the first author, Ziwon, is now a PhD student at Georgia Tech.

Thank you for the great reference! I got one question though - the invariant filtering math in this paper is the same as in Grizzle's paper (R. Hartley, M. Ghaffari, R. M. Eustice, and J. W. Grizzle, “Contact-aided invariant extended Kalman ﬁltering for robot state estimation,” Int. J. Robot. Res., vol. 39, no. 4, pp. 402–430, 2020.), correct? The novelty of this paper is more on the slip detection method? Please correct me if I am wrong.

stefangachter · 2024-12-13T08:32:01Z

@dellaert, the code I provide here is a copy of Brossard's. I did not review the code. I adapted it slightly so that I could integrate and test it with the latest develop branch. I have limited time available to do some further improvements and reviews. Should I resolve your comments above? Or, how would you like to go forward?

dellaert · 2024-12-13T12:16:20Z

I’ll take a look at both PRs in detail - don’t worry for now.

stefan-gaechter-hxgn-geosystems added 6 commits December 6, 2024 17:00

Add options for lie group preintegration

cf2d7ca

Add extended pose

41fd7b5

Add Lie group pre-integration

ad3e6b9

Use extended pose in NavState

05d54bb

Enable Lie group pre-integration

a1351b8

Consider rotating earth

4e289b6

dellaert reviewed Dec 6, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

SE2(3) representation and Lie group pre-integration of Brossard et al. #1916

SE2(3) representation and Lie group pre-integration of Brossard et al. #1916

stefangachter commented Dec 6, 2024 •

edited

Loading

dellaert left a comment

dellaert Dec 6, 2024

dellaert Dec 6, 2024

dellaert Dec 6, 2024

dellaert Dec 6, 2024

dellaert Dec 6, 2024

dellaert Dec 6, 2024

dellaert Dec 6, 2024

dellaert Dec 6, 2024

dellaert Dec 6, 2024

varunagrawal commented Dec 7, 2024

dellaert commented Dec 10, 2024

stefangachter commented Dec 11, 2024

dellaert commented Dec 11, 2024

stefangachter commented Dec 12, 2024

dellaert commented Dec 12, 2024

varunagrawal commented Dec 12, 2024

ProfFan commented Dec 13, 2024 •

edited

Loading

stefangachter commented Dec 13, 2024

dellaert commented Dec 13, 2024

		@@ -18,6 +18,7 @@

		#pragma once

		#include <gtsam/geometry/ExtendedPose3.h>

SE2(3) representation and Lie group pre-integration of Brossard et al. #1916

Are you sure you want to change the base?

SE2(3) representation and Lie group pre-integration of Brossard et al. #1916

Conversation

stefangachter commented Dec 6, 2024 • edited Loading

dellaert left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

varunagrawal commented Dec 7, 2024

dellaert commented Dec 10, 2024

stefangachter commented Dec 11, 2024

dellaert commented Dec 11, 2024

stefangachter commented Dec 12, 2024

dellaert commented Dec 12, 2024

varunagrawal commented Dec 12, 2024

ProfFan commented Dec 13, 2024 • edited Loading

stefangachter commented Dec 13, 2024

dellaert commented Dec 13, 2024

stefangachter commented Dec 6, 2024 •

edited

Loading

ProfFan commented Dec 13, 2024 •

edited

Loading