diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h
index 783605b33..22315bf15 100644
--- a/gtsam/base/Manifold.h
+++ b/gtsam/base/Manifold.h
@@ -23,347 +23,347 @@
 #include <boost/static_assert.hpp>
 #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.
- *
- */
-
-// 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 boost::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 boost::false_type {
-};
-
-// dimension, can return Eigen::Dynamic (-1) if not known at compile time
-// defaults to dynamic, TODO makes sense ?
-typedef boost::integral_constant<int, Eigen::Dynamic> Dynamic;
+//
+//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
+//namespace traits {
+//
+//// is group, by default this is false
 //template<typename T>
-//struct dimension: public Dynamic {
-//struct dimension : public boost::integral_constant<int, 1> {  // just temporary fix to minimize compiler errors while refactoring
+//struct is_group: public boost::false_type {
 //};
-
-/**
- * zero<T>::value is intended to be the origin of a canonical coordinate system
- * with canonical(t) == DefaultChart<T>::local(zero<T>::value, 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
-
-template<>
-struct is_group<double> : public boost::true_type {
-};
-
-template<>
-struct is_manifold<double> : public boost::true_type {
-};
-
-template<>
-struct dimension<double> : public boost::integral_constant<int, 1> {
-};
-
-template<>
-struct zero<double> {
-  static double value() {
-    return 0;
-  }
-};
-
-// Fixed size Eigen::Matrix type
-
-template<int M, int N, int Options, int MaxRows, int MaxCols>
-struct is_group<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type {
-};
-
-template<int M, int N, int Options, int MaxRows, int MaxCols>
-struct is_manifold<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type {
-};
-
-template<int M, int N, int Options, int MaxRows, int MaxCols>
-struct dimension<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::integral_constant<
-    int,
-    M == Eigen::Dynamic ? Eigen::Dynamic :
-        (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> {
-  //TODO after switch to c++11 : the above should should be extracted to a constexpr function
-  // for readability and to reduce code duplication
-};
-
-template<int M, int N, int Options, int MaxRows, int MaxCols>
-struct zero<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
-  BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
-      "traits::zero is only supported for fixed-size matrices");
-  static Eigen::Matrix<double, M, N, Options> value() {
-    return Eigen::Matrix<double, M, N, Options>::Zero();
-  }
-};
-
-template<int M, int N, int Options>
-struct identity<Eigen::Matrix<double, M, N, Options> > : public zero<
-    Eigen::Matrix<double, M, N, Options> > {
-};
-
-template<typename T> struct is_chart: public boost::false_type {
-};
-
-} // \ 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 T type;
-  typedef Eigen::Matrix<double, traits_x<T>::dimension, 1> vector;
-
-  static vector local(const T& origin, const T& other) {
-    return origin.localCoordinates(other);
-  }
-  static T retract(const T& origin, const vector& d) {
-    return origin.retract(d);
-  }
-  static int getDimension(const T& origin) {
-    return origin.dim();
-  }
-};
-
-namespace traits {
-// populate default traits
-template<typename T> struct is_chart<DefaultChart<T> > : public boost::true_type {
-};
-template<typename T> struct dimension<DefaultChart<T> > : public dimension<T> {
-};
-}
-
-template<class C>
-struct ChartConcept {
-public:
-  typedef typename C::type type;
-  typedef typename C::vector vector;
-
-  BOOST_CONCEPT_USAGE(ChartConcept) {
-    // is_chart trait should be true
-    BOOST_STATIC_ASSERT((traits::is_chart<C>::value));
-
-    /**
-     * Returns Retraction update of val_
-     */
-    type retract_ret = C::retract(val_, vec_);
-
-    /**
-     * Returns local coordinates of another object
-     */
-    vec_ = C::local(val_, retract_ret);
-
-    // a way to get the dimension that is compatible with dynamically sized types
-    dim_ = C::getDimension(val_);
-  }
-
-private:
-  type val_;
-  vector vec_;
-  int dim_;
-};
-
-/**
- * CanonicalChart<Chart<T> > is a chart around zero<T>::value
- * Canonical<T> is CanonicalChart<DefaultChart<T> >
- * An example is Canonical<Rot3>
- */
-template<typename C> struct CanonicalChart {
-  BOOST_CONCEPT_ASSERT((ChartConcept<C>));
-
-  typedef C Chart;
-  typedef typename Chart::type type;
-  typedef typename Chart::vector vector;
-
-  // Convert t of type T into canonical coordinates
-  vector local(const type& t) {
-    return Chart::local(traits::zero<type>::value(), t);
-  }
-  // Convert back from canonical coordinates to T
-  type retract(const vector& v) {
-    return Chart::retract(traits::zero<type>::value(), v);
-  }
-};
-template<typename T> struct Canonical: public CanonicalChart<DefaultChart<T> > {
-};
-
-// double
-
-template<>
-struct DefaultChart<double> {
-  typedef double type;
-  typedef Eigen::Matrix<double, 1, 1> vector;
-
-  static vector local(double origin, double other) {
-    vector d;
-    d << other - origin;
-    return d;
-  }
-  static double retract(double origin, const vector& d) {
-    return origin + d[0];
-  }
-  static int getDimension(double) {
-    return 1;
-  }
-};
-
-// Fixed size Eigen::Matrix type
-
-template<int M, int N, int Options, int MaxRows, int MaxCols>
-struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
-  /**
-   * This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options).
-   * Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size.
-   */
-  typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> type;
-  typedef type T;
-  typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
-
-  BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
-      "Internal error: DefaultChart for Dynamic should be handled by template below");
-
-  static vector local(const T& origin, const T& other) {
-    return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other)
-        - reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
-  }
-  static T retract(const T& origin, const vector& d) {
-    return origin + reshape<M, N, Options>(d);
-  }
-  static int getDimension(const T&origin) {
-    return origin.rows() * origin.cols();
-  }
-};
-
-// Dynamically sized Vector
-template<>
-struct DefaultChart<Vector> {
-  typedef Vector type;
-  typedef Vector vector;
-  static vector local(const Vector& origin, const Vector& other) {
-    return other - origin;
-  }
-  static Vector retract(const Vector& origin, const vector& d) {
-    return origin + d;
-  }
-  static int getDimension(const Vector& origin) {
-    return origin.size();
-  }
-};
-
-// Dynamically sized Matrix
-template<>
-struct DefaultChart<Matrix> {
-  typedef Matrix type;
-  typedef Vector vector;
-  static vector local(const Matrix& origin, const Matrix& other) {
-    Matrix d = other - origin;
-    return Eigen::Map<Vector>(d.data(),getDimension(d));
-  }
-  static Matrix retract(const Matrix& origin, const vector& d) {
-    return origin + Eigen::Map<const Matrix>(d.data(),origin.rows(),origin.cols());
-  }
-  static int getDimension(const Matrix& m) {
-    return m.size();
-  }
-};
-
-/**
- * Old Concept check class for Manifold types
- * Requires a mapping between a linear tangent space and the underlying
- * manifold, of which Lie is a specialization.
- *
- * The necessary functions to implement for Manifold are defined
- * below with additional details as to the interface.  The
- * concept checking function in class Manifold will check whether or not
- * the function exists and throw compile-time errors.
- *
- * Returns dimensionality of the tangent space, which may be smaller than the number
- * of nonlinear parameters.
- *     size_t dim() const;
- *
- * Returns a new T that is a result of updating *this with the delta v after pulling
- * the updated value back to the manifold T.
- *     T retract(const Vector& v) const;
- *
- * Returns the linear coordinates of lp in the tangent space centered around *this.
- *     Vector localCoordinates(const T& lp) const;
- *
- * By convention, we use capital letters to designate a static function
- * @tparam T is a Lie type, like Point2, Pose3, etc.
- */
-template<class T>
-class ManifoldConcept {
-private:
-  /** concept checking function - implement the functions this demands */
-  static T concept_check(const T& t) {
-
-    /** assignment */
-    T t2 = t;
-
-    /**
-     * Returns dimensionality of the tangent space
-     */
-    size_t dim_ret = t.dim();
-
-    /**
-     * Returns Retraction update of T
-     */
-    T retract_ret = t.retract(gtsam::zero(dim_ret));
-
-    /**
-     * Returns local coordinates of another object
-     */
-    Vector localCoords_ret = t.localCoordinates(t2);
-
-    return retract_ret;
-  }
-};
-
-} // \ namespace gtsam
-
-/**
- * Macros for using the ManifoldConcept
- *  - An instantiation for use inside unit tests
- *  - A typedef for use inside generic algorithms
- *
- * NOTE: intentionally not in the gtsam namespace to allow for classes not in
- * the gtsam namespace to be more easily enforced as testable
- */
-#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::ManifoldConcept<T>;
-#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::ManifoldConcept<T> _gtsam_ManifoldConcept_##T;
+//
+//// 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 boost::false_type {
+//};
+//
+//// dimension, can return Eigen::Dynamic (-1) if not known at compile time
+//// defaults to dynamic, TODO makes sense ?
+//typedef boost::integral_constant<int, Eigen::Dynamic> Dynamic;
+////template<typename T>
+////struct dimension: public Dynamic {
+////struct dimension : public boost::integral_constant<int, 1> {  // just temporary fix to minimize compiler errors while refactoring
+////};
+//
+///**
+// * zero<T>::value is intended to be the origin of a canonical coordinate system
+// * with canonical(t) == DefaultChart<T>::local(zero<T>::value, 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
+//
+//template<>
+//struct is_group<double> : public boost::true_type {
+//};
+//
+//template<>
+//struct is_manifold<double> : public boost::true_type {
+//};
+//
+//template<>
+//struct dimension<double> : public boost::integral_constant<int, 1> {
+//};
+//
+//template<>
+//struct zero<double> {
+//  static double value() {
+//    return 0;
+//  }
+//};
+//
+//// Fixed size Eigen::Matrix type
+//
+//template<int M, int N, int Options, int MaxRows, int MaxCols>
+//struct is_group<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type {
+//};
+//
+//template<int M, int N, int Options, int MaxRows, int MaxCols>
+//struct is_manifold<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::true_type {
+//};
+//
+//template<int M, int N, int Options, int MaxRows, int MaxCols>
+//struct dimension<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > : public boost::integral_constant<
+//    int,
+//    M == Eigen::Dynamic ? Eigen::Dynamic :
+//        (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> {
+//  //TODO after switch to c++11 : the above should should be extracted to a constexpr function
+//  // for readability and to reduce code duplication
+//};
+//
+//template<int M, int N, int Options, int MaxRows, int MaxCols>
+//struct zero<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
+//  BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
+//      "traits::zero is only supported for fixed-size matrices");
+//  static Eigen::Matrix<double, M, N, Options> value() {
+//    return Eigen::Matrix<double, M, N, Options>::Zero();
+//  }
+//};
+//
+//template<int M, int N, int Options>
+//struct identity<Eigen::Matrix<double, M, N, Options> > : public zero<
+//    Eigen::Matrix<double, M, N, Options> > {
+//};
+//
+//template<typename T> struct is_chart: public boost::false_type {
+//};
+//
+//} // \ 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 T type;
+//  typedef Eigen::Matrix<double, traits_x<T>::dimension, 1> vector;
+//
+//  static vector local(const T& origin, const T& other) {
+//    return origin.localCoordinates(other);
+//  }
+//  static T retract(const T& origin, const vector& d) {
+//    return origin.retract(d);
+//  }
+//  static int getDimension(const T& origin) {
+//    return origin.dim();
+//  }
+//};
+//
+//namespace traits {
+//// populate default traits
+//template<typename T> struct is_chart<DefaultChart<T> > : public boost::true_type {
+//};
+//template<typename T> struct dimension<DefaultChart<T> > : public dimension<T> {
+//};
+//}
+//
+//template<class C>
+//struct ChartConcept {
+//public:
+//  typedef typename C::type type;
+//  typedef typename C::vector vector;
+//
+//  BOOST_CONCEPT_USAGE(ChartConcept) {
+//    // is_chart trait should be true
+//    BOOST_STATIC_ASSERT((traits::is_chart<C>::value));
+//
+//    /**
+//     * Returns Retraction update of val_
+//     */
+//    type retract_ret = C::retract(val_, vec_);
+//
+//    /**
+//     * Returns local coordinates of another object
+//     */
+//    vec_ = C::local(val_, retract_ret);
+//
+//    // a way to get the dimension that is compatible with dynamically sized types
+//    dim_ = C::getDimension(val_);
+//  }
+//
+//private:
+//  type val_;
+//  vector vec_;
+//  int dim_;
+//};
+//
+///**
+// * CanonicalChart<Chart<T> > is a chart around zero<T>::value
+// * Canonical<T> is CanonicalChart<DefaultChart<T> >
+// * An example is Canonical<Rot3>
+// */
+//template<typename C> struct CanonicalChart {
+//  BOOST_CONCEPT_ASSERT((ChartConcept<C>));
+//
+//  typedef C Chart;
+//  typedef typename Chart::type type;
+//  typedef typename Chart::vector vector;
+//
+//  // Convert t of type T into canonical coordinates
+//  vector local(const type& t) {
+//    return Chart::local(traits::zero<type>::value(), t);
+//  }
+//  // Convert back from canonical coordinates to T
+//  type retract(const vector& v) {
+//    return Chart::retract(traits::zero<type>::value(), v);
+//  }
+//};
+//template<typename T> struct Canonical: public CanonicalChart<DefaultChart<T> > {
+//};
+//
+//// double
+//
+//template<>
+//struct DefaultChart<double> {
+//  typedef double type;
+//  typedef Eigen::Matrix<double, 1, 1> vector;
+//
+//  static vector local(double origin, double other) {
+//    vector d;
+//    d << other - origin;
+//    return d;
+//  }
+//  static double retract(double origin, const vector& d) {
+//    return origin + d[0];
+//  }
+//  static int getDimension(double) {
+//    return 1;
+//  }
+//};
+//
+//// Fixed size Eigen::Matrix type
+//
+//template<int M, int N, int Options, int MaxRows, int MaxCols>
+//struct DefaultChart<Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
+//  /**
+//   * This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options).
+//   * Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size.
+//   */
+//  typedef Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> type;
+//  typedef type T;
+//  typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
+//
+//  BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
+//      "Internal error: DefaultChart for Dynamic should be handled by template below");
+//
+//  static vector local(const T& origin, const T& other) {
+//    return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other)
+//        - reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
+//  }
+//  static T retract(const T& origin, const vector& d) {
+//    return origin + reshape<M, N, Options>(d);
+//  }
+//  static int getDimension(const T&origin) {
+//    return origin.rows() * origin.cols();
+//  }
+//};
+//
+//// Dynamically sized Vector
+//template<>
+//struct DefaultChart<Vector> {
+//  typedef Vector type;
+//  typedef Vector vector;
+//  static vector local(const Vector& origin, const Vector& other) {
+//    return other - origin;
+//  }
+//  static Vector retract(const Vector& origin, const vector& d) {
+//    return origin + d;
+//  }
+//  static int getDimension(const Vector& origin) {
+//    return origin.size();
+//  }
+//};
+//
+//// Dynamically sized Matrix
+//template<>
+//struct DefaultChart<Matrix> {
+//  typedef Matrix type;
+//  typedef Vector vector;
+//  static vector local(const Matrix& origin, const Matrix& other) {
+//    Matrix d = other - origin;
+//    return Eigen::Map<Vector>(d.data(),getDimension(d));
+//  }
+//  static Matrix retract(const Matrix& origin, const vector& d) {
+//    return origin + Eigen::Map<const Matrix>(d.data(),origin.rows(),origin.cols());
+//  }
+//  static int getDimension(const Matrix& m) {
+//    return m.size();
+//  }
+//};
+//
+///**
+// * Old Concept check class for Manifold types
+// * Requires a mapping between a linear tangent space and the underlying
+// * manifold, of which Lie is a specialization.
+// *
+// * The necessary functions to implement for Manifold are defined
+// * below with additional details as to the interface.  The
+// * concept checking function in class Manifold will check whether or not
+// * the function exists and throw compile-time errors.
+// *
+// * Returns dimensionality of the tangent space, which may be smaller than the number
+// * of nonlinear parameters.
+// *     size_t dim() const;
+// *
+// * Returns a new T that is a result of updating *this with the delta v after pulling
+// * the updated value back to the manifold T.
+// *     T retract(const Vector& v) const;
+// *
+// * Returns the linear coordinates of lp in the tangent space centered around *this.
+// *     Vector localCoordinates(const T& lp) const;
+// *
+// * By convention, we use capital letters to designate a static function
+// * @tparam T is a Lie type, like Point2, Pose3, etc.
+// */
+//template<class T>
+//class ManifoldConcept {
+//private:
+//  /** concept checking function - implement the functions this demands */
+//  static T concept_check(const T& t) {
+//
+//    /** assignment */
+//    T t2 = t;
+//
+//    /**
+//     * Returns dimensionality of the tangent space
+//     */
+//    size_t dim_ret = t.dim();
+//
+//    /**
+//     * Returns Retraction update of T
+//     */
+//    T retract_ret = t.retract(gtsam::zero(dim_ret));
+//
+//    /**
+//     * Returns local coordinates of another object
+//     */
+//    Vector localCoords_ret = t.localCoordinates(t2);
+//
+//    return retract_ret;
+//  }
+//};
+//
+//} // \ namespace gtsam
+//
+///**
+// * Macros for using the ManifoldConcept
+// *  - An instantiation for use inside unit tests
+// *  - A typedef for use inside generic algorithms
+// *
+// * NOTE: intentionally not in the gtsam namespace to allow for classes not in
+// * the gtsam namespace to be more easily enforced as testable
+// */
+#define GTSAM_CONCEPT_MANIFOLD_INST(T)
+#define GTSAM_CONCEPT_MANIFOLD_TYPE(T)
diff --git a/gtsam/base/concepts.h b/gtsam/base/concepts.h
index 8dac2d55d..4667e87fc 100644
--- a/gtsam/base/concepts.h
+++ b/gtsam/base/concepts.h
@@ -206,6 +206,36 @@ struct LieGroup {
 
 }  // namespace internal
 
