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))); } //******************************************************************************