From 06640cc4144f5c675b84f473e66f21e4a72c5158 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 09:48:31 +0100 Subject: [PATCH 1/9] New naming convention for concept (IsGroup), moved invariant checking out to namespace --- gtsam/base/concepts.h | 77 ++++++++++++------------- gtsam/geometry/tests/testCyclic.cpp | 2 +- gtsam/geometry/tests/testQuaternion.cpp | 7 ++- 3 files changed, 43 insertions(+), 43 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index dc4fca036..a6b2448c2 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -11,6 +11,7 @@ //#include "manifold.h" //#include "chart.h" #include +#include #include #include #include @@ -24,7 +25,7 @@ namespace traits { * @brief Associate a unique tag with each of the main GTSAM concepts */ //@{ -template +template struct structure_category; // specializations should be derived from one of the following tags //@} @@ -50,14 +51,14 @@ namespace traits { /** @name Manifold Traits */ //@{ -template struct TangentVector; -template struct DefaultChart; +template struct TangentVector; +template struct DefaultChart; //@} }// namespace traits /* - template + template class ManifoldConcept { public: typedef T Manifold; @@ -75,7 +76,7 @@ template struct DefaultChart; TangentVector v; }; - template + template class ChartConcept { public: typedef C Chart; @@ -107,8 +108,8 @@ namespace traits { /** @name Group Traits */ //@{ -template struct identity; -template struct flavor; +template struct identity; +template struct flavor; //@} /** @name Group Flavor Tags */ @@ -120,62 +121,60 @@ struct multiplicative_tag { //@} } // \ namespace traits + +/// Check invariants +template +//BOOST_CONCEPT_REQUIRES((Testable)) +bool check_invariants(const G& a, const G& b) { + G e = traits::identity::value; + typename traits::flavor::type flavor; + return (equal(compose(a, inverse(a)), e)) + && (equal(between(a, b), compose(inverse(a), b))) + && (equal(compose(a, between(a, b)), b)) // + && operator_usage(a, b, flavor); +} } // \ namespace group /** * Group Concept */ template -class Group { +class IsGroup { public: typedef typename traits::structure_category::type structure_category_tag; typedef typename group::traits::identity::value_type identity_value_type; typedef typename group::traits::flavor::type flavor_tag; - BOOST_CONCEPT_USAGE(Group) { + void operator_usage(group::traits::multiplicative_tag) { + g = g * h; + } + void operator_usage(group::traits::additive_tag) { + g = g + h; + g = h - g; + g = -g; + } + + BOOST_CONCEPT_USAGE(IsGroup) { using group::compose; using group::between; using group::inverse; BOOST_STATIC_ASSERT( boost::is_base_of::value); e = group::traits::identity::value; - d = compose(g, h); - d = between(g, h); - ig = inverse(g); - test = operator_usage(g, h, flavor); + g = compose(g, h); + g = between(g, h); + g = inverse(g); + operator_usage(flavor); } -// TODO: these all require default constructors :-( -// Also, requires equal which is not required of a group -// Group():e(group::traits::identity::value) { -// } -// -// bool check_invariants(const G& a, const G& b) { -// return (equal(compose(a, inverse(a)), e)) -// && (equal(between(a, b), compose(inverse(a), b))) -// && (equal(compose(a, between(a, b)), b)) -// && operator_usage(a, b, flavor); -// } - private: flavor_tag flavor; - G e, g, h, gh, ig, d; - bool test, test2; - - bool operator_usage(const G& a, const G& b, - group::traits::multiplicative_tag) { -// return group::compose(a, b) == a * b; - return true; - } - bool operator_usage(const G& a, const G& b, group::traits::additive_tag) { - return group::compose(a, b) == a + b; - } - + G e, g, h; }; /* - template + template class LieGroupConcept : public GroupConcept, public ManifoldConcept { BOOST_CONCEPT_USAGE(LieGroupConcept) { @@ -183,7 +182,7 @@ private: } }; - template + template class VectorSpaceConcept : public LieGroupConcept { typedef typename traits::DefaultChart::type Chart; typedef typename GroupConcept::identity identity; diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index 00ea2c853..f9d4a2d77 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -26,7 +26,7 @@ typedef Cyclic<6> G; // Let's use the cyclic group of order 6 //****************************************************************************** TEST(Cyclic, Concept) { - BOOST_CONCEPT_ASSERT((Group)); + BOOST_CONCEPT_ASSERT((IsGroup)); EXPECT_LONGS_EQUAL(0, group::traits::identity::value); G g(2), h(3); // EXPECT(Group().check_invariants(g,h)) diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index a35576259..51ea99d14 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -55,7 +55,7 @@ struct identity { typedef Quaternion value_type; }; -const Quaternion identity::value = Quaternion(0); +const Quaternion identity::value = Quaternion::Identity(); /// Define the trait that asserts Quaternion is an additive group template<> @@ -75,6 +75,7 @@ struct flavor { //#include #include +#include #include using namespace std; @@ -84,12 +85,12 @@ typedef Quaternion Q; // Typedef //****************************************************************************** TEST(Quaternion, Concept) { - BOOST_CONCEPT_ASSERT((Group)); + BOOST_CONCEPT_ASSERT((IsGroup)); } //****************************************************************************** TEST(Quaternion, Constructor) { - Q g(0); + Q g(Eigen::AngleAxisd(1, Vector3(0,0,1))); } //****************************************************************************** From 3a6b89e840abae4126ac20863406f3895537eca2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 10:19:38 +0100 Subject: [PATCH 2/9] Now all Eigen::Quaternion flavors are certified as IsGroup --- gtsam/geometry/tests/testQuaternion.cpp | 68 +++++++++++++++---------- 1 file changed, 41 insertions(+), 27 deletions(-) diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 51ea99d14..90aeffe28 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -19,47 +19,54 @@ namespace gtsam { -/// Typedef to an Eigen Quaternion, we disable alignment because -/// geometry objects are stored in boost pool allocators, in Values -/// containers, and and these pool allocators do not support alignment. -typedef Eigen::Quaternion Quaternion; - namespace traits { -/// Define Quaternion to be a model of the Group concept -template<> -struct structure_category { +/// Define Eigen::Quaternion to be a model of the Group concept +template +struct structure_category > { typedef group_tag type; }; } // \namespace gtsam::traits namespace group { -Quaternion compose(const Quaternion&g, const Quaternion& h) { +template +Eigen::Quaternion<_Scalar, _Options> compose( + const Eigen::Quaternion<_Scalar, _Options> &g, + const Eigen::Quaternion<_Scalar, _Options> & h) { return g * h; } -Quaternion between(const Quaternion&g, const Quaternion& h) { +template +Eigen::Quaternion<_Scalar, _Options> between( + const Eigen::Quaternion<_Scalar, _Options> &g, + const Eigen::Quaternion<_Scalar, _Options> & h) { return g.inverse() * h; } -Quaternion inverse(const Quaternion&g) { +template +Eigen::Quaternion<_Scalar, _Options> inverse( + const Eigen::Quaternion<_Scalar, _Options> &g) { return g.inverse(); } namespace traits { -/// Define the trait that specifies Quaternion's identity element -template<> -struct identity { - static const Quaternion value; - typedef Quaternion value_type; +/// Declare the trait that specifies a quaternion's identity element +template +struct identity > { + static const Eigen::Quaternion<_Scalar, _Options> value; + typedef Eigen::Quaternion<_Scalar, _Options> value_type; }; -const Quaternion identity::value = Quaternion::Identity(); +/// Out of line definition of identity +template +const Eigen::Quaternion<_Scalar, _Options> identity< + Eigen::Quaternion<_Scalar, _Options> >::value = Eigen::Quaternion<_Scalar, + _Options>::Identity(); -/// Define the trait that asserts Quaternion is an additive group -template<> -struct flavor { +/// Define the trait that asserts quaternions are a multiplicative group +template +struct flavor > { typedef multiplicative_tag type; }; @@ -67,6 +74,13 @@ struct flavor { } // \namespace gtsam::group } // \namespace gtsam +/** + * GSAM typedef to an Eigen::Quaternion, we disable alignment because + * geometry objects are stored in boost pool allocators, in Values containers, + * and and these pool allocators do not support alignment. + */ +typedef Eigen::Quaternion Quaternion; + /** * @file testCyclic.cpp * @brief Unit tests for cyclic group @@ -84,25 +98,25 @@ using namespace gtsam; typedef Quaternion Q; // Typedef //****************************************************************************** -TEST(Quaternion, Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); +TEST(Quaternion , Concept) { + BOOST_CONCEPT_ASSERT((IsGroup)); } //****************************************************************************** -TEST(Quaternion, Constructor) { - Q g(Eigen::AngleAxisd(1, Vector3(0,0,1))); +TEST(Quaternion , Constructor) { + Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } //****************************************************************************** -TEST(Quaternion, Compose) { +TEST(Quaternion , Compose) { } //****************************************************************************** -TEST(Quaternion, Between) { +TEST(Quaternion , Between) { } //****************************************************************************** -TEST(Quaternion, Ivnverse) { +TEST(Quaternion , Ivnverse) { } //****************************************************************************** From 187760ce95ad28b64ff774c06e49c90b195af1dd Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 10:20:54 +0100 Subject: [PATCH 3/9] Refactor for readability --- gtsam/geometry/tests/testQuaternion.cpp | 42 +++++++++++-------------- 1 file changed, 19 insertions(+), 23 deletions(-) diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 90aeffe28..339897a06 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -21,52 +21,48 @@ namespace gtsam { namespace traits { /// Define Eigen::Quaternion to be a model of the Group concept -template -struct structure_category > { +template +struct structure_category > { typedef group_tag type; }; } // \namespace gtsam::traits namespace group { -template -Eigen::Quaternion<_Scalar, _Options> compose( - const Eigen::Quaternion<_Scalar, _Options> &g, - const Eigen::Quaternion<_Scalar, _Options> & h) { +template +Eigen::Quaternion compose(const Eigen::Quaternion &g, + const Eigen::Quaternion & h) { return g * h; } -template -Eigen::Quaternion<_Scalar, _Options> between( - const Eigen::Quaternion<_Scalar, _Options> &g, - const Eigen::Quaternion<_Scalar, _Options> & h) { +template +Eigen::Quaternion between(const Eigen::Quaternion &g, + const Eigen::Quaternion & h) { return g.inverse() * h; } -template -Eigen::Quaternion<_Scalar, _Options> inverse( - const Eigen::Quaternion<_Scalar, _Options> &g) { +template +Eigen::Quaternion inverse(const Eigen::Quaternion &g) { return g.inverse(); } namespace traits { /// Declare the trait that specifies a quaternion's identity element -template -struct identity > { - static const Eigen::Quaternion<_Scalar, _Options> value; - typedef Eigen::Quaternion<_Scalar, _Options> value_type; +template +struct identity > { + static const Eigen::Quaternion value; + typedef Eigen::Quaternion value_type; }; /// Out of line definition of identity -template -const Eigen::Quaternion<_Scalar, _Options> identity< - Eigen::Quaternion<_Scalar, _Options> >::value = Eigen::Quaternion<_Scalar, - _Options>::Identity(); +template +const Eigen::Quaternion identity >::value = + Eigen::Quaternion::Identity(); /// Define the trait that asserts quaternions are a multiplicative group -template -struct flavor > { +template +struct flavor > { typedef multiplicative_tag type; }; From 01aab775041cb68427487d538fe71731f1e50cf5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 11:51:40 +0100 Subject: [PATCH 4/9] Made Testable a boost concept --- gtsam/base/Testable.h | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/gtsam/base/Testable.h b/gtsam/base/Testable.h index a308c50a1..83c2fa4e8 100644 --- a/gtsam/base/Testable.h +++ b/gtsam/base/Testable.h @@ -34,6 +34,7 @@ #pragma once #include +#include #include #include @@ -50,17 +51,20 @@ namespace gtsam { * @tparam T is the type this constrains to be testable - assumes print() and equals() */ template - class TestableConcept { - static bool checkTestableConcept(const T& d) { + class Testable { + T t; + bool r1,r2; + public: + + BOOST_CONCEPT_USAGE(Testable) { // check print function, with optional string - d.print(std::string()); - d.print(); + t.print(std::string()); + t.print(); // check print, with optional threshold double tol = 1.0; - bool r1 = d.equals(d, tol); - bool r2 = d.equals(d); - return r1 && r2; + r1 = t.equals(t, tol); + r2 = t.equals(t); } }; @@ -129,6 +133,7 @@ namespace gtsam { * * NOTE: intentionally not in the gtsam namespace to allow for classes not in * the gtsam namespace to be more easily enforced as testable + * @deprecated please use BOOST_CONCEPT_ASSERT and */ -#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::TestableConcept; -#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept _gtsam_TestableConcept_##T; +#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::Testable; +#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::Testable _gtsam_Testable_##T; From e2f250c160b7e2241f471ae5615ab1cdbdf872d6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 11:52:09 +0100 Subject: [PATCH 5/9] Added Manifold, Lie Group, and Vector Space concepts back in --- gtsam/base/concepts.h | 240 ++++++++++++++---------- gtsam/geometry/tests/testCyclic.cpp | 6 + gtsam/geometry/tests/testQuaternion.cpp | 49 ++++- 3 files changed, 195 insertions(+), 100 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index a6b2448c2..6caf1703f 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -25,9 +26,7 @@ namespace traits { * @brief Associate a unique tag with each of the main GTSAM concepts */ //@{ -template -struct structure_category; -// specializations should be derived from one of the following tags +template struct structure_category; //@} /** @@ -35,81 +34,95 @@ struct structure_category; * @brief Possible values for traits::structure_category::type */ //@{ -struct manifold_tag { -}; -struct group_tag { -}; -struct lie_group_tag: public manifold_tag, public group_tag { -}; -struct vector_space_tag: public lie_group_tag { -}; +struct manifold_tag {}; +struct group_tag {}; +struct lie_group_tag: public manifold_tag, public group_tag {}; +struct vector_space_tag: public lie_group_tag {}; //@} }// namespace traits +namespace manifold { + +/** @name Free functions any Manifold needs to define */ +//@{ +//@} + namespace traits { /** @name Manifold Traits */ //@{ +template struct dimension; template struct TangentVector; template struct DefaultChart; //@} -}// namespace traits +}// \ namespace traits -/* - template - class ManifoldConcept { - public: - typedef T Manifold; - typedef typename traits::TangentVector::type TangentVector; - typedef typename traits::DefaultChart::type DefaultChart; - static const size_t dim = traits::dimension::value; +/// Check invariants for Manifold type +template +BOOST_CONCEPT_REQUIRES(((Testable)),(bool)) // +check_invariants(const T& a, const T& b) { + typedef typename traits::DefaultChart::type Chart; + return true; +} - BOOST_CONCEPT_USAGE(ManifoldConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); - // no direct usage for manifold since most usage is through a chart - } - private: - Manifold p; - TangentVector v; - }; - - template - class ChartConcept { - public: - typedef C Chart; - typedef typename traits::Manifold::type Manifold; - typedef typename traits::TangentVector::type TangentVector; - - BOOST_CONCEPT_USAGE(ChartConcept) { - v = Chart::local(p,q); // returns local coordinates of q w.r.t. origin p - q = Chart::retract(p,v); // returns retracted update of p with v - } - - private: - Manifold p,q; - TangentVector v; - - }; +/** + * Base class for Charts + * Derived has to implement local and retract as static methods */ +template +struct Chart { + typedef T ManifoldType; + typedef typename traits::TangentVector::type TangentVector; + static TangentVector Local(const ManifoldType& p, const ManifoldType& q) { + return Derived::local(p, q); + } + static ManifoldType Retract(const ManifoldType& p, const TangentVector& v) { + return Derived::retract(p, v); + } +protected: + Chart() { + (void) &Local; + (void) &Retract; + } // enforce early instantiation. +}; + +} // \ namespace manifold + +template +class IsManifold { +public: + typedef typename traits::structure_category::type structure_category_tag; + static const size_t dim = manifold::traits::dimension::value; + typedef typename manifold::traits::TangentVector::type TangentVector; + typedef typename manifold::traits::DefaultChart::type DefaultChart; + + BOOST_CONCEPT_USAGE(IsManifold) { + BOOST_STATIC_ASSERT(boost::is_base_of::value, "This type's structure_category trait does not assert it as a manifold (or derived)"); + BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); + // no direct usage for manifold since most usage is through a chart + } +private: + T p; + TangentVector v; +}; namespace group { /** @name Free functions any Group needs to define */ //@{ -template G compose(const G&g, const G& h); -template G between(const G&g, const G& h); -template G inverse(const G&g); +template T compose(const T&g, const T& h); +template T between(const T&g, const T& h); +template T inverse(const T&g); //@} namespace traits { /** @name Group Traits */ //@{ -template struct identity; -template struct flavor; +template struct identity; +template struct flavor; //@} /** @name Group Flavor Tags */ @@ -120,31 +133,29 @@ struct multiplicative_tag { }; //@} -} // \ namespace traits +}// \ namespace traits /// Check invariants -template -//BOOST_CONCEPT_REQUIRES((Testable)) -bool check_invariants(const G& a, const G& b) { - G e = traits::identity::value; - typename traits::flavor::type flavor; - return (equal(compose(a, inverse(a)), e)) - && (equal(between(a, b), compose(inverse(a), b))) - && (equal(compose(a, between(a, b)), b)) // - && operator_usage(a, b, flavor); +template +BOOST_CONCEPT_REQUIRES(((Testable)),(bool)) // +check_invariants(const T& a, const T& b, double tol = 1e-9) { + T e = traits::identity::value; + return compose(a, inverse(a)).equals(e, tol) + && between(a, b).equals(compose(inverse(a), b), tol) + && compose(a, between(a, b)).equals(b, tol); } } // \ namespace group /** * Group Concept */ -template +template class IsGroup { public: - typedef typename traits::structure_category::type structure_category_tag; - typedef typename group::traits::identity::value_type identity_value_type; - typedef typename group::traits::flavor::type flavor_tag; + typedef typename traits::structure_category::type structure_category_tag; + typedef typename group::traits::identity::value_type identity_value_type; + typedef typename group::traits::flavor::type flavor_tag; void operator_usage(group::traits::multiplicative_tag) { g = g * h; @@ -159,9 +170,8 @@ public: using group::compose; using group::between; using group::inverse; - BOOST_STATIC_ASSERT( - boost::is_base_of::value); - e = group::traits::identity::value; + BOOST_STATIC_ASSERT( boost::is_base_of::value, "This type's structure_category trait does not assert it as a group (or derived)"); + e = group::traits::identity::value; g = compose(g, h); g = between(g, h); g = inverse(g); @@ -170,42 +180,78 @@ public: private: flavor_tag flavor; - G e, g, h; + T e, g, h; }; -/* - template - class LieGroupConcept : public GroupConcept, public ManifoldConcept { +namespace lie_group { - BOOST_CONCEPT_USAGE(LieGroupConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - } - }; +/** @name Free functions any Group needs to define */ +//@{ +// TODO need Jacobians +//template T compose(const T&g, const T& h); +//template T between(const T&g, const T& h); +//template T inverse(const T&g); +//@} - template - class VectorSpaceConcept : public LieGroupConcept { - typedef typename traits::DefaultChart::type Chart; - typedef typename GroupConcept::identity identity; +namespace traits { - BOOST_CONCEPT_USAGE(VectorSpaceConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - r = p+q; - r = -p; - r = p-q; - } +/** @name Lie Group Traits */ +//@{ +//@} - bool check_invariants(const V& a, const V& b) { - return equal(compose(a, b), a+b) - && equal(inverse(a), -a) - && equal(between(a, b), b-a) - && equal(Chart::retract(a, b), a+b) - && equal(Chart::local(a, b), b-a); - } +}// \ namespace traits - private: - V g,q,r; - }; +/// Check invariants +//template +//BOOST_CONCEPT_REQUIRES(((Testable)),(bool)) check_invariants(const T& a, +// const T& b) { +// bool check_invariants(const V& a, const V& b) { +// return equal(Chart::retract(a, b), a + b) +// && equal(Chart::local(a, b), b - a); +// } +//} +}// \ namespace lie_group + +/** + * Lie Group Concept */ +template +class IsLieGroup: public IsGroup, public IsManifold { +public: + + typedef typename traits::structure_category::type structure_category_tag; + + BOOST_CONCEPT_USAGE(IsLieGroup) { + BOOST_STATIC_ASSERT(boost::is_base_of::value,"This type's trait does not assert it as a Lie group (or derived)"); + // TODO Check with Jacobian +// using lie_group::compose; +// using lie_group::between; +// using lie_group::inverse; +// g = compose(g, h); +// g = between(g, h); +// g = inverse(g); + } +private: + + T g, h; +}; + +template +class IsVectorSpace: public IsLieGroup { +public: + + typedef typename traits::structure_category::type structure_category_tag; + + BOOST_CONCEPT_USAGE(IsVectorSpace) { + BOOST_STATIC_ASSERT(boost::is_base_of::value,"This type's trait does not assert it as a vector space (or derived)"); + r = p + q; + r = -p; + r = p - q; + } + +private: + T p, q, r; +}; } // namespace gtsam diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index f9d4a2d77..3cfa6b2c8 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -81,6 +81,12 @@ TEST(Cyclic, Ivnverse) { EXPECT_LONGS_EQUAL(1, group::inverse(G(5))); } +//****************************************************************************** +TEST(Cyclic , Invariants) { + G g(2), h(5); + group::check_invariants(g,h); +} + //****************************************************************************** int main() { TestResult tr; diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 339897a06..22a96f4a3 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -20,13 +20,47 @@ namespace gtsam { namespace traits { -/// Define Eigen::Quaternion to be a model of the Group concept + +/// Define Eigen::Quaternion to be a model of the Lie Group concept template struct structure_category > { - typedef group_tag type; + typedef lie_group_tag type; }; + } // \namespace gtsam::traits +namespace manifold { + +/// Chart for Eigen Quaternions +template +class QuaternionChart: public manifold::Chart, + QuaternionChart > { + +}; + +namespace traits { + +/// Define the trait that asserts Quaternion manifold has dimension 3 +template +struct dimension > : public boost::integral_constant< + int, 3> { +}; + +/// Define the trait that asserts Quaternion TangentVector is Vector3 +template +struct TangentVector > { + typedef Eigen::Matrix type; +}; + +/// Define the trait that asserts Quaternion TangentVector is Vector3 +template +struct DefaultChart > { + typedef QuaternionChart type; +}; + +} // \namespace gtsam::manifold::traits +} // \namespace gtsam::manifold + namespace group { template @@ -95,7 +129,9 @@ typedef Quaternion Q; // Typedef //****************************************************************************** TEST(Quaternion , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); + // BOOST_CONCEPT_ASSERT((IsGroup)); + // BOOST_CONCEPT_ASSERT((IsManifold)); + // BOOST_CONCEPT_ASSERT((IsLieGroup)); } //****************************************************************************** @@ -103,6 +139,13 @@ TEST(Quaternion , Constructor) { Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } +//****************************************************************************** +TEST(Quaternion , Invariants) { + Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); + Q h(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); + // group::check_invariants(g,h); Does not satisfy Testable concept (yet!) +} + //****************************************************************************** TEST(Quaternion , Compose) { } From 8db8cb54b0ed426e0c98cbc406336b667c35f44a Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 12:35:05 +0100 Subject: [PATCH 6/9] Some chart refinement (early check does not work) --- gtsam/base/concepts.h | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 6caf1703f..2c2a4e07c 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -85,7 +85,7 @@ protected: Chart() { (void) &Local; (void) &Retract; - } // enforce early instantiation. + } // enforce early instantiation. TODO does not seem to work }; } // \ namespace manifold @@ -99,12 +99,19 @@ public: typedef typename manifold::traits::DefaultChart::type DefaultChart; BOOST_CONCEPT_USAGE(IsManifold) { - BOOST_STATIC_ASSERT(boost::is_base_of::value, "This type's structure_category trait does not assert it as a manifold (or derived)"); + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's structure_category trait does not assert it as a manifold (or derived)"); BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); - // no direct usage for manifold since most usage is through a chart + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of, DefaultChart>::value), + "This type's DefaultChart does not derive from manifold::Chart, as required"); + // make sure Derived methods in Chart are defined + v = DefaultChart::local(p,q); + q = DefaultChart::retract(p,v); } private: - T p; + T p,q; TangentVector v; }; @@ -170,7 +177,9 @@ public: using group::compose; using group::between; using group::inverse; - BOOST_STATIC_ASSERT( boost::is_base_of::value, "This type's structure_category trait does not assert it as a group (or derived)"); + 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 = group::traits::identity::value; g = compose(g, h); g = between(g, h); @@ -222,7 +231,9 @@ public: typedef typename traits::structure_category::type structure_category_tag; BOOST_CONCEPT_USAGE(IsLieGroup) { - BOOST_STATIC_ASSERT(boost::is_base_of::value,"This type's trait does not assert it as a Lie group (or derived)"); + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's trait does not assert it as a Lie group (or derived)"); // TODO Check with Jacobian // using lie_group::compose; // using lie_group::between; @@ -243,7 +254,9 @@ public: typedef typename traits::structure_category::type structure_category_tag; BOOST_CONCEPT_USAGE(IsVectorSpace) { - BOOST_STATIC_ASSERT(boost::is_base_of::value,"This type's trait does not assert it as a vector space (or derived)"); + BOOST_STATIC_ASSERT_MSG( + (boost::is_base_of::value), + "This type's trait does not assert it as a vector space (or derived)"); r = p + q; r = -p; r = p - q; From 36da8702f96ddcb26ef626ee1b2514dda8e451c9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 12:35:17 +0100 Subject: [PATCH 7/9] Retract works --- gtsam/geometry/tests/testQuaternion.cpp | 58 ++++++++++++++++++++----- 1 file changed, 46 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 22a96f4a3..8c8f9354f 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -33,9 +33,19 @@ namespace manifold { /// Chart for Eigen Quaternions template -class QuaternionChart: public manifold::Chart, +struct QuaternionChart: public manifold::Chart, QuaternionChart > { - + typedef Eigen::Quaternion Q; + typedef typename traits::TangentVector::type V; + static V local(const Q& p, const Q& q) { + return V(); + } + static Q retract(const Q& p, const V& v) { + double theta = v.norm(); + if (std::abs(theta) < 1e-10) + return p; + return p * Q(Eigen::AngleAxisd(theta, v / theta)); + } }; namespace traits { @@ -49,13 +59,13 @@ struct dimension > : public boost::integral_constant< /// Define the trait that asserts Quaternion TangentVector is Vector3 template struct TangentVector > { - typedef Eigen::Matrix type; + typedef Eigen::Matrix type; }; /// Define the trait that asserts Quaternion TangentVector is Vector3 template struct DefaultChart > { - typedef QuaternionChart type; + typedef QuaternionChart type; }; } // \namespace gtsam::manifold::traits @@ -129,21 +139,45 @@ typedef Quaternion Q; // Typedef //****************************************************************************** TEST(Quaternion , Concept) { - // BOOST_CONCEPT_ASSERT((IsGroup)); - // BOOST_CONCEPT_ASSERT((IsManifold)); - // BOOST_CONCEPT_ASSERT((IsLieGroup)); + BOOST_CONCEPT_ASSERT((IsGroup)); // not strictly needed + BOOST_CONCEPT_ASSERT((IsManifold)); // not strictly needed + BOOST_CONCEPT_ASSERT((IsLieGroup)); } //****************************************************************************** TEST(Quaternion , Constructor) { - Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); + Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); } //****************************************************************************** TEST(Quaternion , Invariants) { - Q g(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); - Q h(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); - // group::check_invariants(g,h); Does not satisfy Testable concept (yet!) + Q q1(Eigen::AngleAxisd(1, Vector3(0, 0, 1))); + Q q2(Eigen::AngleAxisd(2, Vector3(0, 1, 0))); + // group::check_invariants(q1,q2); Does not satisfy Testable concept (yet!) +} + +//****************************************************************************** +TEST(Quaternion , Local) { + Vector3 z_axis(0,0,1); + Q q1(Eigen::AngleAxisd(0,z_axis)); + Q q2(Eigen::AngleAxisd(0.1, z_axis)); + typedef manifold::traits::DefaultChart::type Chart; + Vector3 expected(0,0,0.1); + Vector3 actual = Chart::Local(q1,q2); + cout << expected << endl; + cout << actual << endl; + EXPECT(assert_equal((Vector)expected,actual)); +} + +//****************************************************************************** +TEST(Quaternion , Retract) { + Vector3 z_axis(0,0,1); + Q q(Eigen::AngleAxisd(0,z_axis)); + Q expected(Eigen::AngleAxisd(0.1, z_axis)); + typedef manifold::traits::DefaultChart::type Chart; + Vector3 v(0,0,0.1); + Q actual = Chart::Retract(q,v); + EXPECT(actual.isApprox(expected)); } //****************************************************************************** @@ -155,7 +189,7 @@ TEST(Quaternion , Between) { } //****************************************************************************** -TEST(Quaternion , Ivnverse) { +TEST(Quaternion , Inverse) { } //****************************************************************************** From a31e596448aec80505c6bb6fc4b80c27f0f44f13 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 12:47:26 +0100 Subject: [PATCH 8/9] Working local/Logmap (taken from Rot3Q) --- gtsam/base/concepts.h | 3 ++ gtsam/geometry/tests/testQuaternion.cpp | 72 +++++++++++++++++++------ 2 files changed, 58 insertions(+), 17 deletions(-) diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 2c2a4e07c..0af22171e 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -75,6 +75,9 @@ template struct Chart { typedef T ManifoldType; typedef typename traits::TangentVector::type TangentVector; + + // TODO, maybe we need Retract and Local to be unary, or both + // TOOD, also, this indirection mechanism does not seem to help static TangentVector Local(const ManifoldType& p, const ManifoldType& q) { return Derived::local(p, q); } diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index 8c8f9354f..ef4305948 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -37,14 +37,50 @@ struct QuaternionChart: public manifold::Chart, QuaternionChart > { typedef Eigen::Quaternion Q; typedef typename traits::TangentVector::type V; - static V local(const Q& p, const Q& q) { - return V(); - } - static Q retract(const Q& p, const V& v) { - double theta = v.norm(); + + /// Exponential map, simply be converting omega to AngleAxis + static Q Expmap(const V& omega) { + double theta = omega.norm(); if (std::abs(theta) < 1e-10) - return p; - return p * Q(Eigen::AngleAxisd(theta, v / theta)); + return Q::Identity(); + return Q(Eigen::AngleAxisd(theta, omega / theta)); + } + + /// retract, simply be converting omega to AngleAxis + static Q retract(const Q& p, const V& omega) { + return p * Expmap(omega); + } + + /// We use our own Logmap, as there is a slight bug in Eigen + static V Logmap(const Q& q) { + using std::acos; + using std::sqrt; + static const double twoPi = 2.0 * M_PI, + // define these compile time constants to avoid std::abs: + NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; + + const double qw = q.w(); + if (qw > NearlyOne) { + // Taylor expansion of (angle / s) at 1 + return (2 - 2 * (qw - 1) / 3) * q.vec(); + } else if (qw < NearlyNegativeOne) { + // Angle is zero, return zero vector + return Vector3::Zero(); + } else { + // Normal, away from zero case + double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); + // Important: convert to [-pi,pi] to keep error continuous + if (angle > M_PI) + angle -= twoPi; + else if (angle < -M_PI) + angle += twoPi; + return (angle / s) * q.vec(); + } + } + + /// local is our own, as there is a slight bug in Eigen + static V local(const Q& q1, const Q& q2) { + return Logmap(q1.inverse() * q2); } }; @@ -139,8 +175,10 @@ typedef Quaternion Q; // Typedef //****************************************************************************** TEST(Quaternion , Concept) { - BOOST_CONCEPT_ASSERT((IsGroup)); // not strictly needed - BOOST_CONCEPT_ASSERT((IsManifold)); // not strictly needed + BOOST_CONCEPT_ASSERT((IsGroup)); + // not strictly needed + BOOST_CONCEPT_ASSERT((IsManifold)); + // not strictly needed BOOST_CONCEPT_ASSERT((IsLieGroup)); } @@ -158,12 +196,12 @@ TEST(Quaternion , Invariants) { //****************************************************************************** TEST(Quaternion , Local) { - Vector3 z_axis(0,0,1); - Q q1(Eigen::AngleAxisd(0,z_axis)); + Vector3 z_axis(0, 0, 1); + Q q1(Eigen::AngleAxisd(0, z_axis)); Q q2(Eigen::AngleAxisd(0.1, z_axis)); typedef manifold::traits::DefaultChart::type Chart; - Vector3 expected(0,0,0.1); - Vector3 actual = Chart::Local(q1,q2); + Vector3 expected(0, 0, 0.1); + Vector3 actual = Chart::Local(q1, q2); cout << expected << endl; cout << actual << endl; EXPECT(assert_equal((Vector)expected,actual)); @@ -171,12 +209,12 @@ TEST(Quaternion , Local) { //****************************************************************************** TEST(Quaternion , Retract) { - Vector3 z_axis(0,0,1); - Q q(Eigen::AngleAxisd(0,z_axis)); + Vector3 z_axis(0, 0, 1); + Q q(Eigen::AngleAxisd(0, z_axis)); Q expected(Eigen::AngleAxisd(0.1, z_axis)); typedef manifold::traits::DefaultChart::type Chart; - Vector3 v(0,0,0.1); - Q actual = Chart::Retract(q,v); + Vector3 v(0, 0, 0.1); + Q actual = Chart::Retract(q, v); EXPECT(actual.isApprox(expected)); } From cdc0029158df2a7a2631fb5fd98a55298763356c Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 7 Dec 2014 13:01:25 +0100 Subject: [PATCH 9/9] Reverted on Chart base class. But no chart-specific traits needed, as assumed created by us. --- GTSAM-Concepts.md | 24 ----------- gtsam/base/concepts.h | 53 +++++++++++-------------- gtsam/geometry/tests/testQuaternion.cpp | 24 +++++------ 3 files changed, 35 insertions(+), 66 deletions(-) diff --git a/GTSAM-Concepts.md b/GTSAM-Concepts.md index 77c1621bf..1e7960b56 100644 --- a/GTSAM-Concepts.md +++ b/GTSAM-Concepts.md @@ -146,30 +146,6 @@ TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts. and we also define a limited number of `gtsam::tags` to select the correct implementation of certain functions at compile time (tag dispatching). Charts are done more conventionally, so we start there... -Interfaces ----------- - -Because Charts are always written by the user (or automatically generated, see below for vector spaces), -we enforce the Chart concept using an abstract base class, acting as an interface: - -``` -#!c++ -template -struct Chart { - typedef T ManifoldType; - typedef typename traits::TangentVector::type TangentVector; - static TangentVector Local(const ManifoldType& p, const ManifoldType& q) {return Derived::local(p,q);} - static ManifoldType Retract(const ManifoldType& p, const TangentVector& v) {return Derived::retract(p,v);} - protected: - Chart(){ (void)&Local; (void)&Retract; } // enforce early instantiation. -} -``` - -The [CRTP](http://en.wikipedia.org/wiki/Curiously_recurring_template_pattern) and the protected constructor -automatically check for the existence of the methods in the Derived class, whenever a new Chart is created by - - struct MyChart : Chart { ... } - Traits ------ diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 0af22171e..3f1222cec 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -67,32 +67,30 @@ check_invariants(const T& a, const T& b) { return true; } -/** - * Base class for Charts - * Derived has to implement local and retract as static methods - */ -template -struct Chart { - typedef T ManifoldType; - typedef typename traits::TangentVector::type TangentVector; - - // TODO, maybe we need Retract and Local to be unary, or both - // TOOD, also, this indirection mechanism does not seem to help - static TangentVector Local(const ManifoldType& p, const ManifoldType& q) { - return Derived::local(p, q); - } - static ManifoldType Retract(const ManifoldType& p, const TangentVector& v) { - return Derived::retract(p, v); - } -protected: - Chart() { - (void) &Local; - (void) &Retract; - } // enforce early instantiation. TODO does not seem to work -}; - } // \ namespace manifold +/** + * Chart concept + */ +template +class IsChart { +public: + typedef typename T::ManifoldType ManifoldType; + typedef typename manifold::traits::TangentVector::type V; + + BOOST_CONCEPT_USAGE(IsChart) { + // make sure Derived methods in Chart are defined + v = T::Local(p,q); + q = T::Retract(p,v); + } +private: + ManifoldType p,q; + V v; +}; + +/** + * Manifold concept + */ template class IsManifold { public: @@ -106,12 +104,7 @@ public: (boost::is_base_of::value), "This type's structure_category trait does not assert it as a manifold (or derived)"); BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim); - BOOST_STATIC_ASSERT_MSG( - (boost::is_base_of, DefaultChart>::value), - "This type's DefaultChart does not derive from manifold::Chart, as required"); - // make sure Derived methods in Chart are defined - v = DefaultChart::local(p,q); - q = DefaultChart::retract(p,v); + BOOST_CONCEPT_ASSERT((IsChart)); } private: T p,q; diff --git a/gtsam/geometry/tests/testQuaternion.cpp b/gtsam/geometry/tests/testQuaternion.cpp index ef4305948..89dcd3024 100644 --- a/gtsam/geometry/tests/testQuaternion.cpp +++ b/gtsam/geometry/tests/testQuaternion.cpp @@ -33,13 +33,17 @@ namespace manifold { /// Chart for Eigen Quaternions template -struct QuaternionChart: public manifold::Chart, - QuaternionChart > { - typedef Eigen::Quaternion Q; - typedef typename traits::TangentVector::type V; +struct QuaternionChart { + + // required + typedef Eigen::Quaternion ManifoldType; + + // internal + typedef ManifoldType Q; + typedef typename traits::TangentVector::type Omega; /// Exponential map, simply be converting omega to AngleAxis - static Q Expmap(const V& omega) { + static Q Expmap(const Omega& omega) { double theta = omega.norm(); if (std::abs(theta) < 1e-10) return Q::Identity(); @@ -47,12 +51,12 @@ struct QuaternionChart: public manifold::Chart, } /// retract, simply be converting omega to AngleAxis - static Q retract(const Q& p, const V& omega) { + static Q Retract(const Q& p, const Omega& omega) { return p * Expmap(omega); } /// We use our own Logmap, as there is a slight bug in Eigen - static V Logmap(const Q& q) { + static Omega Logmap(const Q& q) { using std::acos; using std::sqrt; static const double twoPi = 2.0 * M_PI, @@ -79,7 +83,7 @@ struct QuaternionChart: public manifold::Chart, } /// local is our own, as there is a slight bug in Eigen - static V local(const Q& q1, const Q& q2) { + static Omega Local(const Q& q1, const Q& q2) { return Logmap(q1.inverse() * q2); } }; @@ -176,9 +180,7 @@ typedef Quaternion Q; // Typedef //****************************************************************************** TEST(Quaternion , Concept) { BOOST_CONCEPT_ASSERT((IsGroup)); - // not strictly needed BOOST_CONCEPT_ASSERT((IsManifold)); - // not strictly needed BOOST_CONCEPT_ASSERT((IsLieGroup)); } @@ -202,8 +204,6 @@ TEST(Quaternion , Local) { typedef manifold::traits::DefaultChart::type Chart; Vector3 expected(0, 0, 0.1); Vector3 actual = Chart::Local(q1, q2); - cout << expected << endl; - cout << actual << endl; EXPECT(assert_equal((Vector)expected,actual)); }