+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)");
+  typedef traits_x<ManifoldType> Traits;
+  typedef typename Traits::TangentVector TangentVector;
+  enum { dimension = Traits::dimension };
+  typedef OptionalJacobian<dimension, dimension> ChartJacobian;
+
+  static TangentVector Local(const ManifoldType& other) {
+    return Traits::Local(Traits::Identity(), other);
+  }
+
+  static ManifoldType Retract(const TangentVector& v) {
+    return Traits::Retract(Traits::Identity(), v);
+  }
+
+  static TangentVector Local(const ManifoldType& other,
+                             ChartJacobian Hother) {
+    return Traits::Local(Traits::Identity(), other, boost::none, Hother);
+  }
+
+  static ManifoldType Retract(const ManifoldType& origin,
+                              const TangentVector& v,
+                              ChartJacobian Horigin,
+                              ChartJacobian Hv) {
+    return Traits::Retract(Traits::Identity(), v, boost::none, Hv);
+  }
+};
 
 /// Check invariants for Manifold type
 template<typename T>
diff --git a/gtsam/geometry/Cal3Bundler.h b/gtsam/geometry/Cal3Bundler.h
index fc1d63e10..8adbce06c 100644
--- a/gtsam/geometry/Cal3Bundler.h
+++ b/gtsam/geometry/Cal3Bundler.h
@@ -37,6 +37,8 @@ private:
 
 public:
 
