From ff32b912bce7b9576f10a54b9616c7b25ce52b30 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 23 Dec 2014 15:04:53 +0100 Subject: [PATCH] Made Class apparent, made requireemnts simpler (no derivatives needed - for now). --- gtsam/base/Manifold.h | 134 +++++++++++++++++++++--------------------- 1 file changed, 66 insertions(+), 68 deletions(-) diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index 38357b132..358f21a37 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -54,51 +54,73 @@ template struct traits_x; namespace internal { -/// A helper that implements the traits interface for GTSAM manifolds. -/// To use this for your gtsam type, define: -/// template<> struct traits : public Manifold { }; -template -struct Manifold : Testable<_ManifoldType> { - // Typedefs required by all manifold types. - typedef _ManifoldType ManifoldType; - typedef manifold_tag structure_category; - enum { dimension = ManifoldType::dimension }; - typedef Eigen::Matrix TangentVector; - typedef OptionalJacobian ChartJacobian; +/// Requirements on type to pass it to Manifold template below +template +struct HasManifoldPrereqs { - static TangentVector Local(const ManifoldType& origin, - const ManifoldType& other) { + enum { dim = Class::dimension }; + + Class p, q; + Eigen::Matrix v; + OptionalJacobian Hp, Hq, Hv; + + BOOST_CONCEPT_USAGE(HasManifoldPrereqs) { + v = p.localCoordinates(q); + q = p.retract(v); + } +}; + +/// Extra manifold traits for fixed-dimension types +template +struct ManifoldImpl { + // Compile-time dimensionality + static int GetDimension(const Class&) { + return N; + } +}; + +/// Extra manifold traits for variable-dimension types +template +struct ManifoldImpl { + // Run-time dimensionality + static int GetDimension(const Class& m) { + return m.dim(); + } +}; + +/// A helper that implements the traits interface for GTSAM manifolds. +/// To use this for your class type, define: +/// template<> struct traits : public Manifold { }; +template +struct Manifold: Testable, ManifoldImpl { + + // Check that Class has the necessary machinery + BOOST_CONCEPT_ASSERT((HasManifoldPrereqs)); + + // Dimension of the manifold + enum { dimension = Class::dimension }; + + // Typedefs required by all manifold types. + typedef Class ManifoldType; + typedef manifold_tag structure_category; + typedef Eigen::Matrix TangentVector; + + // Local coordinates + static TangentVector Local(const Class& origin, const Class& other) { return origin.localCoordinates(other); } - static ManifoldType Retract(const ManifoldType& origin, - const TangentVector& v) { + // Retraction back to manifold + static Class Retract(const Class& origin, const TangentVector& v) { return origin.retract(v); } - - static TangentVector Local(const ManifoldType& origin, - const ManifoldType& other, - ChartJacobian Horigin, - ChartJacobian Hother) { - return origin.localCoordinates(other, Horigin, Hother); - } - - static ManifoldType Retract(const ManifoldType& origin, - const TangentVector& v, - ChartJacobian Horigin, - ChartJacobian Hv) { - return origin.retract(v, Horigin, Hv); - } - - static int GetDimension(const ManifoldType& m){ return m.dim(); } - }; } // \ namespace internal /// Check invariants for Manifold type template -BOOST_CONCEPT_REQUIRES(((Testable)),(bool)) // +BOOST_CONCEPT_REQUIRES(((IsTestable)),(bool)) // check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { typename traits_x::TangentVector v0 = traits_x::Local(a,a); typename traits_x::TangentVector v = traits_x::Local(a,b); @@ -106,35 +128,16 @@ check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { return v0.norm() < tol && traits_x::Equals(b,c,tol); } -#define GTSAM_MANIFOLD_DECLARATIONS(MANIFOLD,DIM,TANGENT_VECTOR) \ - typedef MANIFOLD ManifoldType;\ - typedef manifold_tag structure_category; \ - struct dimension : public boost::integral_constant {};\ - typedef TANGENT_VECTOR TangentVector;\ - typedef OptionalJacobian ChartJacobian; \ - static TangentVector Local(const ManifoldType& origin, \ - const ManifoldType& other, \ - ChartJacobian Horigin=boost::none, \ - ChartJacobian Hother=boost::none); \ - static ManifoldType Retract(const ManifoldType& origin, \ - const TangentVector& v,\ - ChartJacobian Horigin=boost::none, \ - ChartJacobian Hv=boost::none); \ - static int GetDimension(const ManifoldType& m) { return dimension; } - -/** - * Manifold concept - */ -template +/// Manifold concept +template class IsManifold { public: - typedef typename traits_x::structure_category structure_category_tag; - static const size_t dim = traits_x::dimension; - typedef typename traits_x::ManifoldType ManifoldType; - typedef typename traits_x::TangentVector TangentVector; - typedef typename traits_x::ChartJacobian ChartJacobian; + typedef typename traits_x::structure_category structure_category_tag; + static const size_t dim = traits_x::dimension; + typedef typename traits_x::ManifoldType ManifoldType; + typedef typename traits_x::TangentVector TangentVector; BOOST_CONCEPT_USAGE(IsManifold) { BOOST_STATIC_ASSERT_MSG( @@ -143,26 +146,21 @@ public: BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); // make sure Chart methods are defined - v = traits_x::Local(p, q); - q = traits_x::Retract(p, v); - // and the versions with Jacobians. - //v = traits_x::Local(p,q,Hp,Hq); - //q = traits_x::Retract(p,v,Hp,Hv); + v = traits_x::Local(p, q); + q = traits_x::Retract(p, v); } private: - ManifoldType p, q; - ChartJacobian Hp, Hq, Hv; TangentVector v; - bool b; + ManifoldType p, q; }; /// Give fixed size dimension of a type, fails at compile time if dynamic -template +template struct FixedDimension { typedef const int value_type; - static const int value = traits_x::dimension; + static const int value = traits_x::dimension; BOOST_STATIC_ASSERT_MSG(value != Eigen::Dynamic, "FixedDimension instantiated for dymanically-sized type."); };