Merge pull request #2419 from borglab/copilot/sub-pr-2418

Address PR #2418 review feedback: SWIG Jacobians, trait-based dim, AdjointMap perf note

Author: dellaert

gtsam/base/MatrixLieGroup.h

@@ -245,7 +245,7 @@ namespace gtsam {
   private:
     static Eigen::Index tangentDim(const Class* m, const TangentVector* xi) {
       if constexpr (D == Eigen::Dynamic) {
-        return m ? static_cast<Eigen::Index>(m->dim())
+        return m ? static_cast<Eigen::Index>(traits<Class>::GetDimension(*m))
                  : static_cast<Eigen::Index>(xi->size());
       } else {
         (void)m;

gtsam/base/doc/MatrixLieGroup.md

@@ -33,6 +33,8 @@ By inheriting from `gtsam::MatrixLieGroup`, you get:
 *   **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`.

gtsam/geometry/geometry.i

@@ -660,7 +660,11 @@ class Pose3 {
   // Matrix Lie Group
   gtsam::Matrix AdjointMap() const;
   gtsam::Vector Adjoint(gtsam::Vector xi_b) const;
+  gtsam::Vector Adjoint(gtsam::Vector xi_b, Eigen::Ref<Eigen::MatrixXd> H_this,
+                 Eigen::Ref<Eigen::MatrixXd> H_xib) const;
   gtsam::Vector AdjointTranspose(gtsam::Vector x) const;
+  gtsam::Vector AdjointTranspose(gtsam::Vector x, Eigen::Ref<Eigen::MatrixXd> H_this,
+                          Eigen::Ref<Eigen::MatrixXd> H_x) const;
   static gtsam::Matrix adjointMap(gtsam::Vector xi);
   static gtsam::Vector adjoint(gtsam::Vector xi, gtsam::Vector y);
   static gtsam::Vector adjointTranspose(gtsam::Vector xi, gtsam::Vector y);