+  enum { dimension = 3 };
+
   /// @name Standard Constructors
   /// @{
 
@@ -169,24 +171,7 @@ private:
 
 };
 
-// Define GTSAM traits
-namespace traits {
-
 template<>
-struct GTSAM_EXPORT is_manifold<Cal3Bundler> : public boost::true_type{
-};
-
-template<>
-struct GTSAM_EXPORT dimension<Cal3Bundler> : public boost::integral_constant<int, 3>{
-};
-
-template<>
-struct GTSAM_EXPORT zero<Cal3Bundler> {
-  static Cal3Bundler value() {
-    return Cal3Bundler(0, 0, 0);
-  }
-};
-
-}
+struct traits_x<Cal3Bundler> : public internal::Manifold<Cal3Bundler> {};
 
 } // namespace gtsam
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index fb338431a..6ad9706de 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -43,6 +43,8 @@ class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
 
 public:
 
+  enum { dimension = 9 };
+
   /// @name Standard Constructors
   /// @{
 
@@ -107,18 +109,8 @@ private:
 
 };
 
-// Define GTSAM traits
-namespace traits {
-
 template<>
-struct GTSAM_EXPORT is_manifold<Cal3DS2> : public boost::true_type{
-};
-
-template<>
-struct GTSAM_EXPORT dimension<Cal3DS2> : public boost::integral_constant<int, 9>{
-};
-
-}
+struct traits_x<Cal3DS2> : public internal::Manifold<Cal3DS2> {};
 
 }
 
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index 133e330ba..4513346d8 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -50,6 +50,7 @@ private:
   double xi_;  // mirror parameter
 
 public:
