Merge pull request #2192 from borglab/feature/custom_vecs

Bespoke vec methods

Author: dellaert

gtsam/base/Group.h

@@ -23,6 +23,7 @@
 #include <gtsam/base/Testable.h>
 
 #include <utility>
+#include <type_traits>
 
 namespace gtsam {
 
@@ -47,28 +48,25 @@ class IsGroup {
 
   GTSAM_CONCEPT_USAGE(IsGroup) {
     static_assert(
-        (std::is_base_of<group_tag, structure_category_tag>::value),
+        (std::is_base_of_v<group_tag, structure_category_tag>),
         "This type's structure_category trait does not assert it as a group (or derived)");
     e = traits<G>::Identity();
     e = traits<G>::Compose(g, h);
     e = traits<G>::Between(g, h);
     e = traits<G>::Inverse(g);
-    operator_usage(flavor);
+
+    if constexpr (std::is_same_v<flavor_tag, multiplicative_group_tag>) {
+      e = g * h;
+      //e = -g; // todo this should work, but it is failing for Quaternions
+    } else if constexpr (std::is_same_v<flavor_tag, additive_group_tag>) {
+      e = g + h;
+      e = h - g;
+      e = -g;
+    }
     // todo: how do we test the act concept? or do we even need to?
   }
 
 private:
-  void operator_usage(multiplicative_group_tag) {
-    e = g * h;
-    //e = -g; // todo this should work, but it is failing for Quaternions
-  }
-  void operator_usage(additive_group_tag) {
-    e = g + h;
-    e = h - g;
-    e = -g;
-  }
-
-  flavor_tag flavor;
   G e, g, h;
   bool b;
 };

gtsam/base/Lie.h

@@ -26,6 +26,8 @@
 #include <gtsam/base/Manifold.h>
 #include <gtsam/base/Group.h>
 
+#include <type_traits>
+
 namespace gtsam {
 
 /// A CRTP helper class that implements Lie group methods
@@ -56,15 +58,15 @@ struct LieGroup {
   Class compose(const Class& g, ChartJacobian H1,
       ChartJacobian H2 = {}) const {
     if (H1) *H1 = g.inverse().AdjointMap();
-    if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
+    if (H2) *H2 = identityMatrix();
     return derived() * g;
   }
 
   Class between(const Class& g, ChartJacobian H1,
       ChartJacobian H2 = {}) const {
     Class result = derived().inverse() * g;
     if (H1) *H1 = - result.inverse().AdjointMap();
-    if (H2) *H2 = Eigen::Matrix<double, N, N>::Identity();
+    if (H2) *H2 = identityMatrix();
     return result;
   }
 
@@ -158,6 +160,17 @@ struct LieGroup {
     if (H2) *H2 = D_v_h;
     return v;
   }
+
+ private:
+
+  // Helper to get identity matrix of correct size for static or dynamic N
+  Jacobian identityMatrix() const {
+    if constexpr (N == Eigen::Dynamic) {
+      return Jacobian::Identity(derived().dim(), derived().dim());
+    } else {
+      return Jacobian::Identity();
+    }
+  }
 };
 
 /// tag to assert a type is a Lie group
@@ -271,12 +284,12 @@ class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
 public:
   // Concept marker: allows checking IsLieGroup<T>::value in templates
   static constexpr bool value =
-    std::is_base_of<lie_group_tag, typename traits<T>::structure_category>::value;
+    std::is_base_of_v<lie_group_tag, typename traits<T>::structure_category>;
 
-  typedef typename traits<T>::structure_category structure_category_tag;
-  typedef typename traits<T>::ManifoldType ManifoldType;
-  typedef typename traits<T>::TangentVector TangentVector;
-  typedef typename traits<T>::ChartJacobian ChartJacobian;
+  using structure_category_tag = typename traits<T>::structure_category;
+  using ManifoldType = typename traits<T>::ManifoldType;
+  using TangentVector = typename traits<T>::TangentVector;
+  using ChartJacobian = typename traits<T>::ChartJacobian;
 
   GTSAM_CONCEPT_USAGE(IsLieGroup) {
     static_assert(

gtsam/base/Manifold.h

@@ -24,6 +24,8 @@
 #include <gtsam/base/OptionalJacobian.h>
 #include <gtsam/base/concepts.h>
 
+#include <type_traits>
+
 namespace gtsam {
 
 /// tag to assert a type is a manifold
@@ -67,21 +69,17 @@ struct HasManifoldPrereqs {
   }
 };
 
-/// Extra manifold traits for fixed-dimension types
+/// Traits to get dimension, supporting both fixed and dynamic
 template<class Class, int N>
 struct GetDimensionImpl {
-  // Compile-time dimensionality
-  static int GetDimension(const Class&) {
-    return N;
-  }
-};
-
-/// Extra manifold traits for variable-dimension types
-template<class Class>
-struct GetDimensionImpl<Class, Eigen::Dynamic> {
-  // Run-time dimensionality
+  // Get dimension at compile-time for fixed-size manifolds, and at
+  // run-time for dynamic-size manifolds.
   static int GetDimension(const Class& m) {
-    return m.dim();
+    if constexpr (N == Eigen::Dynamic) {
+      return m.dim();
+    } else {
+      return N;
+    }
   }
 };
 
@@ -134,19 +132,21 @@ class IsManifold {
 
 public:
 
-  typedef typename traits<T>::structure_category structure_category_tag;
-  static const int dim = traits<T>::dimension;
-  typedef typename traits<T>::ManifoldType ManifoldType;
-  typedef typename traits<T>::TangentVector TangentVector;
+  using structure_category_tag = typename traits<T>::structure_category;
+  static inline constexpr int dim = traits<T>::dimension;
+  using ManifoldType = typename traits<T>::ManifoldType;
+  using TangentVector = typename traits<T>::TangentVector;
   // Concept marker: allows checking IsManifold<T>::value in templates
   static constexpr bool value =
-    std::is_base_of<manifold_tag, structure_category_tag>::value;
+    std::is_base_of_v<manifold_tag, structure_category_tag>;
 
   GTSAM_CONCEPT_USAGE(IsManifold) {
     static_assert(
         value,
         "This type's structure_category trait does not assert it as a manifold (or derived)");
-    static_assert(TangentVector::SizeAtCompileTime == dim);
+    if constexpr (dim != Eigen::Dynamic) {
+      static_assert(TangentVector::SizeAtCompileTime == dim);
+    }
 
     // make sure Chart methods are defined
     v = traits<T>::Local(p, q);
@@ -162,8 +162,8 @@ class IsManifold {
 /// Give fixed size dimension of a type, fails at compile time if dynamic
 template<typename T>
 struct FixedDimension {
-  typedef const int value_type;
-  static const int value = traits<T>::dimension;
+  using value_type = const int;
+  static inline constexpr int value = traits<T>::dimension;
   static_assert(value != Eigen::Dynamic,
       "FixedDimension instantiated for dynamically-sized type.");
 };

gtsam/base/MatrixLieGroup.h

@@ -173,8 +173,8 @@ namespace gtsam {
   template<typename T>
   class IsMatrixLieGroup : public IsLieGroup<T> {
   public:
-    typedef typename traits<T>::LieAlgebra LieAlgebra;
-    typedef typename traits<T>::TangentVector TangentVector;
+    using LieAlgebra = typename traits<T>::LieAlgebra;
+    using TangentVector = typename traits<T>::TangentVector;
 
     GTSAM_CONCEPT_USAGE(IsMatrixLieGroup) {
       // hat and vee
@@ -207,7 +207,7 @@ namespace gtsam {
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43
   /// @deprecated: use T::Hat
   template <class T>
-  Matrix wedge(const Vector& x) {
+  [[deprecated("use T::Hat instead")]] Matrix wedge(const Vector& x) {
     return T::Hat(x);
   }
 #endif