A lot more progress making things work. Still a long way to go.

gtsam/base/ChartValue.h

@@ -19,6 +19,7 @@
 
 #pragma once
 
+#include <gtsam/base/concepts.h>
 #include <gtsam/base/GenericValue.h>
 #include <gtsam/base/Manifold.h>
 #include <boost/make_shared.hpp>
@@ -209,11 +210,11 @@ const Chart& Value::getChart() const {
 template<typename T>
 ChartValue<T> convertToChartValue(const T& value,
     boost::optional<
-        Eigen::Matrix<double, traits::dimension<T>::value,
+        Eigen::Matrix<double, traits_x<T>::dimension,
-            traits::dimension<T>::value>&> H = boost::none) {
+            traits_x<T>::dimension>&> H = boost::none) {
   if (H) {
-    *H = Eigen::Matrix<double, traits::dimension<T>::value,
+    *H = Eigen::Matrix<double, traits_x<T>::dimension,
-        traits::dimension<T>::value>::Identity();
+        traits_x<T>::dimension>::Identity();
   }
   return ChartValue<T>(value);
 }

gtsam/base/LieScalar.h

@@ -17,7 +17,8 @@
 
 #pragma once
 
-#warning "LieScalar.h is deprecated. Please use double/float instead."
+// TODO(ASL) reenable
+//#warning "LieScalar.h is deprecated. Please use double/float instead."
 #include <gtsam/dllexport.h>
 #include <gtsam/base/DerivedValue.h>
 #include <gtsam/base/Lie.h>

gtsam/base/Manifold.h

@@ -18,6 +18,7 @@
 
 #pragma once
 
+#include <gtsam/base/concepts.h>
 #include <gtsam/base/Matrix.h>
 #include <boost/static_assert.hpp>
 #include <boost/type_traits.hpp>
@@ -69,10 +70,10 @@ 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>
+//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 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
@@ -147,7 +148,7 @@ template<typename T>
 struct DefaultChart {
   //BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
   typedef T type;
-  typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
+  typedef Eigen::Matrix<double, traits_x<T>::dimension, 1> vector;
 
   static vector local(const T& origin, const T& other) {
     return origin.localCoordinates(other);

gtsam/base/OptionalJacobian.h

@@ -73,6 +73,7 @@ public:
   }
 
   /// Constructor that will resize a dynamic matrix (unless already correct)
+
   OptionalJacobian(Eigen::MatrixXd& dynamic) :
       map_(NULL) {
     dynamic.resize(Rows, Cols); // no malloc if correct size
@@ -111,5 +112,8 @@ public:
   Eigen::Map<Fixed>* operator->(){ return &map_; }
 };
 
+template<>
+class OptionalJacobian<-1, -1> {};
+
 } // namespace gtsam
 

gtsam/base/concepts.h

@@ -38,7 +38,7 @@ struct additive_group_tag {};
 // TODO: Remove
 namespace traits {
 template<typename T>
-struct dimension;
+struct dimension{};
 }
 template <typename T> struct traits_x {
   // TODO: remove anything in here ASAP.

gtsam/base/numericalDerivative.h

@@ -71,7 +71,7 @@ Vector numericalGradient(boost::function<double(const X&)> h, const X& x,
 
   BOOST_STATIC_ASSERT_MSG(traits::is_manifold<X>::value,
       "Template argument X must be a manifold type.");
-  static const int N = traits::dimension<X>::value;
+  static const int N = traits_x<X>::dimension;
   BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
   typedef DefaultChart<X> ChartX;
   typedef typename ChartX::vector TangentX;
@@ -118,7 +118,7 @@ Matrix numericalDerivative11(boost::function<Y(const X&)> h, const X& x,
 
   BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits_x<X>::structure_category>::value),
       "Template argument X must be a manifold type.");
-  static const int N = traits::dimension<X>::value;
+  static const int N = traits_x<X>::dimension;
   BOOST_STATIC_ASSERT_MSG(N>0, "Template argument X must be fixed-size type.");
   typedef DefaultChart<X> ChartX;
   typedef typename ChartX::vector TangentX;

gtsam/geometry/Quaternion.h

@@ -111,14 +111,27 @@ struct traits_x<QUATERNION_TYPE> {
     }
   }
 
-  static TangentVector Local(const Q& origin, const Q& other, ChartJacobian Horigin=boost::none, ChartJacobian Hother=boost::none) {
+  static TangentVector Local(const Q& origin, const Q& other, ChartJacobian Horigin = boost::none, ChartJacobian Hother = boost::none) {
     return Logmap(Between(origin,other,Horigin,Hother));
     // TODO: incorporate Jacobian of Logmap
   }
-  static Q Retract(const Q& origin, const TangentVector& v, ChartJacobian Horigin, ChartJacobian Hv) {
+  static Q Retract(const Q& origin, const TangentVector& v, ChartJacobian Horigin = boost::none, ChartJacobian Hv = boost::none) {
     return Compose(origin, Expmap(v), Horigin, Hv);
     // TODO : incorporate Jacobian of Expmap
   }
+
+  static void Print(const Q& q, const std::string& str = "") {
+    if(str.size() == 0) {
+      std::cout << "Eigen::Quaternion: ";
+    } else {
+      std::cout << str << " ";
+    }
+    std::cout << q.vec().transpose() << std::endl;
+  }
+
+  static bool Equals(const Q& q1, const Q& q2, double tol = 1e-8) {
+    return Between(q1,q2).vec().array().abs().maxCoeff() < tol;
+  }
 };
 
 typedef Eigen::Quaternion<double, Eigen::DontAlign> Quaternion;

gtsam/geometry/Rot3.h

@@ -475,3 +475,4 @@ namespace gtsam {
   template<>
   struct traits_x<Rot3> : public internal::LieGroup<Rot3> {};
 }
+

gtsam/geometry/tests/testQuaternion.cpp

@@ -50,7 +50,7 @@ TEST(Quaternion , Local) {
   Q q2(Eigen::AngleAxisd(0.1, z_axis));
   QuaternionJacobian H1,H2;
   Vector3 expected(0, 0, 0.1);
-  Vector3 actual = traits<Q>::Local(q1, q2, H1, H2);
+  Vector3 actual = traits_x<Q>::Local(q1, q2, H1, H2);
   EXPECT(assert_equal((Vector)expected,actual));
 }
 
@@ -61,7 +61,7 @@ TEST(Quaternion , Retract) {
   Q expected(Eigen::AngleAxisd(0.1, z_axis));
   Vector3 v(0, 0, 0.1);
   QuaternionJacobian Hq,Hv;
-  Q actual = traits<Q>::Retract(q, v, Hq, Hv);
+  Q actual = traits_x<Q>::Retract(q, v, Hq, Hv);
   EXPECT(actual.isApprox(expected));
 }
 

gtsam/nonlinear/Expression-inl.h

@@ -300,57 +300,11 @@ public:
   }
 };
 