+  enum { dimension = 10 };
 
     Vector vector() const ;
 
@@ -138,23 +139,8 @@ private:
 
 };
 
-// Define GTSAM traits
-namespace traits {
-
 template<>
-struct GTSAM_EXPORT is_manifold<Cal3Unified> : public boost::true_type{
-};
-
-template<>
-struct GTSAM_EXPORT dimension<Cal3Unified> : public boost::integral_constant<int, 10>{
-};
-
-template<>
-struct GTSAM_EXPORT zero<Cal3Unified> {
-  static Cal3Unified value() { return Cal3Unified();}
-};
-
-}
+struct traits_x<Cal3Unified> : public internal::Manifold<Cal3Unified> {};
 
 }
 
diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h
index cf358d0b2..aacb5e84c 100644
--- a/gtsam/geometry/Cal3_S2.h
+++ b/gtsam/geometry/Cal3_S2.h
@@ -36,7 +36,7 @@ private:
   double fx_, fy_, s_, u0_, v0_;
 
 public:
-
+  enum { dimension = 5 };
   typedef boost::shared_ptr<Cal3_S2> shared_ptr; ///< shared pointer to calibration object
 
   /// @name Standard Constructors
@@ -224,22 +224,7 @@ private:
 
 };
 
-// Define GTSAM traits
-namespace traits {
-
 template<>
-struct GTSAM_EXPORT is_manifold<Cal3_S2> : public boost::true_type{
-};
-
-template<>
-struct GTSAM_EXPORT dimension<Cal3_S2> : public boost::integral_constant<int, 5>{
-};
-
-template<>
-struct GTSAM_EXPORT zero<Cal3_S2> {
-  static Cal3_S2 value() { return Cal3_S2();}
-};
-
-}
+struct traits_x<Cal3_S2> : public internal::Manifold<Cal3_S2> {};
 
 } // \ namespace gtsam
