Merge branch 'feature/groups' into feature/tighteningTraits
						commit
						2e86da1096
					
				|  | @ -23,8 +23,9 @@ namespace traits { | |||
|  * @brief Associate a unique tag with each of the main GTSAM concepts | ||||
|  */ | ||||
| //@{
 | ||||
| template <class T> | ||||
| struct structure_category {}; // specializations should be derived from one of the following tags
 | ||||
| template<class T> | ||||
| 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<T>::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 <class Manifold> struct TangentVector; | ||||
| template <class Manifold> struct DefaultChart; | ||||
| template<class Manifold> struct TangentVector; | ||||
| template<class Manifold> struct DefaultChart; | ||||
| //@}
 | ||||
| 
 | ||||
| } // namespace traits
 | ||||
| }// namespace traits
 | ||||
| 
 | ||||
| /*
 | ||||
| template<class T> | ||||
| class ManifoldConcept { | ||||
|  template<class T> | ||||
|  class ManifoldConcept { | ||||
|  public: | ||||
|   typedef T Manifold; | ||||
|   typedef typename traits::TangentVector<T>::type TangentVector; | ||||
|   typedef typename traits::DefaultChart<T>::type DefaultChart; | ||||
|   static const size_t dim = traits::dimension<T>::value; | ||||
|  typedef T Manifold; | ||||
|  typedef typename traits::TangentVector<T>::type TangentVector; | ||||
|  typedef typename traits::DefaultChart<T>::type DefaultChart; | ||||
|  static const size_t dim = traits::dimension<T>::value; | ||||
| 
 | ||||
|   BOOST_CONCEPT_USAGE(ManifoldConcept) { | ||||
|     BOOST_STATIC_ASSERT(boost::is_base_of<traits::manifold_tag, traits::structure<Manifold> >); | ||||
|     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<traits::manifold_tag, traits::structure<Manifold> >); | ||||
|  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 C> | ||||
| class ChartConcept { | ||||
|  template<class C> | ||||
|  class ChartConcept { | ||||
|  public: | ||||
|   typedef C Chart; | ||||
|   typedef typename traits::Manifold<Chart>::type Manifold; | ||||
|   typedef typename traits::TangentVector<Manifold>::type TangentVector; | ||||
|  typedef C Chart; | ||||
|  typedef typename traits::Manifold<Chart>::type Manifold; | ||||
|  typedef typename traits::TangentVector<Manifold>::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<typename G> G compose(const G&g, const G& h); | ||||
| template<typename G> G between(const G&g, const G& h); | ||||
| template<typename G> G inverse(const G&g); | ||||
| //@}
 | ||||
| 
 | ||||
| namespace traits { | ||||
| 
 | ||||
| /** @name Group Traits */ | ||||
| //@{
 | ||||
