diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index eafd7e3e2..8ac678e65 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -165,6 +165,50 @@ struct FixedDimension { "FixedDimension instantiated for dymanically-sized type."); }; +/// CRTP to construct the product manifold of two other manifolds, M1 and M2 +/// Assumes manifold structure from M1 and M2, and binary constructor +template +class ProductManifold: public std::pair { + BOOST_CONCEPT_ASSERT((IsManifold)); + BOOST_CONCEPT_ASSERT((IsManifold)); + +private: + const M1& g() const {return this->first;} + const M2& h() const {return this->second;} + +public: + enum { dimension = M1::dimension + M2::dimension }; + inline static size_t Dim() { return dimension;} + inline size_t dim() const { return dimension;} + + typedef Eigen::Matrix TangentVector; + + /// Default constructor yields identity + ProductManifold():std::pair(traits::Identity(),traits::Identity()) {} + + // Construct from two subgroup elements + ProductManifold(const M1& g, const M2& h):std::pair(g,h) {} + + /// Retract delta to manifold + Derived retract(const TangentVector& xi) const { + return Derived(traits::Retract(g(),xi.head(M1::dimension)), + traits::Retract(h(),xi.tail(M2::dimension))); + } + + /// Compute the coordinates in the tangent space + TangentVector localCoordinates(const Derived& other) const { + TangentVector xi; + xi << traits::Local(g(),other.g()), traits::Local(h(),other.h()); + return xi; + } +}; + +// Define any direct product group to be a model of the multiplicative Group concept +template +struct traits > : internal::Manifold< + ProductManifold > { +}; + } // \ namespace gtsam ///**