Re-factor traits, has its own namespace now, as well is_group and zero

gtsam/base/LieMatrix.h

@@ -174,6 +174,10 @@ private:
   }
 
 };
+
+// Define GTSAM traits
+namespace traits {
+
 template<>
 struct is_manifold<LieMatrix> : public std::true_type {
 };
@@ -182,4 +186,6 @@ template<>
 struct dimension<LieMatrix> : public Dynamic {
 };
 
+}
+
 } // \namespace gtsam

gtsam/base/LieScalar.h

@@ -112,6 +112,9 @@ namespace gtsam {
       double d_;
   };
 
+  // Define GTSAM traits
+  namespace traits {
+
   template<>
   struct is_manifold<LieScalar> : public std::true_type {
   };
@@ -120,4 +123,6 @@ namespace gtsam {
   struct dimension<LieScalar> : public Dynamic {
   };
 
+  }
+
 } // \namespace gtsam

gtsam/base/Manifold.h

@@ -13,14 +13,15 @@
  * @file Manifold.h
  * @brief Base class and basic functions for Manifold types
  * @author Alex Cunningham
+ * @author Frank Dellaert
  */
 
 #pragma once
 
-#include <string>
 #include <gtsam/base/Matrix.h>
 #include <boost/static_assert.hpp>
 #include <type_traits>
