diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h index 967a91707..9011c759b 100644 --- a/gtsam/base/concepts.h +++ b/gtsam/base/concepts.h @@ -23,8 +23,9 @@ 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; +// specializations should be derived from one of the following tags //@} /** @@ -32,147 +33,179 @@ struct structure_category {}; // specializations should be derived from one of t * @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 traits namespace traits { /** @name Manifold Traits */ //@{ -template struct TangentVector; -template struct DefaultChart; +template struct TangentVector; +template struct DefaultChart; //@} -} // namespace traits +}// namespace traits /* -template -class ManifoldConcept { + 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; + typedef T Manifold; + typedef typename traits::TangentVector::type TangentVector; + typedef typename traits::DefaultChart::type DefaultChart; + static const size_t dim = traits::dimension::value; - 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 - } + 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; -}; + Manifold p; + TangentVector v; + }; -template -class ChartConcept { + template + class ChartConcept { public: - typedef C Chart; - typedef typename traits::Manifold::type Manifold; - typedef typename traits::TangentVector::type TangentVector; + 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 - } + 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; + Manifold p,q; + 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); +//@} namespace traits { /** @name Group Traits */ //@{ -template struct identity {}; -template struct group_flavor {}; +template struct identity; +template struct flavor; //@} /** @name Group Flavor Tags */ //@{ -struct additive_group_tag {}; -struct multiplicative_group_tag {}; +struct additive_tag { +}; +struct multiplicative_tag { +}; //@} -} // namespace traits +} // \ namespace traits +} // \ namespace group -template -class GroupConcept { - public: - typedef G Group; - static const Group identity = traits::identity::value; +/** + * Group Concept + */ +template +class Group { +public: - BOOST_CONCEPT_USAGE(GroupConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of::type>::value ); - Group ip = inverse(p); - Group pq = compose(p, q); - Group d = between(p, q); - test = equal(p, q); - test2 = operator_usage(p, q, traits::group_flavor::type); + 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) { + 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); } - bool check_invariants(const Group& a, const Group& b) { - return (equal(compose(a, inverse(a)), identity)) - && (equal(between(a, b), compose(inverse(a), b))) - && (equal(compose(a, between(a, b)), b)) - && operator_usage(a, b, traits::group_flavor::type); - } +// 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: - Group p,q; +private: + flavor_tag flavor; + G e, g, h, gh, ig, d; bool test, test2; - bool operator_usage(const Group& a, const Group& b, const traits::multiplicative_group_tag&) { - return equal(compose(a, b), a*b); + bool operator_usage(const G& a, const G& b, + group::traits::multiplicative_tag) { + return group::compose(a, b) == a * b; } - bool operator_usage(const Group& a, const Group& b, const traits::additive_group_tag&) { - return equal(compose(a, b), a+b); + bool operator_usage(const G& a, const G& b, group::traits::additive_tag) { + return group::compose(a, b) == a + b; } }; + /* -template -class LieGroupConcept : public GroupConcept, public ManifoldConcept { + template + class LieGroupConcept : public GroupConcept, public ManifoldConcept { - BOOST_CONCEPT_USAGE(LieGroupConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - } -}; + BOOST_CONCEPT_USAGE(LieGroupConcept) { + BOOST_STATIC_ASSERT(boost::is_base_of >); + } + }; -template -class VectorSpaceConcept : public LieGroupConcept { - typedef typename traits::DefaultChart::type Chart; - typedef typename GroupConcept::identity identity; + template + class VectorSpaceConcept : public LieGroupConcept { + typedef typename traits::DefaultChart::type Chart; + typedef typename GroupConcept::identity identity; - BOOST_CONCEPT_USAGE(VectorSpaceConcept) { - BOOST_STATIC_ASSERT(boost::is_base_of >); - r = p+q; - r = -p; - r = p-q; - } + BOOST_CONCEPT_USAGE(VectorSpaceConcept) { + BOOST_STATIC_ASSERT(boost::is_base_of >); + r = p+q; + r = -p; + r = p-q; + } - 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); - } + 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); + } private: - V p,q,r; -}; -*/ + V g,q,r; + }; + */ } // namespace gtsam - diff --git a/gtsam/geometry/Cyclic.cpp b/gtsam/geometry/Cyclic.cpp new file mode 100644 index 000000000..2b14ec6ba --- /dev/null +++ b/gtsam/geometry/Cyclic.cpp @@ -0,0 +1,27 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cyclic.cpp + * @brief Cyclic group implementation + * @author Frank Dellaert + **/ + +#include + +namespace gtsam { +namespace group { +namespace traits { + +} // \namespace traits +} // \namespace group +} // \namespace gtsam + diff --git a/gtsam/geometry/Cyclic.h b/gtsam/geometry/Cyclic.h new file mode 100644 index 000000000..85602d12d --- /dev/null +++ b/gtsam/geometry/Cyclic.h @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cyclic.h + * @brief Cyclic group, i.e., the integers modulo N + * @author Frank Dellaert + **/ + +#include +#include + +namespace gtsam { + +template +class Cyclic { + size_t i_; ///< we just use an unsigned int +public: + /// Constructor + Cyclic(size_t i) : + i_(i) { + } + /// Cast to size_t + operator size_t() const { + return i_; + } + /// Addition modulo N + Cyclic operator+(const Cyclic& h) const { + return (i_ + h.i_) % N; + } + /// Subtraction modulo N + Cyclic operator-(const Cyclic& h) const { + return (N + i_ - h.i_) % N; + } + /// Inverse + Cyclic operator-() const { + return (N - i_) % N; + } +}; + +namespace traits { +/// Define Cyclic to be a model of the Group concept +template struct structure_category > { + typedef group_tag type; +}; +} // \namespace gtsam::traits + +namespace group { + +template +Cyclic compose(const Cyclic&g, const Cyclic& h) { + return g + h; +} + +template +Cyclic between(const Cyclic&g, const Cyclic& h) { + return h - g; +} + +template +Cyclic inverse(const Cyclic&g) { + return -g; +} + +namespace traits { + +/// Define the trait that specifies Cyclic's identity element +template struct identity > { + static const Cyclic value = Cyclic(0); + typedef Cyclic value_type; +}; + +/// Define the trait that asserts Cyclic is an additive group +template struct flavor > { + typedef additive_tag type; +}; + +} // \namespace gtsam::group::traits +} // \namespace gtsam::group +} // \namespace gtsam + diff --git a/gtsam/geometry/tests/testCyclic.cpp b/gtsam/geometry/tests/testCyclic.cpp index f15654b2e..00ea2c853 100644 --- a/gtsam/geometry/tests/testCyclic.cpp +++ b/gtsam/geometry/tests/testCyclic.cpp @@ -15,53 +15,76 @@ * @author Frank Dellaert **/ -#include -#include - -namespace gtsam { - -template -class Cyclic { - size_t i_; -public: - static const Cyclic Identity = Cyclic(0); - Cyclic(size_t i) : - i_(i) { - } -}; - -namespace traits { -template struct identity > { - static const Cyclic value = Cyclic::Identity; - typedef Cyclic value_type; -}; -template struct group_flavor > { - typedef additive_group_tag type; -}; -} // \namespace traits - -} // \namespace gtsam - -//#include +#include #include #include using namespace std; using namespace gtsam; -BOOST_CONCEPT_ASSERT((GroupConcept >)); +typedef Cyclic<6> G; // Let's use the cyclic group of order 6 -/* ************************************************************************* */ +//****************************************************************************** +TEST(Cyclic, Concept) { + BOOST_CONCEPT_ASSERT((Group)); + EXPECT_LONGS_EQUAL(0, group::traits::identity::value); + G g(2), h(3); + // EXPECT(Group().check_invariants(g,h)) +} + +//****************************************************************************** TEST(Cyclic, Constructor) { -Cyclic<6> g(0); -// EXPECT(assert_equal(p1, p2)); -// EXPECT_LONGS_EQUAL(2,offset2.size()); + G g(0); } -/* ************************************************************************* */ +//****************************************************************************** +TEST(Cyclic, Compose) { + EXPECT_LONGS_EQUAL(0, group::compose(G(0),G(0))); + EXPECT_LONGS_EQUAL(1, group::compose(G(0),G(1))); + EXPECT_LONGS_EQUAL(2, group::compose(G(0),G(2))); + EXPECT_LONGS_EQUAL(3, group::compose(G(0),G(3))); + EXPECT_LONGS_EQUAL(4, group::compose(G(0),G(4))); + EXPECT_LONGS_EQUAL(5, group::compose(G(0),G(5))); + + EXPECT_LONGS_EQUAL(2, group::compose(G(2),G(0))); + EXPECT_LONGS_EQUAL(3, group::compose(G(2),G(1))); + EXPECT_LONGS_EQUAL(4, group::compose(G(2),G(2))); + EXPECT_LONGS_EQUAL(5, group::compose(G(2),G(3))); + EXPECT_LONGS_EQUAL(0, group::compose(G(2),G(4))); + EXPECT_LONGS_EQUAL(1, group::compose(G(2),G(5))); +} + +//****************************************************************************** +TEST(Cyclic, Between) { + EXPECT_LONGS_EQUAL(0, group::between(G(0),G(0))); + EXPECT_LONGS_EQUAL(1, group::between(G(0),G(1))); + EXPECT_LONGS_EQUAL(2, group::between(G(0),G(2))); + EXPECT_LONGS_EQUAL(3, group::between(G(0),G(3))); + EXPECT_LONGS_EQUAL(4, group::between(G(0),G(4))); + EXPECT_LONGS_EQUAL(5, group::between(G(0),G(5))); + + EXPECT_LONGS_EQUAL(4, group::between(G(2),G(0))); + EXPECT_LONGS_EQUAL(5, group::between(G(2),G(1))); + EXPECT_LONGS_EQUAL(0, group::between(G(2),G(2))); + EXPECT_LONGS_EQUAL(1, group::between(G(2),G(3))); + EXPECT_LONGS_EQUAL(2, group::between(G(2),G(4))); + EXPECT_LONGS_EQUAL(3, group::between(G(2),G(5))); +} + +//****************************************************************************** +TEST(Cyclic, Ivnverse) { + EXPECT_LONGS_EQUAL(0, group::inverse(G(0))); + EXPECT_LONGS_EQUAL(5, group::inverse(G(1))); + EXPECT_LONGS_EQUAL(4, group::inverse(G(2))); + EXPECT_LONGS_EQUAL(3, group::inverse(G(3))); + EXPECT_LONGS_EQUAL(2, group::inverse(G(4))); + EXPECT_LONGS_EQUAL(1, group::inverse(G(5))); +} + +//****************************************************************************** int main() { -TestResult tr; -return TestRegistry::runAllTests(tr); + TestResult tr; + return TestRegistry::runAllTests(tr); } -/* ************************************************************************* */ +//******************************************************************************