diff --git a/gtsam/geometry/CalibratedCamera.h b/gtsam/geometry/CalibratedCamera.h
index f5a8b4469..bac0ac0e8 100644
--- a/gtsam/geometry/CalibratedCamera.h
+++ b/gtsam/geometry/CalibratedCamera.h
@@ -44,6 +44,8 @@ private:
 
 public:
 
+  enum { dimension = 6 };
+
   /// @name Standard Constructors
   /// @{
 
@@ -199,23 +201,8 @@ private:
   /// @}
 };
 
-// Define GTSAM traits
-namespace traits {
-
 template<>
-struct GTSAM_EXPORT is_group<CalibratedCamera> : public boost::true_type {
-};
-
-template<>
-struct GTSAM_EXPORT is_manifold<CalibratedCamera> : public boost::true_type {
-};
-
-template<>
-struct GTSAM_EXPORT dimension<CalibratedCamera> : public boost::integral_constant<
-    int, 6> {
-};
-
-}
+struct traits_x<CalibratedCamera> : public internal::LieGroup<CalibratedCamera> {};
 
 }
 
diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h
index b25286412..107ddb3fb 100644
--- a/gtsam/geometry/EssentialMatrix.h
+++ b/gtsam/geometry/EssentialMatrix.h
@@ -30,6 +30,8 @@ private:
 
 public:
 
