More manifolds...more numerical differentiation...more.

gtsam/base/LieScalar.h

@@ -32,6 +32,8 @@ namespace gtsam {
    */
   struct GTSAM_EXPORT LieScalar {
 
+    enum { dimension = 1 };
+
     /** default constructor */
     LieScalar() : d_(0.0) {}
 
@@ -116,21 +118,8 @@ namespace gtsam {
       double d_;
   };
 
-  // Define GTSAM traits
-  namespace traits {
 
   template<>
-  struct is_group<LieScalar> : public boost::true_type {
-  };
-
-  template<>
-  struct is_manifold<LieScalar> : public boost::true_type {
-  };
-
-  template<>
-  struct dimension<LieScalar> : public boost::integral_constant<int, 1> {
-  };
-
-  }
+  struct traits_x<LieScalar> : public internal::LieGroup<LieScalar> {};
 
 } // \namespace gtsam

gtsam/base/Manifold.h

@@ -24,26 +24,27 @@
 #include <boost/type_traits.hpp>
 #include <string>
 //
-//namespace gtsam {
-//
-///**
-// * A manifold defines a space in which there is a notion of a linear tangent space
-// * that can be centered around a given point on the manifold.  These nonlinear
-// * spaces may have such properties as wrapping around (as is the case with rotations),
-// * which might make linear operations on parameters not return a viable element of
-// * the manifold.
-// *
-// * We perform optimization by computing a linear delta in the tangent space of the
-// * current estimate, and then apply this change using a retraction operation, which
-// * maps the change in tangent space back to the manifold itself.
-// *
-// * There may be multiple possible retractions for a given manifold, which can be chosen
-// * between depending on the computational complexity.  The important criteria for
-// * the creation for the retract and localCoordinates functions is that they be
-// * inverse operations. The new notion of a Chart guarantees that.
-// *
-// */
-//
+namespace gtsam {
+
+/**
+ * A manifold defines a space in which there is a notion of a linear tangent space
+ * that can be centered around a given point on the manifold.  These nonlinear
+ * spaces may have such properties as wrapping around (as is the case with rotations),
+ * which might make linear operations on parameters not return a viable element of
+ * the manifold.
+ *
+ * We perform optimization by computing a linear delta in the tangent space of the
+ * current estimate, and then apply this change using a retraction operation, which
+ * maps the change in tangent space back to the manifold itself.
+ *
+ * There may be multiple possible retractions for a given manifold, which can be chosen
+ * between depending on the computational complexity.  The important criteria for
+ * the creation for the retract and localCoordinates functions is that they be
+ * inverse operations. The new notion of a Chart guarantees that.
+ *
+ */
+
+
 //// Traits, same style as Boost.TypeTraits
 //// All meta-functions below ever only declare a single type
 //// or a type/value/value_type
@@ -355,7 +356,7 @@
 //  }
 //};
 //
-//} // \ namespace gtsam
+} // \ namespace gtsam
 //
 
 // TODO(ASL) Remove these and fix the compiler errors.

gtsam/base/concepts.h

@@ -120,7 +120,7 @@ struct Manifold {
 template<typename ManifoldType>
 struct LieGroup {
   // Typedefs required by all manifold types.
-  typedef manifold_tag structure_category;
+  typedef lie_group_tag structure_category;
   enum { dimension = ManifoldType::dimension };
   typedef Eigen::Matrix<double, dimension, 1> TangentVector;
   typedef OptionalJacobian<dimension, dimension> ChartJacobian;
@@ -217,13 +217,155 @@ struct LieGroup {
 
 };
 
+/// A helper that implements the traits interface for GTSAM lie groups.
+/// To use this for your gtsam type, define:
+/// template<> struct traits<Type> : public ScalarTraits<Type> { };
+template<typename Scalar>
+struct ScalarTraits {
+  // Typedefs required by all manifold types.
+  typedef vector_space_tag structure_category;
+  typedef additive_group_tag group_flavor;
+  enum { dimension = 1 };
+  typedef Eigen::Matrix<double, dimension, 1> TangentVector;
+  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
+
+  // For Testable
+  static void Print(Scalar m, const std::string& str = "") {
+    m.print();
+  }
+  static bool Equals(Scalar m1,
+      Scalar m2,
+      double tol = 1e-8) {
+    return fabs(m1 - m2) < tol;
+  }
+
+  static TangentVector Local(Scalar origin,
+      Scalar other) {
+    return TangentVector(other - origin);
+  }
+
+  static Scalar Retract(Scalar origin,
+      const TangentVector& v) {
+    return origin + v[0];
+  }
+
+  static TangentVector Local(Scalar origin,
+      Scalar other,
+      ChartJacobian Horigin,
+      ChartJacobian Hother) {
+    if(Horigin) {
+      (*Horigin)[0] = -1.0;
+    }
+    if(Hother) {
+      (*Hother)[0] = 1.0;
+    }
+    return Local(origin, other);
+  }
+
+  static Scalar Retract(Scalar origin,
+      const TangentVector& v,
+      ChartJacobian Horigin,
+      ChartJacobian Hv) {
+    if(Horigin) {
+      (*Horigin)[0] = 1.0;
+    }
+    if(Hv) {
+      (*Hv)[0] = 1.0;
+    }
+    return Retract(origin, v);
+  }
+
+  static int GetDimension(Scalar m){ return 1; }
+
+  // For Group. Only implemented for groups
+  static Scalar Compose(Scalar m1,
+      Scalar m2) {
+    return m1 + m2;
+  }
+
+  static Scalar Between(Scalar m1,
+      Scalar m2) {
+    return m2 - m1;
+  }
+
+  static Scalar Inverse(Scalar m) {
+    return -m;
+  }
+
+  static Scalar Compose(Scalar m1,
+      Scalar m2,
+      ChartJacobian H1,
+      ChartJacobian H2) {
+    if(H1) {
+      (*H1)[0] = 1.0;
+    }
+    if(H2) {
+      (*H2)[0] = 1.0;
+    }
+    return Compose(m1, m2);
+  }
+
+  static Scalar Between(Scalar m1,
+      Scalar m2,
+      ChartJacobian H1,
+      ChartJacobian H2) {
+    if(H1) {
+      (*H1)[0] = -1.0;
+    }
+    if(H2) {
+      (*H2)[0] = 1.0;
+    }
+    return Between(m1, m2);
+  }
+
+  static Scalar Inverse(Scalar m, ChartJacobian H) {
+    if(H) {
+      (*H)[0] = -1;
+    }
+    return Inverse(m);
+  }
+
+  static Scalar Identity() {
+    return 0;
+  }
+
+  static TangentVector Logmap(Scalar m) {
+    return TangentVector(m);
+  }
+
+  static Scalar Expmap(const TangentVector& v) {
+    return v[0];
+  }
+
+  static TangentVector Logmap(Scalar m, ChartJacobian Hm) {
+    if(Hm) {
+      (*Hm)[0] = 1.0;
+    }
+    return Scalar::Logmap(m);
+  }
+
+  static Scalar Expmap(const TangentVector& v, ChartJacobian Hv) {
+    if(Hv) {
+      (*Hv)[0] = 1.0;
+    }
+    return Scalar::Expmap(v);
+  }
+
+};
+
 }  // namespace internal
 
+template<>
+struct traits_x<double> : public internal::ScalarTraits<double> {};
+template<>
+struct traits_x<float> : public internal::ScalarTraits<float> {};
+
+
 template<typename ManifoldType>
 struct Canonical {
   BOOST_STATIC_ASSERT_MSG(
-      (boost::is_base_of<group_tag, typename traits_x<ManifoldType>::structure_category_tag>::value),
-      "This type's trait does not assert it is a group (or derived)");
+      (boost::is_base_of<manifold_tag, typename traits_x<ManifoldType>::structure_category>::value),
+      "This type's trait does not assert it is a manifold (or derived)");
   typedef traits_x<ManifoldType> Traits;
   typedef typename Traits::TangentVector TangentVector;
   enum { dimension = Traits::dimension };

gtsam/base/numericalDerivative.h

@@ -139,7 +139,7 @@ Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
     dx(j) = delta;
     TangentY dy1 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
     dx(j) = -delta;
-    TangentY dy2 = TraitsY::local(hx, h(TraitsX::Retract(x, dx)));
+    TangentY dy2 = TraitsY::Local(hx, h(TraitsX::Retract(x, dx)));
     dx(j) = 0;
     H.col(j) << (dy1 - dy2) * factor;
   }

gtsam/nonlinear/AdaptAutoDiff.h

@@ -28,7 +28,7 @@ namespace gtsam {
 template<typename F, typename T, typename A1, typename A2>
 class AdaptAutoDiff {
 
-  static const int N = traits:_x<T>:dimension;
+  static const int N = traits_x<T>::dimension;
   static const int M1 = traits_x<A1>::dimension;
   static const int M2 = traits_x<A2>::dimension;
 
@@ -39,9 +39,9 @@ class AdaptAutoDiff {
   typedef Canonical<A1> Canonical1;
   typedef Canonical<A2> Canonical2;
 
-  typedef typename CanonicalT::vector VectorT;
-  typedef typename Canonical1::vector Vector1;
-  typedef typename Canonical2::vector Vector2;
+  typedef typename CanonicalT::TangentVector VectorT;
+  typedef typename Canonical1::TangentVector Vector1;
+  typedef typename Canonical2::TangentVector Vector2;
 
   // Instantiate function and charts
   CanonicalT chartT;

gtsam_unstable/dynamics/PoseRTV.h

@@ -26,6 +26,8 @@ protected:
   Velocity3 v_;
 
 public:
+  enum { dimension = 9 };
+
   // constructors - with partial versions
   PoseRTV() {}
   PoseRTV(const Point3& pt, const Rot3& rot, const Velocity3& vel)
@@ -183,23 +185,8 @@ private:
   }
 };
 
-// Define GTSAM traits
-namespace traits {
 
 template<>
-struct is_manifold<PoseRTV> : public boost::true_type {
-};
+struct traits_x<PoseRTV> : public internal::LieGroup<PoseRTV> {};
 
-template<>
-struct dimension<PoseRTV> : public boost::integral_constant<int, 9> {
-};
-
-template<>
-struct zero<PoseRTV> {
-  static PoseRTV value() {
-    return PoseRTV();
-  }
-};
-
-}
 } // \namespace gtsam