Eigen::Quaternions now satisfy IsManifold, IsGroup, and IsLieGroup! Also reverted back to a simpler treatment of Charts.

Merge branch 'traits/quaternion' into feature/tighteningTraits

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 <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. 
-}
-```
-
-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<MyType,MyChart> { ... }
-
 Traits
 ------
 

gtsam/base/Testable.h

@@ -34,6 +34,7 @@
 #pragma once
 
 #include <boost/shared_ptr.hpp>
+#include <boost/concept_check.hpp>
 #include <stdio.h>
 #include <string>
 
@@ -50,17 +51,20 @@ namespace gtsam {
    * @tparam T is the type this constrains to be testable - assumes print() and equals()
    */
   template <class T>
-  class TestableConcept {
+  class Testable {
-    static bool checkTestableConcept(const T& d) {
+    T t;
+    bool r1,r2;
+  public:
+
+    BOOST_CONCEPT_USAGE(Testable) {
       // check print function, with optional string
-      d.print(std::string());
+      t.print(std::string());
-      d.print();
+      t.print();
 
       // check print, with optional threshold
       double tol = 1.0;
-      bool r1 = d.equals(d, tol);
+      r1 = t.equals(t, tol);
-      bool r2 = d.equals(d);
+      r2 = t.equals(t);
-      return r1 && r2;
     }
   };
 
@@ -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<T>;
+#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::Testable<T>;
-#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::TestableConcept<T> _gtsam_TestableConcept_##T;
+#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::Testable<T> _gtsam_Testable_##T;

gtsam/base/concepts.h

@@ -11,7 +11,9 @@
 //#include "manifold.h"
 //#include "chart.h"
 #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>
 
@@ -24,9 +26,7 @@ namespace traits {
  * @brief Associate a unique tag with each of the main GTSAM concepts
  */
 //@{
-template<class T>
+template<typename T> struct structure_category;
-struct structure_category;
-// specializations should be derived from one of the following tags
 //@}
 
 /**
@@ -34,81 +34,98 @@ 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<class Manifold> struct TangentVector;
+template<typename Manifold> struct dimension;
-template<class Manifold> struct DefaultChart;
+template<typename Manifold> struct TangentVector;
+template<typename Manifold> struct DefaultChart;
 //@}
 
-}// namespace traits
+}// \ namespace traits
 
-/*
+/// Check invariants for Manifold type
- template<class 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> >);
- 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 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:
+} // \ namespace manifold
- Manifold p,q;
- TangentVector v;
 
- };
+/**
+ * Chart concept
  */
+template<typename T>
+class IsChart {
+public:
+  typedef typename T::ManifoldType ManifoldType;
+  typedef typename manifold::traits::TangentVector<ManifoldType>::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<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_MSG(
+        (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);
+    BOOST_CONCEPT_ASSERT((IsChart<DefaultChart >));
+  }
+private:
+  T 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 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<class G> struct identity;
+template<typename T> struct identity;
-template<class G> struct flavor;
+template<typename T> struct flavor;
 //@}
 
 /** @name Group Flavor Tags */
@@ -120,93 +137,130 @@ struct multiplicative_tag {
 //@}
 
 }// \ namespace traits
+
+/// Check invariants
+template<typename T>
+BOOST_CONCEPT_REQUIRES(((Testable<T>)),(bool)) //
+check_invariants(const T& a, const T& b, double tol = 1e-9) {
+  T e = traits::identity<T>::value;
+  return compose(a, inverse(a)).equals(e, tol)
+      && between(a, b).equals(compose(inverse(a), b), tol)
+      && compose(a, between(a, b)).equals<T>(b, tol);
+}
 } // \ namespace group
 
 /**
  * Group Concept
  */
-template<typename G>
+template<typename T>
-class Group {
+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;
 
-  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_STATIC_ASSERT_MSG(
-        boost::is_base_of<traits::group_tag, structure_category_tag>::value);
+        (boost::is_base_of<traits::group_tag, structure_category_tag>::value),
-    e = group::traits::identity<G>::value;
+        "This type's structure_category trait does not assert it as a group (or derived)");
-    d = compose(g, h);
+    e = group::traits::identity<T>::value;
-    d = between(g, h);
+    g = compose(g, h);
-    ig = inverse(g);
+    g = between(g, h);
-    test = operator_usage(g, h, flavor);
+    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<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:
   flavor_tag flavor;
-  G e, g, h, gh, ig, d;
+  T e, g, h;
-  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;
-  }
-
 };
 
-/*
+namespace lie_group {
- template <class 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);
+//@}
+
+namespace traits {
+
+/** @name Lie Group Traits */
+//@{
+//@}
+
+}// \ namespace traits
+
+/// Check invariants
+//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_MSG(
+        (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 <class V>
+template<typename T>
- class VectorSpaceConcept : public LieGroupConcept {
+class IsVectorSpace: public IsLieGroup<T> {
- typedef typename traits::DefaultChart<V>::type Chart;
+public:
- typedef typename GroupConcept<V>::identity identity;
 
- BOOST_CONCEPT_USAGE(VectorSpaceConcept) {
+  typedef typename traits::structure_category<T>::type structure_category_tag;
- BOOST_STATIC_ASSERT(boost::is_base_of<traits::vector_space_tag, traits::structure<L> >);
+
+  BOOST_CONCEPT_USAGE(IsVectorSpace) {
+    BOOST_STATIC_ASSERT_MSG(
+        (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;
   }
 
- 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 g,q,r;
+  T p, q, r;
 };
- */
 
 } // namespace gtsam
 

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<G>));
+  BOOST_CONCEPT_ASSERT((IsGroup<G>));
   EXPECT_LONGS_EQUAL(0, group::traits::identity<G>::value);
   G g(2), h(3);
   // EXPECT(Group<G>().check_invariants(g,h))
@@ -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

@@ -19,47 +19,134 @@
 
 namespace gtsam {
 
-/// Typedef to an Eigen Quaternion<double>, we disable alignment because
+namespace traits {
-/// geometry objects are stored in boost pool allocators, in Values
+
-/// containers, and and these pool allocators do not support alignment.
+/// Define Eigen::Quaternion to be a model of the Lie Group concept
-typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;
+template<typename S, int O>
+struct structure_category<Eigen::Quaternion<S, O> > {
+  typedef lie_group_tag type;
+};
+
+} // \namespace gtsam::traits
+
+namespace manifold {
+
+/// Chart for Eigen Quaternions
+template<typename S, int O>
+struct QuaternionChart {
+
+  // required
+  typedef Eigen::Quaternion<S, O> ManifoldType;
+
+  // internal
+  typedef ManifoldType Q;
+  typedef typename traits::TangentVector<Q>::type Omega;
+
+  /// Exponential map, simply be converting omega to AngleAxis
+  static Q Expmap(const Omega& omega) {
+    double theta = omega.norm();
+    if (std::abs(theta) < 1e-10)
+      return Q::Identity();
+    return Q(Eigen::AngleAxisd(theta, omega / theta));
+  }
+
+  /// retract, simply be converting omega to AngleAxis
+  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 Omega 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 Omega Local(const Q& q1, const Q& q2) {
+    return Logmap(q1.inverse() * q2);
+  }
+};
 
 namespace traits {
-/// Define Quaternion to be a model of the Group concept
+
-template<>
+/// Define the trait that asserts Quaternion manifold has dimension 3
-struct structure_category<Quaternion> {
+template<typename S, int O>
-  typedef group_tag type;
+struct dimension<Eigen::Quaternion<S, O> > : public boost::integral_constant<
+    int, 3> {
 };
-} // \namespace gtsam::traits
+
+/// 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 {
 
-Quaternion compose(const Quaternion&g, const Quaternion& h) {
+template<typename S, int O>
+Eigen::Quaternion<S, O> compose(const Eigen::Quaternion<S, O> &g,
+    const Eigen::Quaternion<S, O> & h) {
   return g * h;
 }
 
-Quaternion between(const Quaternion&g, const Quaternion& h) {
+template<typename S, int O>
+Eigen::Quaternion<S, O> between(const Eigen::Quaternion<S, O> &g,
+    const Eigen::Quaternion<S, O> & h) {
   return g.inverse() * h;
 }
 
-Quaternion inverse(const Quaternion&g) {
+template<typename S, int O>
+Eigen::Quaternion<S, O> inverse(const Eigen::Quaternion<S, O> &g) {
   return g.inverse();
 }
 
 namespace traits {
 
-/// Define the trait that specifies Quaternion's identity element
+/// Declare the trait that specifies a quaternion's identity element
-template<>
+template<typename S, int O>
-struct identity<Quaternion> {
+struct identity<Eigen::Quaternion<S, O> > {
-  static const Quaternion value;
+  static const Eigen::Quaternion<S, O> value;
-  typedef Quaternion value_type;
+  typedef Eigen::Quaternion<S, O> value_type;
 };
 
-const Quaternion identity<Quaternion>::value = Quaternion(0);
+/// Out of line definition of identity
+template<typename S, int O>
+const Eigen::Quaternion<S, O> identity<Eigen::Quaternion<S, O> >::value =
+    Eigen::Quaternion<S, O>::Identity();
 
-/// Define the trait that asserts Quaternion is an additive group
+/// Define the trait that asserts quaternions are a multiplicative group
-template<>
+template<typename S, int O>
-struct flavor<Quaternion> {
+struct flavor<Eigen::Quaternion<S, O> > {
   typedef multiplicative_tag type;
 };
 
@@ -67,6 +154,13 @@ struct flavor<Quaternion> {
 } // \namespace gtsam::group
 } // \namespace gtsam
 
+/**
+ *  GSAM typedef to an Eigen::Quaternion<double>, 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<double, Eigen::DontAlign> Quaternion;
+
 /**
  * @file   testCyclic.cpp
  * @brief  Unit tests for cyclic group
@@ -75,6 +169,7 @@ struct flavor<Quaternion> {
 
 //#include <gtsam/geometry/Quaternion.h>
 #include <gtsam/base/Testable.h>
+#include <gtsam/base/Vector.h>
 #include <CppUnitLite/TestHarness.h>
 
 using namespace std;
@@ -84,12 +179,43 @@ typedef Quaternion Q; // Typedef
 
 //******************************************************************************
 TEST(Quaternion , Concept) {
-  BOOST_CONCEPT_ASSERT((Group<Q>));
+  BOOST_CONCEPT_ASSERT((IsGroup<Quaternion >));
+  BOOST_CONCEPT_ASSERT((IsManifold<Quaternion >));
+  BOOST_CONCEPT_ASSERT((IsLieGroup<Quaternion >));
 }
 
 //******************************************************************************
 TEST(Quaternion , Constructor) {
-  Q g(0);
+  Q q(Eigen::AngleAxisd(1, Vector3(0, 0, 1)));
+}
+
+//******************************************************************************
+TEST(Quaternion , Invariants) {
+  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<Q>::type Chart;
+  Vector3 expected(0, 0, 0.1);
+  Vector3 actual = Chart::Local(q1, q2);
+  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<Q>::type Chart;
+  Vector3 v(0, 0, 0.1);
+  Q actual = Chart::Retract(q, v);
+  EXPECT(actual.isApprox(expected));
 }
 
 //******************************************************************************
@@ -101,7 +227,7 @@ TEST(Quaternion, Between) {
 }
 
 //******************************************************************************
-TEST(Quaternion, Ivnverse) {
+TEST(Quaternion , Inverse) {
 }
 
 //******************************************************************************