attemping to expose ChartValue for expressions with non DefaultCharts, but needs testing

gtsam/base/ChartValue.h

@@ -139,6 +139,11 @@ private:
   struct PoolTag { };
 };
 
+namespace traits {
+template <typename T, typename Chart>
+struct dimension<ChartValue<T,Chart> > : public dimension<Chart> {};
+}
+
 template<typename Chart>
 const Chart& Value::getChart() const {
 // define Value::cast here since now ChartValue has been declared

gtsam/base/Manifold.h

@@ -161,8 +161,11 @@ struct DefaultChart {
     return origin.dim();
   }
 };
+
 namespace traits {
+// populate default traits
 template <typename T> struct is_chart<DefaultChart<T> > : public std::true_type {};
+template <typename T> struct dimension<DefaultChart<T> > : public dimension<T> {};
 }
 
 template<class C>

gtsam_unstable/nonlinear/Expression-inl.h

@@ -293,8 +293,9 @@ public:
 
 //-----------------------------------------------------------------------------
 /// Leaf Expression
-template<class T>
+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_;
@@ -303,6 +304,53 @@ class LeafExpression: public ExpressionNode<T> {
   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, size_t>& 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,
+      char* raw) 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> {
+  typedef T value_type;
+
+  /// 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<T> ;
 
@@ -374,7 +422,7 @@ struct Jacobian {
 
 /// meta-function to generate JacobianTA optional reference
 template<class T, class A>
-struct Optional {
+struct OptionalJacobian {
   typedef Eigen::Matrix<double, traits::dimension<T>::value,
       traits::dimension<A>::value> Jacobian;
   typedef boost::optional<Jacobian&> type;
@@ -504,7 +552,7 @@ struct FunctionalNode {
     // Argument types and derived, note these are base 0 !
     typedef TYPES Arguments;
     typedef typename boost::mpl::transform<TYPES, Jacobian<T, MPL::_1> >::type Jacobians;
-    typedef typename boost::mpl::transform<TYPES, Optional<T, MPL::_1> >::type Optionals;
+    typedef typename boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
 
     /// Reset expression shared pointer
     template<class A, size_t N>
@@ -559,7 +607,7 @@ class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
 
 public:
 
-  typedef boost::function<T(const A1&, typename Optional<T, A1>::type)> Function;
+  typedef boost::function<T(const A1&, typename OptionalJacobian<T, A1>::type)> Function;
   typedef typename FunctionalNode<T, boost::mpl::vector<A1> >::type Base;
   typedef typename Base::Record Record;
 
@@ -604,8 +652,8 @@ class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::t
 public:
 
   typedef boost::function<
-      T(const A1&, const A2&, typename Optional<T, A1>::type,
-          typename Optional<T, A2>::type)> Function;
+      T(const A1&, const A2&, typename OptionalJacobian<T, A1>::type,
+          typename OptionalJacobian<T, A2>::type)> Function;
   typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
   typedef typename Base::Record Record;
 
@@ -658,8 +706,8 @@ class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3>
 public:
 
   typedef boost::function<
-      T(const A1&, const A2&, const A3&, typename Optional<T, A1>::type,
-          typename Optional<T, A2>::type, typename Optional<T, A3>::type)> Function;
+      T(const A1&, const A2&, const A3&, typename OptionalJacobian<T, A1>::type,
+          typename OptionalJacobian<T, A2>::type, typename OptionalJacobian<T, A3>::type)> Function;
   typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
   typedef typename Base::Record Record;
 

gtsam_unstable/nonlinear/Expression.h

@@ -61,7 +61,7 @@ public:
   /// Construct a nullary method expression
   template<typename A>
   Expression(const Expression<A>& expression,
-      T (A::*method)(typename Optional<T, A>::type) const) :
+      T (A::*method)(typename OptionalJacobian<T, A>::type) const) :
       root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)) {
   }
 
@@ -75,8 +75,8 @@ public:
   /// Construct a unary method expression
   template<typename A1, typename A2>
   Expression(const Expression<A1>& expression1,
-      T (A1::*method)(const A2&, typename Optional<T, A1>::type,
-          typename Optional<T, A2>::type) const,
+      T (A1::*method)(const A2&, typename OptionalJacobian<T, A1>::type,
+          typename OptionalJacobian<T, A2>::type) const,
       const Expression<A2>& expression2) :
       root_(
           new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),

gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp

@@ -201,7 +201,7 @@ Pose3  pose;
   // Now, let's create the optional Jacobian arguments
   typedef Point3 T;
   typedef boost::mpl::vector<Pose3, Point3> TYPES;
-  typedef boost::mpl::transform<TYPES, Optional<T, MPL::_1> >::type Optionals;
+  typedef boost::mpl::transform<TYPES, OptionalJacobian<T, MPL::_1> >::type Optionals;
 
   // Unfortunately this is moot: we need a pointer to a function with the
   // optional derivatives; I don't see a way of calling a function that we