Added Manifold, Lie Group, and Vector Space concepts back in

gtsam/base/concepts.h

@@ -13,6 +13,7 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Testable.h>
 #include <boost/concept_check.hpp>
+#include <boost/concept/requires.hpp>
 #include <boost/static_assert.hpp>
 #include <boost/type_traits/is_base_of.hpp>
 
@@ -25,9 +26,7 @@ namespace traits {
  * @brief Associate a unique tag with each of the main GTSAM concepts
  */
 //@{
-template<typename T>
+template<typename T> struct structure_category;
-struct structure_category;
-// specializations should be derived from one of the following tags
 //@}
 
 /**
@@ -35,81 +34,95 @@ struct structure_category;
  * @brief Possible values for traits::structure_category<T>::type
  */
 //@{
-struct manifold_tag {
+struct manifold_tag {};
-};
+struct group_tag {};
-struct group_tag {
+struct lie_group_tag: public manifold_tag, public group_tag {};
-};
+struct vector_space_tag: public lie_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<typename Manifold> struct dimension;
 template<typename Manifold> struct TangentVector;
 template<typename Manifold> struct DefaultChart;
 //@}
 
-}// namespace traits
+}// \ namespace traits
 
-/*
+/// Check invariants for Manifold type
- template<typename T>
+template<typename T>
- class ManifoldConcept {
+BOOST_CONCEPT_REQUIRES(((Testable<T>)),(bool)) //
- public:
+check_invariants(const T& a, const T& b) {
- typedef T Manifold;
+  typedef typename traits::DefaultChart<T>::type Chart;
- typedef typename traits::TangentVector<T>::type TangentVector;
+  return true;
- 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> >);
+ * Base class for Charts
- BOOST_STATIC_ASSERT(TangentVector::SizeAtCompileTime == dim);
+ * Derived has to implement local and retract as static methods
- // no direct usage for manifold since most usage is through a chart
- }
- private:
- Manifold p;
- TangentVector v;
- };
-
- template<typename C>
- class ChartConcept {
- public:
- 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
- }
-
- private:
- Manifold p,q;
- TangentVector v;
-
- };
  */
+template <class T, class Derived>
+struct Chart {
+  typedef T ManifoldType;
+  typedef typename traits::TangentVector<T>::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<typename T>
+class IsManifold {
+public:
+  typedef typename traits::structure_category<T>::type structure_category_tag;
+  static const size_t dim = manifold::traits::dimension<T>::value;
+  typedef typename manifold::traits::TangentVector<T>::type TangentVector;
+  typedef typename manifold::traits::DefaultChart<T>::type DefaultChart;
+
+  BOOST_CONCEPT_USAGE(IsManifold) {
+    BOOST_STATIC_ASSERT(boost::is_base_of<traits::manifold_tag, structure_category_tag>::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<typename G> G compose(const G&g, const G& h);
+template<typename T> T compose(const T&g, const T& h);
-template<typename G> G between(const G&g, const G& h);
+template<typename T> T between(const T&g, const T& h);
-template<typename G> G inverse(const G&g);
+template<typename T> T inverse(const T&g);
 //@}
 
 namespace traits {
 
 /** @name Group Traits */
 //@{
-template<typename G> struct identity;
+template<typename T> struct identity;
-template<typename G> struct flavor;
+template<typename T> struct flavor;
 //@}
 
 /** @name Group Flavor Tags */
@@ -120,31 +133,29 @@ struct multiplicative_tag {
 };
 //@}
 
-} // \ namespace traits
+}// \ namespace traits
 
 /// Check invariants
-template <typename G>
+template<typename T>
-//BOOST_CONCEPT_REQUIRES((Testable))
+BOOST_CONCEPT_REQUIRES(((Testable<T>)),(bool)) //
-bool check_invariants(const G& a, const G& b) {
+check_invariants(const T& a, const T& b, double tol = 1e-9) {
-  G e = traits::identity<G>::value;
+  T e = traits::identity<T>::value;
-  typename traits::flavor<G>::type flavor;
+  return compose(a, inverse(a)).equals(e, tol)
-  return (equal(compose(a, inverse(a)), e))
+      && between(a, b).equals(compose(inverse(a), b), tol)
-      && (equal(between(a, b), compose(inverse(a), b)))
+      && compose(a, between(a, b)).equals<T>(b, tol);
-      && (equal(compose(a, between(a, b)), b)) //
-      && operator_usage(a, b, flavor);
 }
 } // \ namespace group
 
 /**
  * Group Concept
  */
-template<typename G>
+template<typename T>
 class IsGroup {
 public:
 
-  typedef typename traits::structure_category<G>::type structure_category_tag;
+  typedef typename traits::structure_category<T>::type structure_category_tag;
-  typedef typename group::traits::identity<G>::value_type identity_value_type;
+  typedef typename group::traits::identity<T>::value_type identity_value_type;
-  typedef typename group::traits::flavor<G>::type flavor_tag;
+  typedef typename group::traits::flavor<T>::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_STATIC_ASSERT( boost::is_base_of<traits::group_tag, structure_category_tag>::value, "This type's structure_category trait does not assert it as a group (or derived)");
-        boost::is_base_of<traits::group_tag, structure_category_tag>::value);
+    e = group::traits::identity<T>::value;
-    e = group::traits::identity<G>::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;
 };
 
-/*
+namespace lie_group {
- template <typename L>
- class LieGroupConcept : public GroupConcept<L>, public ManifoldConcept<L> {
 
- BOOST_CONCEPT_USAGE(LieGroupConcept) {
+/** @name Free functions any Group needs to define */
- BOOST_STATIC_ASSERT(boost::is_base_of<traits::lie_group_tag, traits::structure<L> >);
+//@{
- }
+// TODO need Jacobians
- };
+//template<typename T> T compose(const T&g, const T& h);
+//template<typename T> T between(const T&g, const T& h);
+//template<typename T> T inverse(const T&g);
+//@}
 