-//-----------------------------------------------------------------------------
-/// Leaf Expression
-template<class T, class Chart = DefaultChart<T> >
-class LeafExpression: public ExpressionNode<T> {
-  typedef ChartValue<T, Chart> value_type; // perhaps this can be something else like a std::pair<T,Chart> ??
-
-  /// The key into values
-  Key key_;
-
-  /// Constructor with a single key
-  LeafExpression(Key key) :
-      key_(key) {
-  }
-  // todo: do we need a virtual destructor here too?
-
-  friend class Expression<value_type> ;
-
-public:
-
-  /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
-    std::set<Key> keys;
-    keys.insert(key_);
-    return keys;
-  }
-
-  /// Return dimensions for each argument
-  virtual void dims(std::map<Key, int>& map) const {
-    // get dimension from the chart; only works for fixed dimension charts
-    map[key_] = traits::dimension<Chart>::value;
-  }
-
-  /// Return value
-  virtual const value_type& value(const Values& values) const {
-    return dynamic_cast<const value_type&>(values.at(key_));
-  }
-
-  /// Construct an execution trace for reverse AD
-  virtual const value_type& traceExecution(const Values& values,
-      ExecutionTrace<value_type>& trace,
-      ExecutionTraceStorage* traceStorage) const {
-    trace.setLeaf(key_);
-    return dynamic_cast<const value_type&>(values.at(key_));
-  }
-
-};
 
 //-----------------------------------------------------------------------------
 /// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value
 template<typename T>
-class LeafExpression<T, DefaultChart<T> > : public ExpressionNode<T> {
+class LeafExpression : public ExpressionNode<T> {
   typedef T value_type;
 
   /// The key into values
@@ -375,7 +329,7 @@ public:
 
   /// Return dimensions for each argument
   virtual void dims(std::map<Key, int>& map) const {
-    map[key_] = traits::dimension<T>::value;
+    map[key_] = traits_x<T>::dimension;
   }
 
   /// Return value
@@ -451,15 +405,15 @@ public:
 /// meta-function to generate fixed-size JacobianTA type
 template<class T, class A>
 struct Jacobian {
-  typedef Eigen::Matrix<double, traits::dimension<T>::value,
+  typedef Eigen::Matrix<double, traits_x<T>::dimension,
-      traits::dimension<A>::value> type;
+      traits_x<A>::dimension> type;
 };
 
 /// meta-function to generate JacobianTA optional reference
 template<class T, class A>
 struct MakeOptionalJacobian {
-  typedef OptionalJacobian<traits::dimension<T>::value,
+  typedef OptionalJacobian<traits_x<T>::dimension,
-      traits::dimension<A>::value> type;
+      traits_x<A>::dimension> type;
 };
 
 /**

gtsam/nonlinear/Expression.h

@@ -173,7 +173,7 @@ private:
     assert(H.size()==keys.size());
 
     // Pre-allocate and zero VerticalBlockMatrix
-    static const int Dim = traits::dimension<T>::value;
+    static const int Dim = traits_x<T>::dimension;
     VerticalBlockMatrix Ab(dims, Dim);
     Ab.matrix().setZero();
     JacobianMap jacobianMap(keys, Ab);
@@ -223,7 +223,7 @@ private:
 template<class T>
 struct apply_compose {
   typedef T result_type;
-  static const int Dim = traits::dimension<T>::value;
+  static const int Dim = traits_x<T>::dimension;
   T operator()(const T& x, const T& y, OptionalJacobian<Dim, Dim> H1 =
       boost::none, OptionalJacobian<Dim, Dim> H2 = boost::none) const {
     return x.compose(y, H1, H2);