diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 49339c99e..827141e58 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -184,7 +184,6 @@ struct LieGroupTraits: public GetDimensionImpl { /// @{ using ManifoldType = Class; inline constexpr static auto dimension = Class::dimension; - using LieAlgebra = Class::LieAlgebra; using TangentVector = Eigen::Matrix; using ChartJacobian = OptionalJacobian; @@ -223,19 +222,29 @@ struct LieGroupTraits: public GetDimensionImpl { ChartJacobian H = {}) { return m.inverse(H); } + /// @} +}; + + +/// Both LieGroupTraits and Testable +template struct LieGroup: LieGroupTraits, Testable {}; + +/// Adds LieAlgebra, Hat, and Vie to LieGroupTraits +template struct MatrixLieGroupTraits: LieGroupTraits { + using LieAlgebra = typename Class::LieAlgebra; + using TangentVector = typename LieGroupTraits::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 struct LieGroup: LieGroupTraits, Testable {}; +template struct MatrixLieGroup: MatrixLieGroupTraits, Testable {}; } // \ namespace internal @@ -270,7 +279,6 @@ class IsLieGroup: public IsGroup, public IsManifold { public: typedef typename traits::structure_category structure_category_tag; typedef typename traits::ManifoldType ManifoldType; - typedef typename traits::LieAlgebra LieAlgebra; typedef typename traits::TangentVector TangentVector; typedef typename traits::ChartJacobian ChartJacobian; @@ -289,17 +297,32 @@ public: // log and exponential map with Jacobians g = traits::Expmap(v, Hg); v = traits::Logmap(g, Hg); - // hat and vee - X = traits::Hat(v); - v = traits::Vee(X); } private: T g, h; - LieAlgebra X; TangentVector v; ChartJacobian Hg, Hh; }; +/** + * Matrix Lie Group Concept + */ +template +class IsMatrixLieGroup: public IsLieGroup { +public: +typedef typename traits::LieAlgebra LieAlgebra; +typedef typename traits::TangentVector TangentVector; + + BOOST_CONCEPT_USAGE(IsMatrixLieGroup) { + // hat and vee + X = traits::Hat(xi); + xi = traits::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) diff --git a/gtsam/base/VectorSpace.h b/gtsam/base/VectorSpace.h index 4ed83d16b..21c8710f1 100644 --- a/gtsam/base/VectorSpace.h +++ b/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; + } + /// @} };