Merge pull request #2416 from borglab/feature/ExtendedPose

Extended Pose SE_k(3)

Author: dellaert

.github/copilot-instructions.md

@@ -4,8 +4,16 @@ For reviewing PRs:
 * Use meaningful variable names, e.g. `measurement` not `msm`, avoid abbreviations.
 * Flag overly complex or long/functions: break up in smaller functions
 * On Windows it is necessary to explicitly export all functions from the library which should be externally accessible. To do this, include the macro `GTSAM_EXPORT` in your class or function definition.
+* When adding or modifying public classes/methods, always review and follow `Using-GTSAM-EXPORT.md` before finalizing changes, including template specialization/export rules.
 * If we add a C++ function to a `.i` file to expose it to the wrapper, we must ensure that the parameter names match exactly between the declaration in the header file and the declaration in the `.i`. Similarly, if we change any parameter names in a wrapped function in a header file, or change any parameter names in a `.i` file, we must change the corresponding function in the other file to reflect those changes.
 * Classes are Uppercase, methods and functions lowerMixedCase.
 * Public fields in structs keep plain names (no trailing underscore).
 * Apart from those naming conventions, we adopt Google C++ style.
+* Notebooks in `*/doc/*.ipynb` and `*/examples/*.ipynb` should follow the standard preamble:
+  1) title/introduction markdown cell,
+  2) copyright markdown cell tagged `remove-cell`,
+  3) Colab badge markdown cell,
+  4) Colab install code cell tagged `remove-cell`,
+  5) imports/setup code cell.
+  Use the same `remove-cell` tagging convention as existing notebooks so docs build and Colab behavior stay consistent.
 * After any code change, always run relevant tests via `make -j6 testXXX.run` in the build folder $WORKSPACE/build. If in VS code, ask for escalated permissions if needed.

gtsam/base/Lie.h