+  enum { dimension = 5 };
+
   /// Static function to convert Point2 to homogeneous coordinates
   static Vector Homogeneous(const Point2& p) {
     return (Vector(3) << p.x(), p.y(), 1).finished();
@@ -196,23 +198,8 @@ private:
 
 };
 
-// Define GTSAM traits
-namespace traits {
-
 template<>
-struct GTSAM_EXPORT is_manifold<EssentialMatrix> : public boost::true_type{
-};
-
-template<>
-struct GTSAM_EXPORT dimension<EssentialMatrix> : public boost::integral_constant<int, 5>{
-};
-
-template<>
-struct GTSAM_EXPORT zero<EssentialMatrix> {
-  static EssentialMatrix value() { return EssentialMatrix();}
-};
-
-}
+struct traits_x<EssentialMatrix> : public internal::LieGroup<EssentialMatrix> {};
 
 } // gtsam
 
diff --git a/gtsam/geometry/Point2.h b/gtsam/geometry/Point2.h
index 8c8e03db4..618d2ed94 100644
--- a/gtsam/geometry/Point2.h
+++ b/gtsam/geometry/Point2.h
@@ -39,7 +39,7 @@ private:
   double x_, y_;
 
 public:
-
+  enum { dimension = 3 };
   /// @name Standard Constructors
   /// @{
 