+#include <string>
 
 namespace gtsam {
 
@@ -45,6 +46,21 @@ namespace gtsam {
 // Traits, same style as Boost.TypeTraits
 // All meta-functions below ever only declare a single type
 // or a type/value/value_type
+namespace traits {
+
+// is group, by default this is false
+template<typename T>
+struct is_group: public std::false_type {
+};
+
+// identity, no default provided, by default given by default constructor
+template<typename T>
+struct identity {
+  static T value() {
+    return T();
+  }
+};
+
 // is manifold, by default this is false
 template<typename T>
 struct is_manifold: public std::false_type {
@@ -54,22 +70,13 @@ struct is_manifold: public std::false_type {
 template<typename T>
 struct dimension;
 
-// Chart is a map from T -> vector, retract is its inverse
-template<typename T>
-struct DefaultChart {
-  BOOST_STATIC_ASSERT(is_manifold<T>::value);
-  typedef Eigen::Matrix<double, dimension<T>::value, 1> vector;
-  DefaultChart(const T& t) :
-      t_(t) {
-  }
-  vector apply(const T& other) {
-    return t_.localCoordinates(other);
-  }
-  T retract(const vector& d) {
-    return t_.retract(d);
-  }
-private:
-  T const & t_;
+/**
+ * zero<T>::value is intended to be the origin of a canonical coordinate system
+ * with canonical(t) == DefaultChart<T>(zero<T>::value).apply(t)
+ * Below we provide the group identity as zero *in case* it is a group
+ */
+template<typename T> struct zero: public identity<T> {
+  BOOST_STATIC_ASSERT(is_group<T>::value);
 };
 
 // double
@@ -82,24 +89,6 @@ template<>
 struct dimension<double> : public std::integral_constant<int, 1> {
 };
 
-template<>
-struct DefaultChart<double> {
-  typedef Eigen::Matrix<double, 1, 1> vector;
-  DefaultChart(double t) :
-      t_(t) {
-  }
-  vector apply(double other) {
-    vector d;
-    d << other - t_;
-    return d;
-  }
-  double retract(const vector& d) {
-    return t_ + d[0];
-  }
-private:
-  double t_;
-};
-
 // Fixed size Eigen::Matrix type
 
 template<int M, int N, int Options>
@@ -130,10 +119,74 @@ struct dimension<Eigen::Matrix<double, M, N, Options> > : public std::integral_c
   BOOST_STATIC_ASSERT(M!=Eigen::Dynamic && N!=Eigen::Dynamic);
 };
 
+} // \ namespace traits
+
+// Chart is a map from T -> vector, retract is its inverse
+template<typename T>
+struct DefaultChart {
+  BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
+  typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
+  DefaultChart(const T& t) :
+      t_(t) {
+  }
+  vector apply(const T& other) {
+    return t_.localCoordinates(other);
+  }
+  T retract(const vector& d) {
+    return t_.retract(d);
+  }
+private:
+  T const & t_;
+};
+
+/**
+ * Canonical<T>::value is a chart around zero<T>::value
+ * An example is Canonical<Rot3>
+ */
+template<typename T> class Canonical {
+  DefaultChart<T> chart;
+public:
+  typedef T type;
+  typedef typename DefaultChart<T>::vector vector;
+  Canonical() :
+      chart(traits::zero<T>::value()) {
+  }
+  // Convert t of type T into canonical coordinates
+  vector apply(const T& t) {
+    return chart.apply(t);
+  }
+  // Convert back from canonical coordinates to T
+  T retract(const vector& v) {
+    return chart.retract(v);
+  }
+};
+
+// double
+
+template<>
+struct DefaultChart<double> {
+  typedef Eigen::Matrix<double, 1, 1> vector;
+  DefaultChart(double t) :
+      t_(t) {
+  }
+  vector apply(double other) {
+    vector d;
+    d << other - t_;
+    return d;
+  }
+  double retract(const vector& d) {
+    return t_ + d[0];
+  }
+private:
+  double t_;
+};
+
+// Fixed size Eigen::Matrix type
+
 template<int M, int N, int Options>
 struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
   typedef Eigen::Matrix<double, M, N, Options> T;
-  typedef Eigen::Matrix<double, dimension<T>::value, 1> vector;
+  typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
   DefaultChart(const T& t) :
       t_(t) {
   }
@@ -202,7 +255,7 @@ private:
   }
 };
 
-} // namespace gtsam
+} // \ namespace gtsam
 
 /**
  * Macros for using the ManifoldConcept

gtsam/geometry/Cal3Bundler.h

@@ -169,6 +169,9 @@ private:
 
 };
 
+// Define GTSAM traits
+namespace traits {
+
 template<>
 struct is_manifold<Cal3Bundler> : public std::true_type {
 };
@@ -177,4 +180,13 @@ template<>
 struct dimension<Cal3Bundler> : public std::integral_constant<int, 3> {
 };
 
+template<>
+struct zero<Cal3Bundler> {
+  static Cal3Bundler value() {
+    return Cal3Bundler(0, 0, 0);
+  }
+};
+
+}
+
 } // namespace gtsam

gtsam/geometry/Cal3DS2.h

@@ -168,6 +168,9 @@ private:
 
 };
 
+// Define GTSAM traits
+namespace traits {
+
 template<>
 struct is_manifold<Cal3DS2> : public std::true_type {
 };
@@ -175,5 +178,8 @@ struct is_manifold<Cal3DS2> : public std::true_type {
 template<>
 struct dimension<Cal3DS2> : public std::integral_constant<int, 9> {
 };
+
+}
+
 }
 

gtsam/geometry/Cal3_S2.h

@@ -240,6 +240,9 @@ private:
 
 };
 
+// Define GTSAM traits
+namespace traits {
+
 template<>
 struct is_manifold<Cal3_S2> : public std::true_type {
 };
@@ -248,5 +251,11 @@ template<>
 struct dimension<Cal3_S2> : public std::integral_constant<int, 5> {
 };
 
+template<>
+struct zero<Cal3_S2> {
+  static Cal3_S2 value() { return Cal3_S2();}
+};
+
+}
 
 } // \ namespace gtsam

gtsam/geometry/PinholeCamera.h

@@ -303,7 +303,7 @@ public:
     return K_.uncalibrate(pn);
   }
 
-  typedef Eigen::Matrix<double,2,dimension<Calibration>::value> Matrix2K;
+  typedef Eigen::Matrix<double,2,traits::dimension<Calibration>::value> Matrix2K;
 
   /** project a point from world coordinate to the image
    *  @param pw is a point in world coordinates
@@ -613,6 +613,9 @@ private:
 
 };
 
+// Define GTSAM traits
+namespace traits {
+
 template<typename Calibration>
 struct is_manifold<PinholeCamera<Calibration> > : public std::true_type {
 };
@@ -622,4 +625,14 @@ struct dimension<PinholeCamera<Calibration> > : public std::integral_constant<
     int, dimension<Pose3>::value + dimension<Calibration>::value> {
 };
 
+template<typename Calibration>
+struct zero<PinholeCamera<Calibration> > {
+  static PinholeCamera<Calibration> value() {
+    return PinholeCamera<Calibration>(zero<Pose3>::value(),
+        zero<Calibration>::value());
+  }
+};
+
 }
+
+} // \ gtsam

gtsam/geometry/Point2.h

@@ -250,6 +250,13 @@ private:
 /// multiply with scalar
 inline Point2 operator*(double s, const Point2& p) {return p*s;}
 
+// Define GTSAM traits
+namespace traits {
+
+template<>
+struct is_group<Point2> : public std::true_type {
+};
+
 template<>
 struct is_manifold<Point2> : public std::true_type {
 };
@@ -260,3 +267,5 @@ struct dimension<Point2> : public std::integral_constant<int, 2> {
 
 }
 
+}
+

gtsam/geometry/Point3.h

@@ -242,6 +242,13 @@ namespace gtsam {
   /// Syntactic sugar for multiplying coordinates by a scalar s*p
   inline Point3 operator*(double s, const Point3& p) { return p*s;}
 
+  // Define GTSAM traits
+  namespace traits {
+
+  template<>
+  struct is_group<Point3> : public std::true_type {
+  };
+
   template<>
   struct is_manifold<Point3> : public std::true_type {
   };
@@ -250,4 +257,5 @@ namespace gtsam {
   struct dimension<Point3> : public std::integral_constant<int, 3> {
   };
 
+  }
 }

gtsam/geometry/Pose2.h

@@ -321,6 +321,9 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
 typedef std::pair<Point2,Point2> Point2Pair;
 GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
 
+// Define GTSAM traits
+namespace traits {
+
 template<>
 struct is_manifold<Pose2> : public std::true_type {
 };
@@ -329,5 +332,7 @@ template<>
 struct dimension<Pose2> : public std::integral_constant<int, 3> {
 };
 
+}
+
 } // namespace gtsam
 

gtsam/geometry/Pose3.h

@@ -354,6 +354,13 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
 typedef std::pair<Point3, Point3> Point3Pair;
 GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
 
+// Define GTSAM traits
+namespace traits {
+
+template<>
+struct is_group<Pose3> : public std::true_type {
+};
+
 template<>
 struct is_manifold<Pose3> : public std::true_type {
 };
@@ -362,4 +369,6 @@ template<>
 struct dimension<Pose3> : public std::integral_constant<int, 6> {
 };
 
+}
+
 } // namespace gtsam

gtsam/geometry/Rot3.h

@@ -491,6 +491,13 @@ namespace gtsam {
    */
   GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
 
+  // Define GTSAM traits
+  namespace traits {
+
+  template<>
+  struct is_group<Rot3> : public std::true_type {
+  };
+
   template<>
   struct is_manifold<Rot3> : public std::true_type {
   };
@@ -499,5 +506,5 @@ namespace gtsam {
   struct dimension<Rot3> : public std::integral_constant<int, 3> {
   };
 
-
+  }
 }