@@ -209,6 +209,7 @@ struct LieGroupTraits : public GetDimensionImpl<Class, Class::dimension> {
   // GetDimensionImpl handles resolving this to a static value or providing GetDimension(obj).
   inline constexpr static auto dimension = Class::dimension;
   using TangentVector = Eigen::Matrix<double, dimension, 1>;
+  using Jacobian = Eigen::Matrix<double, dimension, dimension>;
   using ChartJacobian = OptionalJacobian<dimension, dimension>;
 
   static TangentVector Local(const Class& origin, const Class& other,

gtsam/base/MatrixLieGroup.h

@@ -19,6 +19,7 @@
 #pragma once
 
 #include <gtsam/base/Lie.h>
+#include <array>
 #include <type_traits>
 
 namespace gtsam {
@@ -56,6 +57,9 @@ namespace gtsam {
     using Jacobian = typename Base::Jacobian;
     using TangentVector = typename Base::TangentVector;
 
+    /// @name Matrix Lie Group
+    /// @{
+
     /**
      * Vectorize the matrix representation of a Lie group element.
      * The derivative `H` is the `(N*N) x D` Jacobian of this vectorization map.
@@ -130,7 +134,149 @@ namespace gtsam {
       return adj;
     }
 
+    /**
+     * Adjoint action on a tangent vector.
+     *
+     * Returns Ad_g * xi with optional Jacobians with respect to g and xi.
+     */
+    TangentVector Adjoint(const TangentVector& xi,
+                          ChartJacobian H_this = {},
+                          ChartJacobian H_xi = {}) const {
+      const auto& m = static_cast<const Class&>(*this);
+      const Jacobian Ad = m.AdjointMap();
+      if (H_this) *H_this = -Ad * Class::adjointMap(xi);
+      if (H_xi) *H_xi = Ad;
+      return Ad * xi;
+    }
+
+    /**
+     * Dual Adjoint action on a tangent covector.
+     *
+     * Returns Ad_g^T * x with optional Jacobians with respect to g and x.
+     */
+    TangentVector AdjointTranspose(const TangentVector& x,
+                                   ChartJacobian H_this = {},
+                                   ChartJacobian H_x = {}) const {
+      const auto& m = static_cast<const Class&>(*this);
+      const Jacobian Ad = m.AdjointMap();
+      const TangentVector AdTx = Ad.transpose() * x;
+
+      if (H_this) {
+        const Eigen::Index d = tangentDim(&m, nullptr);
+        setZeroJacobian(H_this, d);
+        if constexpr (D == Eigen::Dynamic) {
+          for (Eigen::Index i = 0; i < d; ++i) {
+            H_this->col(i) =
+                Class::adjointMap(TangentVector::Unit(d, i)).transpose() * AdTx;
+          }
+        } else {
+          const auto& basis = adjointBasis();
+          for (Eigen::Index i = 0; i < d; ++i) {
+            H_this->col(i) = basis[static_cast<size_t>(i)].transpose() * AdTx;
+          }
+        }
+      }
+
+      if (H_x) *H_x = Ad.transpose();
+      return AdTx;
+    }
+
+    /**
+     * Lie algebra adjoint map ad_xi, with optional specialization in derived
+     * classes.
+     */
+    static Jacobian adjointMap(const TangentVector& xi) {
+      const Eigen::Index d = tangentDim(nullptr, &xi);
+      Jacobian ad;
+      if constexpr (D == Eigen::Dynamic) {
+        ad.setZero(d, d);
+      } else {
+        ad.setZero();
+      }
+      const auto Xi = Class::Hat(xi);
+      for (Eigen::Index i = 0; i < d; ++i) {
+        const auto Ei = Class::Hat(TangentVector::Unit(d, i));
+        ad.col(i) = Class::Vee(Xi * Ei - Ei * Xi);
+      }
+      return ad;
+    }
+
+    /**
+     * Lie algebra action ad_xi(y), with optional Jacobians.
+     */
+    static TangentVector adjoint(const TangentVector& xi,
+                                 const TangentVector& y, ChartJacobian Hxi = {},
+                                 ChartJacobian H_y = {}) {
+      const Jacobian ad_xi = Class::adjointMap(xi);
+      if (Hxi) *Hxi = -Class::adjointMap(y);
+      if (H_y) *H_y = ad_xi;
+      return ad_xi * y;
+    }
+
+    /**
+     * Dual Lie algebra action ad_xi^T(y), with optional Jacobians.
+     */
+    static TangentVector adjointTranspose(const TangentVector& xi,
+                                          const TangentVector& y,
+                                          ChartJacobian Hxi = {},
+                                          ChartJacobian H_y = {}) {
+      const Jacobian adT_xi = Class::adjointMap(xi).transpose();
+      if (Hxi) {
+        const Eigen::Index d = tangentDim(nullptr, &xi);
+        setZeroJacobian(Hxi, d);
+        if constexpr (D == Eigen::Dynamic) {
+          for (Eigen::Index i = 0; i < d; ++i) {
+            Hxi->col(i) =
+                Class::adjointMap(TangentVector::Unit(d, i)).transpose() * y;
+          }
+        } else {
+          const auto& basis = adjointBasis();
+          for (Eigen::Index i = 0; i < d; ++i) {
+            Hxi->col(i) = basis[static_cast<size_t>(i)].transpose() * y;
+          }
+        }
+      }
+      if (H_y) *H_y = adT_xi;
+      return adT_xi * y;
+    }
+
+    /// @}
+
   private:
+    static Eigen::Index tangentDim(const Class* m, const TangentVector* xi) {
+      if constexpr (D == Eigen::Dynamic) {
+        return m ? static_cast<Eigen::Index>(traits<Class>::GetDimension(*m))
+                 : static_cast<Eigen::Index>(xi->size());
+      } else {
+        (void)m;
+        (void)xi;
+        return D;
+      }
+    }
+
+    static void setZeroJacobian(ChartJacobian H, Eigen::Index d) {
+      if constexpr (D == Eigen::Dynamic) {
+        H->setZero(d, d);
+      } else {
+        (void)d;
+        H->setZero();
+      }
+    }
+
+    /// Basis maps ad_{e_i}, cached for fixed-size groups.
+    template <int DD = D, typename std::enable_if_t<DD != Eigen::Dynamic, int> = 0>
+    static const std::array<Jacobian, DD>& adjointBasis() {
+      static const std::array<Jacobian, DD> basis = []() {
+        std::array<Jacobian, DD> B{};
+        for (int i = 0; i < DD; ++i) {
+          B[static_cast<size_t>(i)] =
+              Class::adjointMap(TangentVector::Unit(DD, i));
+        }
+        return B;
+      }();
+      return basis;
+    }
+
     /// Pre-compute and store vectorized generators for fixed-size groups.
     inline static const Eigen::Matrix<double, internal::product(N, N), D>&
       VectorizedGenerators() {
@@ -142,10 +288,12 @@ namespace gtsam {
 
   namespace internal {
 
-    /// Adds LieAlgebra, Hat, Vee, and Vec to LieGroupTraits
+    /// Adds MatrixLieGroup methods to LieGroupTraits
     template <class Class, int N> struct MatrixLieGroupTraits : LieGroupTraits<Class> {
       using LieAlgebra = typename Class::LieAlgebra;
       using TangentVector = typename LieGroupTraits<Class>::TangentVector;
+      using Jacobian = typename LieGroupTraits<Class>::Jacobian;
+      using ChartJacobian = typename LieGroupTraits<Class>::ChartJacobian;
 
       static LieAlgebra Hat(const TangentVector& v) {
         return Class::Hat(v);
@@ -162,6 +310,37 @@ namespace gtsam {
         LieGroupTraits<Class>::dimension> H = {}) {
         return m.vec(H);
       }
+
+      static TangentVector AdjointTranspose(const Class& m,
+                                            const TangentVector& x,
+                                            ChartJacobian Hm = {},
+                                            ChartJacobian Hx = {}) {
+        return m.AdjointTranspose(x, Hm, Hx);
+      }
+
+      static TangentVector Adjoint(const Class& m, const TangentVector& x,
+                                   ChartJacobian Hm = {},
+                                   ChartJacobian Hx = {}) {
+        return m.Adjoint(x, Hm, Hx);
+      }
+
+      static Jacobian adjointMap(const TangentVector& xi) {
+        return Class::adjointMap(xi);
+      }
+
+      static TangentVector adjoint(const TangentVector& xi,
+                                   const TangentVector& y,
+                                   ChartJacobian Hxi = {},
+                                   ChartJacobian H_y = {}) {
+        return Class::adjoint(xi, y, Hxi, H_y);
+      }
+
+      static TangentVector adjointTranspose(const TangentVector& xi,
+                                            const TangentVector& y,
+                                            ChartJacobian Hxi = {},
+                                            ChartJacobian H_y = {}) {
+        return Class::adjointTranspose(xi, y, Hxi, H_y);
+      }
     };
 
     /// Both LieGroupTraits and Testable

gtsam/base/doc/MatrixLieGroup.md

@@ -29,9 +29,12 @@ You must implement everything from the `LieGroup.md` guide, plus the following:
 
 By inheriting from `gtsam::MatrixLieGroup`, you get:
 
-*   **A default `AdjointMap()` implementation**: This generic version works by repeatedly calling your `Hat` and `Vee` methods. While it is correct, it is often slow. For performance-critical applications, you should still provide a faster, closed-form `AdjointMap()` implementation in your class, which will override the default.
+*   **Default group adjoint methods**: `AdjointMap()`, `Adjoint(xi)`, and `AdjointTranspose(x)` are provided generically, including Jacobians for the latter two.
+*   **Default Lie algebra methods**: `adjointMap(xi)`, `adjoint(xi, y)`, and `adjointTranspose(xi, y)` are provided generically, including Jacobians for the latter two.
 *   **A `vec()` method**: This vectorizes the `N x N` matrix representation of your group element into an `(N*N) x 1` vector.
 
+**Performance Note:** The generic implementation of `AdjointMap()` is correct but may be slower because it is derived via the `Hat`/`Vee` mappings. If a closed-form expression for `AdjointMap()` is available for your group, consider overriding the default implementation for better performance.
+
 ### 4. Traits and Concept Checking
 
 Finally, the traits specialization in your header file must be updated to reflect that your class is now a `MatrixLieGroup`.