@@ -245,22 +245,8 @@ private:
 /// multiply with scalar
 inline Point2 operator*(double s, const Point2& p) {return p*s;}
 
-// Define GTSAM traits
-namespace traits {
-
 template<>
-struct GTSAM_EXPORT is_group<Point2> : public boost::true_type{
-};
-
-template<>
-struct GTSAM_EXPORT is_manifold<Point2> : public boost::true_type{
-};
-
-template<>
-struct GTSAM_EXPORT dimension<Point2> : public boost::integral_constant<int, 2>{
-};
-
-}
+struct traits_x<Point2> : public internal::LieGroup<Point2> {};
 
 }
 
diff --git a/gtsam/geometry/Point3.h b/gtsam/geometry/Point3.h
index 56d9b8bef..b256fa9f3 100644
--- a/gtsam/geometry/Point3.h
+++ b/gtsam/geometry/Point3.h
@@ -43,6 +43,7 @@ namespace gtsam {
     double x_, y_, z_;  
     
   public:
+    enum { dimension = 3 };
 
     /// @name Standard Constructors
     /// @{
@@ -248,20 +249,6 @@ 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 GTSAM_EXPORT is_group<Point3> : public boost::true_type{
-  };
-
-  template<>
-  struct GTSAM_EXPORT is_manifold<Point3> : public boost::true_type{
-  };
-
-  template<>
-  struct GTSAM_EXPORT dimension<Point3> : public boost::integral_constant<int, 3>{
-  };
-
-  }
+  struct traits_x<Point3> : public internal::LieGroup<Point3> {};
 }
diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h
index 49eb444b2..33782fdb4 100644
--- a/gtsam/geometry/Pose2.h
+++ b/gtsam/geometry/Pose2.h
@@ -47,6 +47,8 @@ private:
 
 public:
 
+  enum { dimension = 3 };
+
   /// @name Standard Constructors
   /// @{
 
@@ -298,18 +300,8 @@ 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 GTSAM_EXPORT is_manifold<Pose2> : public boost::true_type{
-};
-
-template<>
-struct GTSAM_EXPORT dimension<Pose2> : public boost::integral_constant<int, 3>{
-};
-
-}
+struct traits_x<Pose2> : public internal::LieGroup<Pose2> {};
 
 } // namespace gtsam
 
diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h
index 6a7ed8456..140db7b41 100644
--- a/gtsam/geometry/Pose3.h
+++ b/gtsam/geometry/Pose3.h
@@ -53,6 +53,8 @@ private:
 
 public:
 
+  enum { dimension = 3 };
+
   /// @name Standard Constructors
   /// @{
 