| template <class Group> struct identity {}; | ||||
| template <class Group> struct group_flavor {}; | ||||
| template<class G> struct identity; | ||||
| template<class G> 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 G> | ||||
| class GroupConcept { | ||||
|  public: | ||||
|   typedef G Group; | ||||
|   static const Group identity = traits::identity<G>::value; | ||||
| /**
 | ||||
|  * Group Concept | ||||
|  */ | ||||
| template<typename G> | ||||
| class Group { | ||||
| public: | ||||
| 
 | ||||
|   BOOST_CONCEPT_USAGE(GroupConcept) { | ||||
|     BOOST_STATIC_ASSERT(boost::is_base_of<traits::group_tag,typename traits::structure_category<Group>::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<Group>::type); | ||||
|   typedef typename traits::structure_category<G>::type structure_category_tag; | ||||
|   typedef typename group::traits::identity<G>::value_type identity_value_type; | ||||
|   typedef typename group::traits::flavor<G>::type flavor_tag; | ||||
| 
 | ||||
|   BOOST_CONCEPT_USAGE(Group) { | ||||
|     using group::compose; | ||||
|     using group::between; | ||||
|     using group::inverse; | ||||
|     BOOST_STATIC_ASSERT( | ||||
|         boost::is_base_of<traits::group_tag, structure_category_tag>::value); | ||||
|     e = group::traits::identity<G>::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<Group>::type); | ||||
|   } | ||||
| // TODO: these all require default constructors :-(
 | ||||
| // Also, requires equal which is not required of a group
 | ||||
| //  Group():e(group::traits::identity<G>::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 L> | ||||
| class LieGroupConcept : public GroupConcept<L>, public ManifoldConcept<L> { | ||||
|  template <class L> | ||||
|  class LieGroupConcept : public GroupConcept<L>, public ManifoldConcept<L> { | ||||
| 
 | ||||
|   BOOST_CONCEPT_USAGE(LieGroupConcept) { | ||||
|     BOOST_STATIC_ASSERT(boost::is_base_of<traits::lie_group_tag, traits::structure<L> >); | ||||
|   } | ||||
| }; | ||||
|  BOOST_CONCEPT_USAGE(LieGroupConcept) { | ||||
|  BOOST_STATIC_ASSERT(boost::is_base_of<traits::lie_group_tag, traits::structure<L> >); | ||||
|  } | ||||
|  }; | ||||
| 
 | ||||
| template <class V> | ||||
| class VectorSpaceConcept : public LieGroupConcept { | ||||
|   typedef typename traits::DefaultChart<V>::type Chart; | ||||
|   typedef typename GroupConcept<V>::identity identity; | ||||
|  template <class V> | ||||
|  class VectorSpaceConcept : public LieGroupConcept { | ||||
|  typedef typename traits::DefaultChart<V>::type Chart; | ||||
|  typedef typename GroupConcept<V>::identity identity; | ||||
| 
 | ||||
|   BOOST_CONCEPT_USAGE(VectorSpaceConcept) { | ||||
|     BOOST_STATIC_ASSERT(boost::is_base_of<traits::vector_space_tag, traits::structure<L> >); | ||||
|     r = p+q; | ||||
|     r = -p; | ||||
|     r = p-q; | ||||
|   } | ||||
|  BOOST_CONCEPT_USAGE(VectorSpaceConcept) { | ||||
|  BOOST_STATIC_ASSERT(boost::is_base_of<traits::vector_space_tag, traits::structure<L> >); | ||||
|  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
 | ||||
| 
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -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 <gtsam/geometry/Cyclic.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| namespace group { | ||||
| namespace traits { | ||||
| 
 | ||||
| } // \namespace traits
 | ||||
| } // \namespace group
 | ||||
| } // \namespace gtsam
 | ||||
| 
 | ||||
|  | @ -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 <gtsam/base/concepts.h> | ||||
| #include <cstddef> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| template<size_t N> | ||||
| 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<size_t N> struct structure_category<Cyclic<N> > { | ||||
|   typedef group_tag type; | ||||
| }; | ||||
| } // \namespace gtsam::traits
 | ||||
| 
 | ||||
| namespace group { | ||||
| 
 | ||||
| template<size_t N> | ||||
| Cyclic<N> compose(const Cyclic<N>&g, const Cyclic<N>& h) { | ||||
|   return g + h; | ||||
| } | ||||
| 
 | ||||
| template<size_t N> | ||||
| Cyclic<N> between(const Cyclic<N>&g, const Cyclic<N>& h) { | ||||
|   return h - g; | ||||
| } | ||||
| 
 | ||||
| template<size_t N> | ||||
| Cyclic<N> inverse(const Cyclic<N>&g) { | ||||
|   return -g; | ||||
| } | ||||
| 
 | ||||
| namespace traits { | ||||
| 
 | ||||
| /// Define the trait that specifies Cyclic's identity element
 | ||||
| template<size_t N> struct identity<Cyclic<N> > { | ||||
|   static const Cyclic<N> value = Cyclic<N>(0); | ||||
|   typedef Cyclic<N> value_type; | ||||
| }; | ||||
| 
 | ||||
| /// Define the trait that asserts Cyclic is an additive group
 | ||||
| template<size_t N> struct flavor<Cyclic<N> > { | ||||
|   typedef additive_tag type; | ||||
| }; | ||||
| 
 | ||||
| } // \namespace gtsam::group::traits
 | ||||
| } // \namespace gtsam::group
 | ||||
| } // \namespace gtsam
 | ||||
| 
 | ||||
|  | @ -15,53 +15,76 @@ | |||
|  * @author Frank Dellaert | ||||
|  **/ | ||||
| 
 | ||||
| #include <gtsam/base/concepts.h> | ||||
| #include <cstddef> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| template<size_t N> | ||||
| class Cyclic { | ||||
|   size_t i_; | ||||
| public: | ||||
|   static const Cyclic Identity = Cyclic(0); | ||||
|   Cyclic(size_t i) : | ||||
|       i_(i) { | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
| namespace traits { | ||||
| template<size_t N> struct identity<Cyclic<N> > { | ||||
|   static const Cyclic<N> value = Cyclic<N>::Identity; | ||||
|   typedef Cyclic<N> value_type; | ||||
| }; | ||||
| template<size_t N> struct group_flavor<Cyclic<N> > { | ||||
|   typedef additive_group_tag type; | ||||
| }; | ||||
| } // \namespace traits
 | ||||
| 
 | ||||
| } // \namespace gtsam
 | ||||
| 
 | ||||
| //#include <gtsam/geometry/Cyclic.h>
 | ||||
| #include <gtsam/geometry/Cyclic.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| 
 | ||||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| BOOST_CONCEPT_ASSERT((GroupConcept<Cyclic<6> >)); | ||||
| typedef Cyclic<6> G; // Let's use the cyclic group of order 6
 | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| //******************************************************************************
 | ||||
| TEST(Cyclic, Concept) { | ||||
|   BOOST_CONCEPT_ASSERT((Group<G>)); | ||||
|   EXPECT_LONGS_EQUAL(0, group::traits::identity<G>::value); | ||||
|   G g(2), h(3); | ||||
|   // EXPECT(Group<G>().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); | ||||
| } | ||||
| /* ************************************************************************* */ | ||||
| //******************************************************************************
 | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue