diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 52e8a4d01..a955b18a8 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -16,12 +16,9 @@ #include #include -//FIXME temporary until all conflicts with namespace traits resolved -#define traits traits_foo - namespace gtsam { -template struct traits {}; +template struct traits_x {}; /** * @name Algebraic Structure Tags @@ -38,90 +35,170 @@ struct vector_space_tag: public lie_group_tag {}; struct multiplicative_group_tag {}; struct additive_group_tag {}; -// a fictitious example -class Transformation; -template<> -struct traits { +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 { // Typedefs required by all manifold types. - typedef multiplicative_group_tag group_flavor; - typedef lie_group_tag structure_category; - typedef Transformation ManifoldType; - - enum { dimension = 6 }; + typedef manifold_tag structure_category; + enum { dimension = ManifoldType::dimension }; typedef Eigen::Matrix TangentVector; typedef OptionalJacobian ChartJacobian; - // Required by all Manifold types. + // For Testable + void Print(const ManifoldType& m) { + m.print(); + } + void Equals(const ManifoldType& m1, + const ManifoldType& m2, + double tol = 1e-8) { + return m1.equals(m2, tol); + } + static TangentVector Local(const ManifoldType& origin, - const ManifoldType& other); + const ManifoldType& other) { + return origin.localCoordinates(other); + } static ManifoldType Retract(const ManifoldType& origin, - const TangentVector& v); + const TangentVector& v) { + return origin.retract(v); + } - static int GetDimension(const ManifoldType& m){ return dimension; } - - // For Group. Only implemented for groups - static ManifoldType Compose(const ManifoldType& m1, - const ManifoldType& m2); - static ManifoldType Between(const ManifoldType& m1, - const ManifoldType& m2); - static ManifoldType Inverse(const ManifoldType& m); - - static Vector3 Act(const ManifoldType& T, - const Vector3& p); - static Vector3 Act(const ManifoldType& T, - const Vector3& p, - OptionalJacobian<3,3> Hp); - - - // For Lie Group. Only implemented for lie groups. - static ManifoldType Compose(const ManifoldType& m1, - const ManifoldType& m2, - ChartJacobian H1, - ChartJacobian H2); - static ManifoldType Between(const ManifoldType& m1, - const ManifoldType& m2, - ChartJacobian H1, - ChartJacobian H2); - static ManifoldType Inverse(const ManifoldType& m, - ChartJacobian H); - static Vector3 Act(const ManifoldType& T, - const Vector3& p, - OptionalJacobian<3, dimension> HT, - OptionalJacobian<3, 3> Hp); - static const ManifoldType Identity; static TangentVector Local(const ManifoldType& origin, const ManifoldType& other, ChartJacobian Horigin, - ChartJacobian Hother); + ChartJacobian Hother) { + return origin.localCoordinates(other, Horigin, Hother); + } static ManifoldType Retract(const ManifoldType& origin, const TangentVector& v, ChartJacobian Horigin, - ChartJacobian Hv); + ChartJacobian Hv) { + return origin.retract(v, Horigin, Hv); + } - static TangentVector Logmap(const ManifoldType& m); - static ManifoldType Expmap(const TangentVector& v); - static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm); - static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv); + static int GetDimension(const ManifoldType& m){ return m.dim(); } + +}; + + +/// A helper that implements the traits interface for GTSAM lie groups. +/// To use this for your gtsam type, define: +/// template<> struct traits : public LieGroup { }; +template +struct LieGroup { + // Typedefs required by all manifold types. + typedef manifold_tag structure_category; + enum { dimension = ManifoldType::dimension }; + typedef Eigen::Matrix TangentVector; + typedef OptionalJacobian ChartJacobian; // For Testable - static void Print(const ManifoldType& T); - static void Equals(const ManifoldType& m1, - const ManifoldType& m2, - double tol = 1e-8); + void Print(const ManifoldType& m) { + m.print(); + } + void Equals(const ManifoldType& m1, + const ManifoldType& m2, + double tol = 1e-8) { + return m1.equals(m2, tol); + } + + static TangentVector Local(const ManifoldType& origin, + const ManifoldType& other) { + return origin.localCoordinates(other); + } + + static ManifoldType Retract(const ManifoldType& 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(); } + + // For Group. Only implemented for groups + static ManifoldType Compose(const ManifoldType& m1, + const ManifoldType& m2) { + return m1.compose(m2); + } + + static ManifoldType Between(const ManifoldType& m1, + const ManifoldType& m2) { + return m1.between(m2); + } + + static ManifoldType Inverse(const ManifoldType& m) { + return m.inverse(); + } + + static ManifoldType Compose(const ManifoldType& m1, + const ManifoldType& m2, + ChartJacobian H1, + ChartJacobian H2) { + return m1.compose(m2, H1, H2); + } + + static ManifoldType Between(const ManifoldType& m1, + const ManifoldType& m2, + ChartJacobian H1, + ChartJacobian H2) { + return m1.between(m2, H1, H2); + } + + static ManifoldType Inverse(const ManifoldType& m, + ChartJacobian H) { + return m.inverse(H); + } + + static const ManifoldType identity = ManifoldType::Identity(); + + static TangentVector Logmap(const ManifoldType& m) { + return ManifoldType::Logmap(m); + } + + static ManifoldType Expmap(const TangentVector& v) { + return ManifoldType::Expmap(v); + } + + static TangentVector Logmap(const ManifoldType& m, ChartJacobian Hm) { + return ManifoldType::Logmap(m, Hm); + } + + static ManifoldType Expmap(const TangentVector& v, ChartJacobian Hv) { + return ManifoldType::Expmap(v, Hv); + } }; +} // namespace internal + /// Check invariants for Manifold type template -BOOST_CONCEPT_REQUIRES(((Testable >)),(bool)) // +BOOST_CONCEPT_REQUIRES(((Testable >)),(bool)) // check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { - typename traits::TangentVector v0 = traits::Local(a,a); - typename traits::TangentVector v = traits::Local(a,b); - T c = traits::Retract(a,v); - return v0.norm() < tol && traits::Equals(b,c,tol); + typename traits_x::TangentVector v0 = traits_x::Local(a,a); + typename traits_x::TangentVector v = traits_x::Local(a,b); + T c = traits_x::Retract(a,v); + return v0.norm() < tol && traits_x::Equals(b,c,tol); } #define GTSAM_MANIFOLD_DECLARATIONS(MANIFOLD,DIM,TANGENT_VECTOR) \ @@ -146,11 +223,11 @@ check_manifold_invariants(const T& a, const T& b, double tol=1e-9) { template class IsManifold { public: - typedef typename traits::structure_category structure_category_tag; - static const size_t dim = traits::dimension; - typedef typename traits::ManifoldType ManifoldType; - typedef typename traits::TangentVector TangentVector; - typedef typename traits::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; + typedef typename traits_x::ChartJacobian ChartJacobian; BOOST_CONCEPT_USAGE(IsManifold) { BOOST_STATIC_ASSERT_MSG( @@ -159,11 +236,11 @@ public: BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); // make sure Chart methods are defined - v = traits::Local(p,q); - q = traits::Retract(p,v); + v = traits_x::Local(p,q); + q = traits_x::Retract(p,v); // and the versions with Jacobians. - v = traits::Local(p,q,Hp,Hq); - q = traits::Retract(p,v,Hp,Hv); + v = traits_x::Local(p,q,Hp,Hq); + q = traits_x::Retract(p,v,Hp,Hv); } private: ManifoldType p,q; @@ -177,18 +254,18 @@ private: template class IsGroup { public: - typedef typename traits::structure_category structure_category_tag; - typedef typename traits::group_flavor flavor_tag; - //typedef typename traits::identity::value_type identity_value_type; + typedef typename traits_x::structure_category structure_category_tag; + typedef typename traits_x::group_flavor flavor_tag; + //typedef typename traits_x::identity::value_type identity_value_type; BOOST_CONCEPT_USAGE(IsGroup) { BOOST_STATIC_ASSERT_MSG( (boost::is_base_of::value), "This type's structure_category trait does not assert it as a group (or derived)"); - e = traits::identity; - e = traits::Compose(g, h); - e = traits::Between(g, h); - e = traits::Inverse(g); + e = traits_x::identity; + e = traits_x::Compose(g, h); + e = traits_x::Between(g, h); + e = traits_x::Inverse(g); operator_usage(flavor); // todo: how do we test the act concept? or do we even need to? } @@ -212,10 +289,10 @@ private: template BOOST_CONCEPT_REQUIRES(((IsGroup)),(bool)) // check_group_invariants(const G& a, const G& b, double tol = 1e-9) { - G e = traits::identity; - return traits::Equals(traits::Compose(a, traits::inverse(a)), e, tol) - && traits::Equals(traits::Between(a, b), traits::Compose(traits::Inverse(a), b), tol) - && traits::Equals(traits::Compose(a, traits::Between(a, b)), b, tol); + G e = traits_x::identity; + return traits_x::Equals(traits_x::Compose(a, traits_x::inverse(a)), e, tol) + && traits_x::Equals(traits_x::Between(a, b), traits_x::Compose(traits_x::Inverse(a), b), tol) + && traits_x::Equals(traits_x::Compose(a, traits_x::Between(a, b)), b, tol); } @@ -239,10 +316,10 @@ check_group_invariants(const G& a, const G& b, double tol = 1e-9) { template class IsLieGroup: public IsGroup, public IsManifold { public: - typedef typename traits::structure_category structure_category_tag; - typedef typename traits::ManifoldType ManifoldType; - typedef typename traits::TangentVector TangentVector; - typedef typename traits::ChartJacobian ChartJacobian; + typedef typename traits_x::structure_category structure_category_tag; + typedef typename traits_x::ManifoldType ManifoldType; + typedef typename traits_x::TangentVector TangentVector; + typedef typename traits_x::ChartJacobian ChartJacobian; BOOST_CONCEPT_USAGE(IsLieGroup) { BOOST_STATIC_ASSERT_MSG( @@ -250,15 +327,15 @@ public: "This type's trait does not assert it is a Lie group (or derived)"); // group opertations with Jacobians - g = traits::Compose(g, h, Hg, Hh); - g = traits::Between(g, h, Hg, Hh); - g = traits::Inverse(g, Hg); + g = traits_x::Compose(g, h, Hg, Hh); + g = traits_x::Between(g, h, Hg, Hh); + g = traits_x::Inverse(g, Hg); // log and exp map without Jacobians - g = traits::Expmap(v); - v = traits::Logmap(g); + g = traits_x::Expmap(v); + v = traits_x::Logmap(g); // log and exp map with Jacobians - g = traits::Expmap(v, Hg); - v = traits::Logmap(g, Hg); + g = traits_x::Expmap(v, Hg); + v = traits_x::Logmap(g, Hg); } private: LG g, h; @@ -271,7 +348,7 @@ template class IsVectorSpace: public IsLieGroup { public: - typedef typename traits::structure_category structure_category_tag; + typedef typename traits_x::structure_category structure_category_tag; BOOST_CONCEPT_USAGE(IsVectorSpace) { BOOST_STATIC_ASSERT_MSG( diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h index 3e9ab0ec7..4c226e166 100644 --- a/gtsam/geometry/Cyclic.h +++ b/gtsam/geometry/Cyclic.h @@ -67,7 +67,7 @@ public: /// Define cyclic group traits to be a model of the Group concept template -struct traits { +struct traits_x { typedef group_tag structure_category; GTSAM_ADDITIVE_GROUP(CYCLIC_TYPE); static const CYCLIC_TYPE identity = CYCLIC_TYPE::Identity(); diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index b9bfeee47..41cadc7d9 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -25,7 +25,7 @@ namespace gtsam { // Define group traits template -struct traits { +struct traits_x { typedef QUATERNION_TYPE ManifoldType; typedef QUATERNION_TYPE Q; typedef lie_group_tag structure_category; diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 9de35c46b..c8770fe3b 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -48,26 +48,6 @@ public: } }; -#define SO3_TEMPLATE -GTSAM_GROUP_IDENTITY0(SO3) -GTSAM_MULTIPLICATIVE_GROUP(SO3_TEMPLATE, SO3) - -/** - * Chart for SO3 comprising of exponential map and its inverse (log-map) - */ -typedef LieGroupChart SO3Chart; - -#define SO3_TANGENT Vector3 -#define SO3_CHART SO3Chart -GTSAM_MANIFOLD(SO3_TEMPLATE, SO3, 3, SO3_TANGENT, SO3_CHART) - -/// Define SO3 to be a model of the Lie Group concept -namespace traits { -template<> -struct structure_category { - typedef lie_group_tag type; -}; -} } // end namespace gtsam