@@ -354,21 +356,7 @@ 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 GTSAM_EXPORT is_group<Pose3> : public boost::true_type{
-};
-
-template<>
-struct GTSAM_EXPORT is_manifold<Pose3> : public boost::true_type{
-};
-
-template<>
-struct GTSAM_EXPORT dimension<Pose3> : public boost::integral_constant<int, 6>{
-};
-
-}
+struct traits_x<Pose3> : public internal::LieGroup<Pose3> {};
 
 } // namespace gtsam
diff --git a/gtsam/geometry/Rot2.h b/gtsam/geometry/Rot2.h
index 744e246b3..4ede4326f 100644
--- a/gtsam/geometry/Rot2.h
+++ b/gtsam/geometry/Rot2.h
@@ -241,20 +241,7 @@ namespace gtsam {
 
   };
 
-  // Define GTSAM traits
-  namespace traits {
-
   template<>
-  struct GTSAM_EXPORT is_group<Rot2> : public boost::true_type{
-  };
+  struct traits_x<Rot2> : public internal::LieGroup<Rot2> {};
 
-  template<>
-  struct GTSAM_EXPORT is_manifold<Rot2> : public boost::true_type{
-  };
-
-  template<>
-  struct GTSAM_EXPORT dimension<Rot2> : public boost::integral_constant<int, 1>{
-  };
-
-  }
 } // gtsam
diff --git a/gtsam/geometry/StereoCamera.h b/gtsam/geometry/StereoCamera.h
index b69da71bd..1d03352da 100644
--- a/gtsam/geometry/StereoCamera.h
+++ b/gtsam/geometry/StereoCamera.h
@@ -44,6 +44,8 @@ private:
 
 public:
 
+  enum { dimension = 6 };
+
   /// @name Standard Constructors
   /// @{
 
@@ -150,22 +152,7 @@ private:
 
 };
 
-// Define GTSAM traits
-namespace traits {
-
 template<>
-struct GTSAM_EXPORT is_manifold<StereoCamera> : public boost::true_type{
-};
-
-template<>
-struct GTSAM_EXPORT dimension<StereoCamera> : public boost::integral_constant<int, 6>{
-};
-
-template<>
-struct GTSAM_EXPORT zero<StereoCamera> {
-  static StereoCamera value() { return StereoCamera();}
-};
-
-}
+struct traits_x<StereoCamera> : public internal::LieGroup<StereoCamera> {};
 
 }
diff --git a/gtsam/geometry/StereoPoint2.h b/gtsam/geometry/StereoPoint2.h
index 694bf525b..fcd37c5d8 100644
--- a/gtsam/geometry/StereoPoint2.h
+++ b/gtsam/geometry/StereoPoint2.h
@@ -174,20 +174,6 @@ namespace gtsam {
 
   };
 
-  // Define GTSAM traits
-  namespace traits {
-
   template<>
-  struct is_group<StereoPoint2> : public boost::true_type {
-  };
-
-  template<>
-  struct is_manifold<StereoPoint2> : public boost::true_type {
-  };
-
-  template<>
-  struct dimension<StereoPoint2> : public boost::integral_constant<int, 3> {
-  };
-
-  }
+  struct traits_x<StereoPoint2> : public internal::LieGroup<StereoPoint2> {};
 }
diff --git a/gtsam/nonlinear/AdaptAutoDiff.h b/gtsam/nonlinear/AdaptAutoDiff.h
index 5118426b4..9ba59d977 100644
--- a/gtsam/nonlinear/AdaptAutoDiff.h
+++ b/gtsam/nonlinear/AdaptAutoDiff.h
@@ -28,9 +28,9 @@ namespace gtsam {
 template<typename F, typename T, typename A1, typename A2>
 class AdaptAutoDiff {
 
-  static const int N = traits::dimension<T>::value;
-  static const int M1 = traits::dimension<A1>::value;
-  static const int M2 = traits::dimension<A2>::value;
+  static const int N = traits:_x<T>:dimension;
+  static const int M1 = traits_x<A1>::dimension;
+  static const int M2 = traits_x<A2>::dimension;
 
   typedef Eigen::Matrix<double, N, M1, Eigen::RowMajor> RowMajor1;
   typedef Eigen::Matrix<double, N, M2, Eigen::RowMajor> RowMajor2;