MatrixLieGroup

gtsam/base/Lie.h

@@ -184,7 +184,6 @@ struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> {
   /// @{
   using ManifoldType = Class;
   inline constexpr static auto dimension = Class::dimension;
-  using LieAlgebra = Class::LieAlgebra;
   using TangentVector = Eigen::Matrix<double, dimension, 1>;
   using ChartJacobian = OptionalJacobian<dimension, dimension>;
 
@@ -223,19 +222,29 @@ struct LieGroupTraits: public GetDimensionImpl<Class, Class::dimension> {
       ChartJacobian H = {}) {
     return m.inverse(H);
   }
+  /// @}
+};
+
+
+/// Both LieGroupTraits and Testable
+template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
+
+/// Adds LieAlgebra, Hat, and Vie to LieGroupTraits
+template<class Class> struct MatrixLieGroupTraits: LieGroupTraits<Class> {
+  using LieAlgebra = typename Class::LieAlgebra;
+  using TangentVector = typename LieGroupTraits<Class>::TangentVector;
 
   static LieAlgebra Hat(const TangentVector& v) {
     return Class::Hat(v);
   }
-  
+
   static TangentVector Vee(const LieAlgebra& X) {
     return Class::Vee(X);
   }
-  /// @}
 };
 
 /// Both LieGroupTraits and Testable
-template<class Class> struct LieGroup: LieGroupTraits<Class>, Testable<Class> {};
+template<class Class> struct MatrixLieGroup: MatrixLieGroupTraits<Class>, Testable<Class> {};
 
 } // \ namespace internal
 
@@ -270,7 +279,6 @@ class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
 public:
   typedef typename traits<T>::structure_category structure_category_tag;
   typedef typename traits<T>::ManifoldType ManifoldType;
-  typedef typename traits<T>::LieAlgebra LieAlgebra;
   typedef typename traits<T>::TangentVector TangentVector;
   typedef typename traits<T>::ChartJacobian ChartJacobian;
 
@@ -289,17 +297,32 @@ public:
     // log and exponential map with Jacobians
     g = traits<T>::Expmap(v, Hg);
     v = traits<T>::Logmap(g, Hg);
-    // hat and vee
-    X = traits<T>::Hat(v);
-    v = traits<T>::Vee(X);
   }
 private:
   T g, h;
-  LieAlgebra X;
   TangentVector v;
   ChartJacobian Hg, Hh;
 };
 
+/**
+ * Matrix Lie Group Concept
+ */
+template<typename T>
+class IsMatrixLieGroup: public IsLieGroup<T> {
+public:
+typedef typename traits<T>::LieAlgebra LieAlgebra;
+typedef typename traits<T>::TangentVector TangentVector;
+
+  BOOST_CONCEPT_USAGE(IsMatrixLieGroup) {
+    // hat and vee
+    X = traits<T>::Hat(xi);
+    xi = traits<T>::Vee(X);
+  }
+private:
+  LieAlgebra X;
+  TangentVector xi;
+};
+
 /**
  *  Three term approximation of the Baker-Campbell-Hausdorff formula
  *  In non-commutative Lie groups, when composing exp(Z) = exp(X)exp(Y)

gtsam/base/VectorSpace.h

@@ -420,6 +420,8 @@ struct DynamicTraits {
 
   /// @name Lie Group
   /// @{
+  using LieAlgebra = Dynamic;
+    
   static TangentVector Logmap(const Dynamic& m, ChartJacobian H = {}) {
     if (H) *H = Eye(m);
     TangentVector result(GetDimension(m));
@@ -450,6 +452,15 @@ struct DynamicTraits {
     if (H2) *H2 = Eye(v1);
     return v2 - v1;
   }
+  
+  static LieAlgebra Hat(const TangentVector& v) {
+    return v;
+  }
+  
+  static TangentVector Vee(const LieAlgebra& X) {
+    return X;
+  }
+  
   /// @}
 
 };