- template <typename V>
+namespace traits {
- class VectorSpaceConcept : public LieGroupConcept {
- typedef typename traits::DefaultChart<V>::type Chart;
- typedef typename GroupConcept<V>::identity identity;
 
- BOOST_CONCEPT_USAGE(VectorSpaceConcept) {
+/** @name Lie Group Traits */
- 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) {
+}// \ namespace traits
- 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:
+/// Check invariants
- V g,q,r;
+//template<typename T>
- };
+//BOOST_CONCEPT_REQUIRES(((Testable<T>)),(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<typename T>
+class IsLieGroup: public IsGroup<T>, public IsManifold<T> {
+public:
+
+  typedef typename traits::structure_category<T>::type structure_category_tag;
+
+  BOOST_CONCEPT_USAGE(IsLieGroup) {
+    BOOST_STATIC_ASSERT(boost::is_base_of<traits::lie_group_tag, structure_category_tag>::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<typename T>
+class IsVectorSpace: public IsLieGroup<T> {
+public:
+
+  typedef typename traits::structure_category<T>::type structure_category_tag;
+
+  BOOST_CONCEPT_USAGE(IsVectorSpace) {
+    BOOST_STATIC_ASSERT(boost::is_base_of<traits::vector_space_tag, structure_category_tag>::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
 

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;

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<typename S, int O>
 struct structure_category<Eigen::Quaternion<S, O> > {
-  typedef group_tag type;
+  typedef lie_group_tag type;
 };
+
 } // \namespace gtsam::traits
 
+namespace manifold {
+
+/// Chart for Eigen Quaternions
+template<typename S, int O>
+class QuaternionChart: public manifold::Chart<Eigen::Quaternion<S, O>,
+    QuaternionChart<S, O> > {
+
+};
+
+namespace traits {
+
+/// Define the trait that asserts Quaternion manifold has dimension 3
+template<typename S, int O>
+struct dimension<Eigen::Quaternion<S, O> > : public boost::integral_constant<
+    int, 3> {
+};
+
+/// Define the trait that asserts Quaternion TangentVector is Vector3
+template<typename S, int O>
+struct TangentVector<Eigen::Quaternion<S, O> > {
+  typedef Eigen::Matrix<S,3,1,O,3,1> type;
+};
+
+/// Define the trait that asserts Quaternion TangentVector is Vector3
+template<typename S, int O>
+struct DefaultChart<Eigen::Quaternion<S, O> > {
+  typedef QuaternionChart<S,O> type;
+};
+
+} // \namespace gtsam::manifold::traits
+} // \namespace gtsam::manifold
+
 namespace group {
 
 template<typename S, int O>
@@ -95,7 +129,9 @@ typedef Quaternion Q; // Typedef
 
 //******************************************************************************
 TEST(Quaternion , Concept) {
-  BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >));
+  //  BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >));
+  //  BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >));
+  //  BOOST_CONCEPT_ASSERT((IsLieGroup<Quaternion >));
 }
 
 //******************************************************************************
@@ -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) {
 }