Merge branch 'feature/BAD_linearize' into feature/BAD: Now awesome performance!

In this I branch essentially implemented two things
1) I used template-meta-programming to automate adding more functional nodes
2) I overloaded "linearize" so that now reverse AD writes to Eigen::Block<Matrix> views of the VerticalBlockMatrix that will be passed to the JacobianFacor. This did wonders for performance, making the AD pipeline faster than dedicated factors. Because the latter is based on optional<Matrix> references, they cannot apply the same trick :-(
3) Finally, I removed the forward AD pipeline. Not worth keeping (this implementation) alive.

.cproject

@@ -600,7 +600,6 @@
 			</target>
 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -608,7 +607,6 @@
 			</target>
 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testBinaryBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -656,7 +654,6 @@
 			</target>
 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNet.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -664,7 +661,6 @@
 			</target>
 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testSymbolicFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -672,7 +668,6 @@
 			</target>
 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicFactorGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -688,7 +683,6 @@
 			</target>
 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>tests/testBayesTree</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1040,7 +1034,6 @@
 			</target>
 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testErrors.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1270,46 +1263,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testBTree.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSF.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSFMap.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDSFVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testFixedVector.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1392,6 +1345,7 @@
 			</target>
 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2DOriented.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1431,6 +1385,7 @@
 			</target>
 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated2D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1438,6 +1393,7 @@
 			</target>
 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSimulated3D.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1451,6 +1407,46 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testBTree.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSF.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFMap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDSFVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testFixedVector.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -1708,7 +1704,6 @@
 			</target>
 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G DEB</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1716,7 +1711,6 @@
 			</target>
 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G RPM</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1724,7 +1718,6 @@
 			</target>
 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>-G TGZ</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1732,7 +1725,6 @@
 			</target>
 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
-				<buildArguments/>
 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2459,7 +2451,6 @@
 			</target>
 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2467,7 +2458,6 @@
 			</target>
 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testJunctionTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2475,7 +2465,6 @@
 			</target>
 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments/>
 				<buildTarget>testSymbolicBayesNetB.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2561,6 +2550,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testExpressionMeta.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testExpressionMeta.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -2963,6 +2960,7 @@
 			</target>
 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>tests/testGaussianISAM2</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>

gtsam/nonlinear/NonlinearFactor.cpp

@@ -72,24 +72,24 @@ bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
               && noiseModel_->equals(*e->noiseModel_, tol)));
 }
 
-static void check(const SharedNoiseModel& noiseModel, const Vector& b) {
+static void check(const SharedNoiseModel& noiseModel, size_t m) {
   if (!noiseModel)
     throw std::invalid_argument("NoiseModelFactor: no NoiseModel.");
-  if ((size_t) b.size() != noiseModel->dim())
+  if (m != noiseModel->dim())
     throw std::invalid_argument(
         "NoiseModelFactor was created with a NoiseModel of incorrect dimension.");
 }
 
 Vector NoiseModelFactor::whitenedError(const Values& c) const {
   const Vector b = unwhitenedError(c);
-  check(noiseModel_, b);
+  check(noiseModel_, b.size());
   return noiseModel_->whiten(b);
 }
 
 double NoiseModelFactor::error(const Values& c) const {
   if (this->active(c)) {
     const Vector b = unwhitenedError(c);
-    check(noiseModel_, b);
+    check(noiseModel_, b.size());
     return 0.5 * noiseModel_->distance(b);
   } else {
     return 0.0;
@@ -106,7 +106,7 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
   // Call evaluate error to get Jacobians and RHS vector b
   std::vector<Matrix> A(this->size());
   Vector b = -unwhitenedError(x, A);
-  check(noiseModel_, b);
+  check(noiseModel_, b.size());
 
   // Whiten the corresponding system now
   this->noiseModel_->WhitenSystem(A, b);
@@ -124,7 +124,7 @@ boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
       boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
   if (constrained) {
     // Create a factor of reduced row dimension d_
-    size_t d_ = terms[0].second.rows() - constrained->dim();
+    size_t d_ = b.size() - constrained->dim();
     Vector zero_ = zero(d_);
     Vector b_ = concatVectors(2, &b, &zero_);
     noiseModel::Constrained::shared_ptr model = constrained->unit(d_);

gtsam_unstable/nonlinear/Expression-inl.h

@@ -24,8 +24,8 @@
 #include <gtsam/base/Testable.h>
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
-#include <new> // for placement new
+#include <boost/range/adaptor/map.hpp>
-struct TestBinaryExpression;
+#include <boost/range/algorithm.hpp>
 
 // template meta-programming headers
 #include <boost/mpl/vector.hpp>
@@ -35,15 +35,21 @@ struct TestBinaryExpression;
 #include <boost/mpl/fold.hpp>
 #include <boost/mpl/empty_base.hpp>
 #include <boost/mpl/placeholders.hpp>
-
+#include <boost/mpl/transform.hpp>
+#include <boost/mpl/at.hpp>
 namespace MPL = boost::mpl::placeholders;
 
+#include <new> // for placement new
+
+class ExpressionFactorBinaryTest;
+// Forward declare for testing
+
 namespace gtsam {
 
 template<typename T>
 class Expression;
 
-typedef std::map<Key, Matrix> JacobianMap;
+typedef std::map<Key, Eigen::Block<Matrix> > JacobianMap;
 
 //-----------------------------------------------------------------------------
 /**
@@ -55,6 +61,7 @@ typedef std::map<Key, Matrix> JacobianMap;
  */
 template<int COLS>
 struct CallRecord {
+  static size_t const N = 0;
   virtual void print(const std::string& indent) const {
   }
   virtual void startReverseAD(JacobianMap& jacobians) const {
@@ -79,7 +86,7 @@ template<class T>
 class ExecutionTrace {
   enum {
     Constant, Leaf, Function
-  } type;
+  } kind;
   union {
     Key key;
     CallRecord<T::dimension>* ptr;
@@ -87,25 +94,25 @@ class ExecutionTrace {
 public:
   /// Pointer always starts out as a Constant
   ExecutionTrace() :
-      type(Constant) {
+      kind(Constant) {
   }
   /// Change pointer to a Leaf Record
   void setLeaf(Key key) {
-    type = Leaf;
+    kind = Leaf;
     content.key = key;
   }
   /// Take ownership of pointer to a Function Record
   void setFunction(CallRecord<T::dimension>* record) {
-    type = Function;
+    kind = Function;
     content.ptr = record;
   }
   /// Print
   void print(const std::string& indent = "") const {
-    if (type == Constant)
+    if (kind == Constant)
       std::cout << indent << "Constant" << std::endl;
-    else if (type == Leaf)
+    else if (kind == Leaf)
       std::cout << indent << "Leaf, key = " << content.key << std::endl;
-    else if (type == Function) {
+    else if (kind == Function) {
       std::cout << indent << "Function" << std::endl;
       content.ptr->print(indent + "  ");
     }
@@ -113,54 +120,64 @@ public:
   /// Return record pointer, quite unsafe, used only for testing
   template<class Record>
   boost::optional<Record*> record() {
-    if (type != Function)
+    if (kind != Function)
       return boost::none;
     else {
       Record* p = dynamic_cast<Record*>(content.ptr);
       return p ? boost::optional<Record*>(p) : boost::none;
     }
   }
-  // *** This is the main entry point for reverseAD, called from Expression::augmented ***
+  /// reverseAD in case of Leaf
-  // Called only once, either inserts identity into Jacobians (Leaf) or starts AD (Function)
+  template<class Derived>
+  static void handleLeafCase(const Eigen::MatrixBase<Derived>& dTdA,
+      JacobianMap& jacobians, Key key) {
+    JacobianMap::iterator it = jacobians.find(key);
+    if (it == jacobians.end()) {
+      std::cout << "No block for key " << key << std::endl;
+      throw std::runtime_error("Reverse AD internal error");
+    }
+    // we have pre-loaded them with zeros
+    Eigen::Block<Matrix>& block = it->second;
+    block += dTdA;
+  }
+  /**
+   *  *** This is the main entry point for reverseAD, called from Expression ***
+   * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function)
+   */
   void startReverseAD(JacobianMap& jacobians) const {
-    if (type == Leaf) {
+    if (kind == Leaf) {
       // This branch will only be called on trivial Leaf expressions, i.e. Priors
       size_t n = T::Dim();
-      jacobians[content.key] = Eigen::MatrixXd::Identity(n, n);
+      handleLeafCase(Eigen::MatrixXd::Identity(n, n), jacobians, content.key);
-    } else if (type == Function)
+    } else if (kind == Function)
       // This is the more typical entry point, starting the AD pipeline
-      // It is inside the startReverseAD that the correctly dimensioned pipeline is chosen.
+      // Inside the startReverseAD that the correctly dimensioned pipeline is chosen.
       content.ptr->startReverseAD(jacobians);
   }
   // Either add to Jacobians (Leaf) or propagate (Function)
   void reverseAD(const Matrix& dTdA, JacobianMap& jacobians) const {
-    if (type == Leaf) {
+    if (kind == Leaf)
-      JacobianMap::iterator it = jacobians.find(content.key);
+      handleLeafCase(dTdA, jacobians, content.key);
-      if (it != jacobians.end())
+    else if (kind == Function)
-        it->second += dTdA;
-      else
-        jacobians[content.key] = dTdA;
-    } else if (type == Function)
       content.ptr->reverseAD(dTdA, jacobians);
   }
   // Either add to Jacobians (Leaf) or propagate (Function)
   typedef Eigen::Matrix<double, 2, T::dimension> Jacobian2T;
   void reverseAD2(const Jacobian2T& dTdA, JacobianMap& jacobians) const {
-    if (type == Leaf) {
+    if (kind == Leaf)
-      JacobianMap::iterator it = jacobians.find(content.key);
+      handleLeafCase(dTdA, jacobians, content.key);
-      if (it != jacobians.end())
+    else if (kind == Function)
-        it->second += dTdA;
-      else
-        jacobians[content.key] = dTdA;
-    } else if (type == Function)
       content.ptr->reverseAD2(dTdA, jacobians);
   }
+
+  /// Define type so we can apply it as a meta-function
+  typedef ExecutionTrace<T> type;
 };
 
 /// Primary template calls the generic Matrix reverseAD pipeline
-template<size_t M, class A>
+template<size_t ROWS, class A>
 struct Select {
-  typedef Eigen::Matrix<double, M, A::dimension> Jacobian;
+  typedef Eigen::Matrix<double, ROWS, A::dimension> Jacobian;
   static void reverseAD(const ExecutionTrace<A>& trace, const Jacobian& dTdA,
       JacobianMap& jacobians) {
     trace.reverseAD(dTdA, jacobians);
@@ -177,208 +194,6 @@ struct Select<2, A> {
   }
 };
 
-//-----------------------------------------------------------------------------
-/**
- * Record the evaluation of a single argument in a functional expression
- * Building block for Recursive Record Class
- */
-template<class T, class A, size_t N>
-struct Argument {
-  typedef Eigen::Matrix<double, T::dimension, A::dimension> JacobianTA;
-  ExecutionTrace<A> trace;
-  JacobianTA dTdA;
-};
-
-/**
- * Recursive Record Class for Functional Expressions
- * Abrahams, David; Gurtovoy, Aleksey (2004-12-10).
- * C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
- * and Beyond. Pearson Education.
- */
-template<class T, class AN, class More>
-struct Record: Argument<T, typename AN::type, AN::value>, More {
-
-  typedef T return_type;
-  typedef typename AN::type A;
-  const static size_t N = AN::value;
-  typedef Argument<T, A, N> This;
-
-  /// Print to std::cout
-  virtual void print(const std::string& indent) const {
-    More::print(indent);
-    static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
-    std::cout << This::dTdA.format(matlab) << std::endl;
-    This::trace.print(indent);
-  }
-
-  /// Start the reverse AD process
-  virtual void startReverseAD(JacobianMap& jacobians) const {
-    More::startReverseAD(jacobians);
-    Select<T::dimension, A>::reverseAD(This::trace, This::dTdA, jacobians);
-  }
-
-  /// Given df/dT, multiply in dT/dA and continue reverse AD process
-  virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
-    More::reverseAD(dFdT, jacobians);
-    This::trace.reverseAD(dFdT * This::dTdA, jacobians);
-  }
-
-  /// Version specialized to 2-dimensional output
-  typedef Eigen::Matrix<double, 2, T::dimension> Jacobian2T;
-  virtual void reverseAD2(const Jacobian2T& dFdT,
-      JacobianMap& jacobians) const {
-    More::reverseAD2(dFdT, jacobians);
-    This::trace.reverseAD2(dFdT * This::dTdA, jacobians);
-  }
-};
-
-/// Meta-function for generating a numbered type
-template<class A, size_t N>
-struct Numbered {
-  typedef A type;
-  typedef size_t value_type;
-  static const size_t value = N;
-};
-
-/// Recursive Record class Generator
-template<class T, class TYPES>
-struct GenerateRecord {
-  typedef typename boost::mpl::fold<TYPES, CallRecord<T::dimension>,
-      Record<T, MPL::_2, MPL::_1> >::type type;
-};
-
-/// Access Argument
-template<class A, size_t N, class Record>
-Argument<typename Record::return_type, A, N>& argument(Record& record) {
-  return static_cast<Argument<typename Record::return_type, A, N>&>(record);
-}
-
-/// Access Trace
-template<class A, size_t N, class Record>
-ExecutionTrace<A>& getTrace(Record* record) {
-  return argument<A, N>(*record).trace;
-}
-
-/// Access Jacobian
-template<class A, size_t N, class Record>
-Eigen::Matrix<double, Record::return_type::dimension, A::dimension>& jacobian(
-    Record* record) {
-  return argument<A, N>(*record).dTdA;
-}
-
-//-----------------------------------------------------------------------------
-/**
- * Value and Jacobians
- */
-template<class T>
-class Augmented {
-
-private:
-
-  T value_;
-  JacobianMap jacobians_;
-
-  typedef std::pair<Key, Matrix> Pair;
-
-  /// Insert terms into jacobians_, adding if already exists
-  void add(const JacobianMap& terms) {
-    BOOST_FOREACH(const Pair& term, terms) {
-      JacobianMap::iterator it = jacobians_.find(term.first);
-      if (it != jacobians_.end())
-        it->second += term.second;
-      else
-        jacobians_[term.first] = term.second;
-    }
-  }
-
-  /// Insert terms into jacobians_, premultiplying by H, adding if already exists
-  void add(const Matrix& H, const JacobianMap& terms) {
-    BOOST_FOREACH(const Pair& term, terms) {
-      JacobianMap::iterator it = jacobians_.find(term.first);
-      if (it != jacobians_.end())
-        it->second += H * term.second;
-      else
-        jacobians_[term.first] = H * term.second;
-    }
-  }
-
-public:
-
-  /// Construct value that does not depend on anything
-  Augmented(const T& t) :
-      value_(t) {
-  }
-
-  /// Construct value dependent on a single key
-  Augmented(const T& t, Key key) :
-      value_(t) {
-    size_t n = t.dim();
-    jacobians_[key] = Eigen::MatrixXd::Identity(n, n);
-  }
-
-  /// Construct value, pre-multiply jacobians by dTdA
-  Augmented(const T& t, const Matrix& dTdA, const JacobianMap& jacobians) :
-      value_(t) {
-    add(dTdA, jacobians);
-  }
-
-  /// Construct value, pre-multiply jacobians
-  Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1,
-      const Matrix& dTdA2, const JacobianMap& jacobians2) :
-      value_(t) {
-    add(dTdA1, jacobians1);
-    add(dTdA2, jacobians2);
-  }
-
-  /// Construct value, pre-multiply jacobians
-  Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1,
-      const Matrix& dTdA2, const JacobianMap& jacobians2, const Matrix& dTdA3,
-      const JacobianMap& jacobians3) :
-      value_(t) {
-    add(dTdA1, jacobians1);
-    add(dTdA2, jacobians2);
-    add(dTdA3, jacobians3);
-  }
-
-  /// Return value
-  const T& value() const {
-    return value_;
-  }
-
-  /// Return jacobians
-  const JacobianMap& jacobians() const {
-    return jacobians_;
-  }
-
-  /// Return jacobians
-  JacobianMap& jacobians() {
-    return jacobians_;
-  }
-
-  /// Not dependent on any key
-  bool constant() const {
-    return jacobians_.empty();
-  }
-
-  /// debugging
-  void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) {
-    BOOST_FOREACH(const Pair& term, jacobians_)
-      std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows()
-          << "x" << term.second.cols() << ") ";
-    std::cout << std::endl;
-  }
-
-  /// Move terms to array, destroys content
-  void move(std::vector<Matrix>& H) {
-    assert(H.size()==jacobains.size());
-    size_t j = 0;
-    JacobianMap::iterator it = jacobians_.begin();
-    for (; it != jacobians_.end(); ++it)
-      it->second.swap(H[j++]);
-  }
-
-};
-
 //-----------------------------------------------------------------------------
 /**
  * Expression node. The superclass for objects that do the heavy lifting
@@ -392,7 +207,11 @@ class ExpressionNode {
 
 protected:
 
-  ExpressionNode() {
+  size_t traceSize_;
+
+  /// Constructor, traceSize is size of the execution trace of expression rooted here
+  ExpressionNode(size_t traceSize = 0) :
+      traceSize_(traceSize) {
   }
 
 public:
@@ -402,19 +221,33 @@ public:
   }
 
   /// Return keys that play in this expression as a set
-  virtual std::set<Key> keys() const = 0;
+  virtual std::set<Key> keys() const {
+    std::set<Key> keys;
+    return keys;
+  }
+
+  /// Return dimensions for each argument, as a map
+  virtual std::map<Key, size_t> dims() const {
+    std::map<Key, size_t> map;
+    return map;
+  }
+
+  /// Return dimensions as vector, ordered as keys
+  std::vector<size_t> dimensions() const {
+    std::map<Key,size_t> map = dims();
+    std::vector<size_t> dims(map.size());
+    boost::copy(map | boost::adaptors::map_values, dims.begin());
+    return dims;
+  }
+
+  // Return size needed for memory buffer in traceExecution
+  size_t traceSize() const {
+    return traceSize_;
+  }
 
   /// Return value
   virtual T value(const Values& values) const = 0;
 
-  /// Return value and derivatives
-  virtual Augmented<T> forward(const Values& values) const = 0;
-
-  // Return size needed for memory buffer in traceExecution
-  virtual size_t traceSize() const {
-    return 0;
-  }
-
   /// Construct an execution trace for reverse AD
   virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
       char* raw) const = 0;
@@ -437,22 +270,11 @@ class ConstantExpression: public ExpressionNode<T> {
 
 public:
 
-  /// Return keys that play in this expression, i.e., the empty set
-  virtual std::set<Key> keys() const {
-    std::set<Key> keys;
-    return keys;
-  }
-
   /// Return value
   virtual T value(const Values& values) const {
     return constant_;
   }
 
-  /// Return value and derivatives
-  virtual Augmented<T> forward(const Values& values) const {
-    return Augmented<T>(constant_);
-  }
-
   /// Construct an execution trace for reverse AD
   virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
       char* raw) const {
@@ -484,16 +306,18 @@ public:
     return keys;
   }
 
+  /// Return dimensions for each argument
+  virtual std::map<Key, size_t> dims() const {
+    std::map<Key, size_t> map;
+    map[key_] = T::dimension;
+    return map;
+  }
+
   /// Return value
   virtual T value(const Values& values) const {
     return values.at<T>(key_);
   }
 
-  /// Return value and derivatives
-  virtual Augmented<T> forward(const Values& values) const {
-    return Augmented<T>(values.at<T>(key_), key_);
-  }
-
   /// Construct an execution trace for reverse AD
   virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
       char* raw) const {
@@ -504,68 +328,262 @@ public:
 };
 
 //-----------------------------------------------------------------------------
+// Below we use the "Class Composition" technique described in the book
+//   C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost
+//   and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education.
+// to recursively generate a class, that will be the base for function nodes.
+// The class generated, for two arguments A1, A2, and A3 will be
+//
+// struct Base1 : Argument<T,A1,1>, FunctionalBase<T> {
+//   ... storage related to A1 ...
+//   ... methods that work on A1 ...
+// };
+//
+// struct Base2 : Argument<T,A2,2>, Base1 {
+//   ... storage related to A2 ...
+//   ... methods that work on A2 and (recursively) on A2 ...
+// };
+//
+// struct Base2 : Argument<T,A3,3>, Base2 {
+//   ... storage related to A3 ...
+//   ... methods that work on A3 and (recursively) on A2 and A3 ...
+// };
+//
+// struct FunctionalNode : Base3 {
+//   Provides convenience access to storage in hierarchy by using
+//   static_cast<Argument<T, A, N> &>(*this)
+// }
+//
+// All this magic happens when  we generate the Base3 base class of FunctionalNode
+// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode
+//-----------------------------------------------------------------------------
+
+/// meta-function to generate fixed-size JacobianTA type
+template<class T, class A>
+struct Jacobian {
+  typedef Eigen::Matrix<double, T::dimension, A::dimension> type;
+};
+
+/// meta-function to generate JacobianTA optional reference
+template<class T, class A>
+struct Optional {
+  typedef Eigen::Matrix<double, T::dimension, A::dimension> Jacobian;
+  typedef boost::optional<Jacobian&> type;
+};
+
+/**
+ * Base case for recursive FunctionalNode class
+ */
+template<class T>
+struct FunctionalBase: ExpressionNode<T> {
+  static size_t const N = 0; // number of arguments
+
+  typedef CallRecord<T::dimension> Record;
+
+  /// Construct an execution trace for reverse AD
+  void trace(const Values& values, Record* record, char*& raw) const {
+  }
+};
+
+/**
+ * Building block for recursive FunctionalNode class
+ * The integer argument N is to guarantee a unique type signature,
+ * so we are guaranteed to be able to extract their values by static cast.
+ */
+template<class T, class A, size_t N>
+struct Argument {
+  /// Expression that will generate value/derivatives for argument
+  boost::shared_ptr<ExpressionNode<A> > expression;
+};
+
+/**
+ * Building block for Recursive Record Class
+ * Records the evaluation of a single argument in a functional expression
+ */
+template<class T, class A, size_t N>
+struct JacobianTrace {
+  A value;
+  ExecutionTrace<A> trace;
+  typename Jacobian<T, A>::type dTdA;
+};
+
+/**
+ * Recursive Definition of Functional ExpressionNode
+ */
+template<class T, class A, class Base>
+struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
+
+  static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy
+  typedef Argument<T, A, N> This; ///< The storage we have direct access to
+
+  /// Return keys that play in this expression
+  virtual std::set<Key> keys() const {
+    std::set<Key> keys = Base::keys();
+    std::set<Key> myKeys = This::expression->keys();
+    keys.insert(myKeys.begin(), myKeys.end());
+    return keys;
+  }
+
+  /// Return dimensions for each argument
+  virtual std::map<Key, size_t> dims() const {
+    std::map<Key, size_t> map = Base::dims();
+    std::map<Key, size_t> myMap = This::expression->dims();
+    map.insert(myMap.begin(), myMap.end());
+    return map;
+  }
+
+  /// Recursive Record Class for Functional Expressions
+  struct Record: JacobianTrace<T, A, N>, Base::Record {
+
+    typedef T return_type;
+    typedef JacobianTrace<T, A, N> This;
+
+    /// Print to std::cout
+    virtual void print(const std::string& indent) const {
+      Base::Record::print(indent);
+      static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
+      std::cout << This::dTdA.format(matlab) << std::endl;
+      This::trace.print(indent);
+    }
+
+    /// Start the reverse AD process
+    virtual void startReverseAD(JacobianMap& jacobians) const {
+      Base::Record::startReverseAD(jacobians);
+      Select<T::dimension, A>::reverseAD(This::trace, This::dTdA, jacobians);
+    }
+
+    /// Given df/dT, multiply in dT/dA and continue reverse AD process
+    virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const {
+      Base::Record::reverseAD(dFdT, jacobians);
+      This::trace.reverseAD(dFdT * This::dTdA, jacobians);
+    }
+
+    /// Version specialized to 2-dimensional output
+    typedef Eigen::Matrix<double, 2, T::dimension> Jacobian2T;
+    virtual void reverseAD2(const Jacobian2T& dFdT,
+        JacobianMap& jacobians) const {
+      Base::Record::reverseAD2(dFdT, jacobians);
+      This::trace.reverseAD2(dFdT * This::dTdA, jacobians);
+    }
+  };
+
+  /// Construct an execution trace for reverse AD
+  void trace(const Values& values, Record* record, char*& raw) const {
+    Base::trace(values, record, raw); // recurse
+    // Write an Expression<A> execution trace in record->trace
+    // Iff Constant or Leaf, this will not write to raw, only to trace.
+    // Iff the expression is functional, write all Records in raw buffer
+    // Return value of type T is recorded in record->value
+    record->Record::This::value = This::expression->traceExecution(values,
+        record->Record::This::trace, raw);
+    // raw is never modified by traceExecution, but if traceExecution has
+    // written in the buffer, the next caller expects we advance the pointer
+    raw += This::expression->traceSize();
+  }
+};
+
+/**
+ *  Recursive GenerateFunctionalNode class Generator
+ */
+template<class T, class TYPES>
+struct FunctionalNode {
+
+  typedef typename boost::mpl::fold<TYPES, FunctionalBase<T>,
+      GenerateFunctionalNode<T, MPL::_2, MPL::_1> >::type Base;
+
+  struct type: public Base {
+
+    // 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;
+
+    /// Reset expression shared pointer
+    template<class A, size_t N>
+    void reset(const boost::shared_ptr<ExpressionNode<A> >& ptr) {
+      static_cast<Argument<T, A, N> &>(*this).expression = ptr;
+    }
+
+    /// Access Expression shared pointer
+    template<class A, size_t N>
+    boost::shared_ptr<ExpressionNode<A> > expression() const {
+      return static_cast<Argument<T, A, N> const &>(*this).expression;
+    }
+
+    /// Provide convenience access to Record storage
+    struct Record: public Base::Record {
+
+      /// Access Value
+      template<class A, size_t N>
+      const A& value() const {
+        return static_cast<JacobianTrace<T, A, N> const &>(*this).value;
+      }
+
+      /// Access Jacobian
+      template<class A, size_t N>
+      typename Jacobian<T, A>::type& jacobian() {
+        return static_cast<JacobianTrace<T, A, N>&>(*this).dTdA;
+      }
+
+    };
+
+    /// Construct an execution trace for reverse AD
+    Record* trace(const Values& values, char* raw) const {
+
+      // Create the record and advance the pointer
+      Record* record = new (raw) Record();
+      raw = (char*) (record + 1);
+
+      // Record the traces for all arguments
+      // After this, the raw pointer is set to after what was written
+      Base::trace(values, record, raw);
+
+      // Return the record for this function evaluation
+      return record;
+    }
+  };
+};
+//-----------------------------------------------------------------------------
+
 /// Unary Function Expression
 template<class T, class A1>
-class UnaryExpression: public ExpressionNode<T> {
+class UnaryExpression: public FunctionalNode<T, boost::mpl::vector<A1> >::type {
 
 public:
 
-  typedef Eigen::Matrix<double, T::dimension, A1::dimension> JacobianTA;
+  typedef boost::function<T(const A1&, typename Optional<T, A1>::type)> Function;
-  typedef boost::function<T(const A1&, boost::optional<JacobianTA&>)> Function;
+  typedef typename FunctionalNode<T, boost::mpl::vector<A1> >::type Base;
+  typedef typename Base::Record Record;
 
 private:
 
   Function function_;
-  boost::shared_ptr<ExpressionNode<A1> > expressionA1_;
 
   /// Constructor with a unary function f, and input argument e
-  UnaryExpression(Function f, const Expression<A1>& e) :
+  UnaryExpression(Function f, const Expression<A1>& e1) :
-      function_(f), expressionA1_(e.root()) {
+      function_(f) {
+    this->template reset<A1, 1>(e1.root());
+    ExpressionNode<T>::traceSize_ = sizeof(Record) + e1.traceSize();
   }
 
   friend class Expression<T> ;
 
 public:
 
-  /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
-    return expressionA1_->keys();
-  }
-
   /// Return value
   virtual T value(const Values& values) const {
-    return function_(this->expressionA1_->value(values), boost::none);
+    return function_(this->template expression<A1, 1>()->value(values), boost::none);
-  }
-
-  /// Return value and derivatives
-  virtual Augmented<T> forward(const Values& values) const {
-    using boost::none;
-    Augmented<A1> argument = this->expressionA1_->forward(values);
-    JacobianTA dTdA;
-    T t = function_(argument.value(),
-        argument.constant() ? none : boost::optional<JacobianTA&>(dTdA));
-    return Augmented<T>(t, dTdA, argument.jacobians());
-  }
-
-  /// CallRecord structure for reverse AD
-  typedef boost::mpl::vector<Numbered<A1, 1> > Arguments;
-  typedef typename GenerateRecord<T, Arguments>::type Record;
-
-  // Return size needed for memory buffer in traceExecution
-  virtual size_t traceSize() const {
-    return sizeof(Record) + expressionA1_->traceSize();
   }
 
   /// Construct an execution trace for reverse AD
   virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
       char* raw) const {
-    Record* record = new (raw) Record();
+
+    Record* record = Base::trace(values, raw);
     trace.setFunction(record);
 
-    raw = (char*) (record + 1);
+    return function_(record->template value<A1, 1>(),
-    A1 a1 = expressionA1_->traceExecution(values, getTrace<A1, 1>(record), raw);
+        record->template jacobian<A1, 1>());
-
-    return function_(a1, jacobian<A1, 1>(record));
   }
 };
 
@@ -573,183 +591,109 @@ public:
 /// Binary Expression
 
 template<class T, class A1, class A2>
-class BinaryExpression: public ExpressionNode<T> {
+class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::type {
 
 public:
 
-  typedef Eigen::Matrix<double, T::dimension, A1::dimension> JacobianTA1;
-  typedef Eigen::Matrix<double, T::dimension, A2::dimension> JacobianTA2;
   typedef boost::function<
-      T(const A1&, const A2&, boost::optional<JacobianTA1&>,
+      T(const A1&, const A2&, typename Optional<T, A1>::type,
-          boost::optional<JacobianTA2&>)> Function;
+          typename Optional<T, A2>::type)> Function;
+  typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
+  typedef typename Base::Record Record;
 
 private:
 
   Function function_;
-  boost::shared_ptr<ExpressionNode<A1> > expressionA1_;
-  boost::shared_ptr<ExpressionNode<A2> > expressionA2_;
 
-  /// Constructor with a binary function f, and two input arguments
+  /// Constructor with a ternary function f, and three input arguments
-  BinaryExpression(Function f, //
+  BinaryExpression(Function f, const Expression<A1>& e1,
-      const Expression<A1>& e1, const Expression<A2>& e2) :
+      const Expression<A2>& e2) :
-      function_(f), expressionA1_(e1.root()), expressionA2_(e2.root()) {
+      function_(f) {
+    this->template reset<A1, 1>(e1.root());
+    this->template reset<A2, 2>(e2.root());
+    ExpressionNode<T>::traceSize_ = //
+        sizeof(Record) + e1.traceSize() + e2.traceSize();
   }
 
   friend class Expression<T> ;
-  friend struct ::TestBinaryExpression;
+  friend class ::ExpressionFactorBinaryTest;
 
 public:
 
-  /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
-    std::set<Key> keys1 = expressionA1_->keys();
-    std::set<Key> keys2 = expressionA2_->keys();
-    keys1.insert(keys2.begin(), keys2.end());
-    return keys1;
-  }
-
   /// Return value
   virtual T value(const Values& values) const {
     using boost::none;
-    return function_(this->expressionA1_->value(values),
+    return function_(this->template expression<A1, 1>()->value(values),
-        this->expressionA2_->value(values), none, none);
+    this->template expression<A2, 2>()->value(values),
-  }
+    none, none);
-
-  /// Return value and derivatives
-  virtual Augmented<T> forward(const Values& values) const {
-    using boost::none;
-    Augmented<A1> a1 = this->expressionA1_->forward(values);
-    Augmented<A2> a2 = this->expressionA2_->forward(values);
-    JacobianTA1 dTdA1;
-    JacobianTA2 dTdA2;
-    T t = function_(a1.value(), a2.value(),
-        a1.constant() ? none : boost::optional<JacobianTA1&>(dTdA1),
-        a2.constant() ? none : boost::optional<JacobianTA2&>(dTdA2));
-    return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians());
-  }
-
-  /// CallRecord structure for reverse AD
-  typedef boost::mpl::vector<Numbered<A1, 1>, Numbered<A2, 2> > Arguments;
-  typedef typename GenerateRecord<T, Arguments>::type Record;
-
-  // Return size needed for memory buffer in traceExecution
-  virtual size_t traceSize() const {
-    return sizeof(Record) + expressionA1_->traceSize()
-        + expressionA2_->traceSize();
   }
 
   /// Construct an execution trace for reverse AD
-  /// The raw buffer is [Record | A1 raw | A2 raw]
   virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
       char* raw) const {
-    Record* record = new (raw) Record();
+
+    Record* record = Base::trace(values, raw);
     trace.setFunction(record);
 
-    raw = (char*) (record + 1);
+    return function_(record->template value<A1, 1>(),
-    A1 a1 = expressionA1_->traceExecution(values, getTrace<A1, 1>(record), raw);
+        record->template value<A2,2>(), record->template jacobian<A1, 1>(),
-    raw = raw + expressionA1_->traceSize();
+        record->template jacobian<A2, 2>());
-    A2 a2 = expressionA2_->traceExecution(values, getTrace<A2, 2>(record), raw);
-
-    return function_(a1, a2, jacobian<A1, 1>(record), jacobian<A2, 2>(record));
   }
-
 };
 
 //-----------------------------------------------------------------------------
 /// Ternary Expression
 
 template<class T, class A1, class A2, class A3>
-class TernaryExpression: public ExpressionNode<T> {
+class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type {
 
 public:
 
-  typedef Eigen::Matrix<double, T::dimension, A1::dimension> JacobianTA1;
-  typedef Eigen::Matrix<double, T::dimension, A2::dimension> JacobianTA2;
-  typedef Eigen::Matrix<double, T::dimension, A3::dimension> JacobianTA3;
   typedef boost::function<
-      T(const A1&, const A2&, const A3&, boost::optional<JacobianTA1&>,
+      T(const A1&, const A2&, const A3&, typename Optional<T, A1>::type,
-          boost::optional<JacobianTA2&>, boost::optional<JacobianTA3&>)> Function;
+          typename Optional<T, A2>::type, typename Optional<T, A3>::type)> Function;
+  typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
+  typedef typename Base::Record Record;
 
 private:
 
   Function function_;
-  boost::shared_ptr<ExpressionNode<A1> > expressionA1_;
-  boost::shared_ptr<ExpressionNode<A2> > expressionA2_;
-  boost::shared_ptr<ExpressionNode<A3> > expressionA3_;
 
   /// Constructor with a ternary function f, and three input arguments
-  TernaryExpression(
+  TernaryExpression(Function f, const Expression<A1>& e1,
-      Function f, //
+      const Expression<A2>& e2, const Expression<A3>& e3) :
-      const Expression<A1>& e1, const Expression<A2>& e2,
+      function_(f) {
-      const Expression<A3>& e3) :
+    this->template reset<A1, 1>(e1.root());
-      function_(f), expressionA1_(e1.root()), expressionA2_(e2.root()), expressionA3_(
+    this->template reset<A2, 2>(e2.root());
-          e3.root()) {
+    this->template reset<A3, 3>(e3.root());
+    ExpressionNode<T>::traceSize_ = //
+        sizeof(Record) + e1.traceSize() + e2.traceSize() + e3.traceSize();
   }
 
   friend class Expression<T> ;
 
 public:
 
-  /// Return keys that play in this expression
-  virtual std::set<Key> keys() const {
-    std::set<Key> keys1 = expressionA1_->keys();
-    std::set<Key> keys2 = expressionA2_->keys();
-    std::set<Key> keys3 = expressionA3_->keys();
-    keys2.insert(keys3.begin(), keys3.end());
-    keys1.insert(keys2.begin(), keys2.end());
-    return keys1;
-  }
-
   /// Return value
   virtual T value(const Values& values) const {
     using boost::none;
-    return function_(this->expressionA1_->value(values),
+    return function_(this->template expression<A1, 1>()->value(values),
-        this->expressionA2_->value(values), this->expressionA3_->value(values),
+    this->template expression<A2, 2>()->value(values),
-        none, none, none);
+    this->template expression<A3, 3>()->value(values),
-  }
+    none, none, none);
-
-  /// Return value and derivatives
-  virtual Augmented<T> forward(const Values& values) const {
-    using boost::none;
-    Augmented<A1> a1 = this->expressionA1_->forward(values);
-    Augmented<A2> a2 = this->expressionA2_->forward(values);
-    Augmented<A3> a3 = this->expressionA3_->forward(values);
-    JacobianTA1 dTdA1;
-    JacobianTA2 dTdA2;
-    JacobianTA3 dTdA3;
-    T t = function_(a1.value(), a2.value(), a3.value(),
-        a1.constant() ? none : boost::optional<JacobianTA1&>(dTdA1),
-        a2.constant() ? none : boost::optional<JacobianTA2&>(dTdA2),
-        a3.constant() ? none : boost::optional<JacobianTA3&>(dTdA3));
-    return Augmented<T>(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians(), dTdA3,
-        a3.jacobians());
-  }
-
-  /// CallRecord structure for reverse AD
-  typedef boost::mpl::vector<Numbered<A1, 1>, Numbered<A2, 2>, Numbered<A3, 3> > Arguments;
-  typedef typename GenerateRecord<T, Arguments>::type Record;
-
-  // Return size needed for memory buffer in traceExecution
-  virtual size_t traceSize() const {
-    return sizeof(Record) + expressionA1_->traceSize()
-        + expressionA2_->traceSize() + expressionA2_->traceSize();
   }
 
   /// Construct an execution trace for reverse AD
   virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
       char* raw) const {
-    Record* record = new (raw) Record();
+
+    Record* record = Base::trace(values, raw);
     trace.setFunction(record);
 
-    raw = (char*) (record + 1);
+    return function_(
-    A1 a1 = expressionA1_->traceExecution(values, getTrace<A1, 1>(record), raw);
+        record->template value<A1, 1>(), record->template value<A2, 2>(),
-    raw = raw + expressionA1_->traceSize();
+        record->template value<A3, 3>(), record->template jacobian<A1, 1>(),
-    A2 a2 = expressionA2_->traceExecution(values, getTrace<A2, 2>(record), raw);
+        record->template jacobian<A2, 2>(), record->template jacobian<A3, 3>());
-    raw = raw + expressionA2_->traceSize();
-    A3 a3 = expressionA3_->traceExecution(values, getTrace<A3, 3>(record), raw);
-
-    return function_(a1, a2, a3, jacobian<A1, 1>(record),
-        jacobian<A2, 2>(record), jacobian<A3, 3>(record));
   }
 
 };

gtsam_unstable/nonlinear/Expression.h

@@ -34,73 +34,83 @@ class Expression {
 private:
 
   // Paul's trick shared pointer, polymorphic root of entire expression tree
-  boost::shared_ptr<ExpressionNode<T> > root_;
+  const boost::shared_ptr<ExpressionNode<T> > root_;
+
+  // Fixed dimensions: an Expression is assumed unmutable
+  const std::vector<size_t> dimensions_;
 
 public:
 
   // Construct a constant expression
   Expression(const T& value) :
-      root_(new ConstantExpression<T>(value)) {
+      root_(new ConstantExpression<T>(value)), //
+      dimensions_(root_->dimensions()) {
   }
 
   // Construct a leaf expression, with Key
   Expression(const Key& key) :
-      root_(new LeafExpression<T>(key)) {
+      root_(new LeafExpression<T>(key)), //
+      dimensions_(root_->dimensions()) {
   }
 
   // Construct a leaf expression, with Symbol
   Expression(const Symbol& symbol) :
-      root_(new LeafExpression<T>(symbol)) {
+      root_(new LeafExpression<T>(symbol)), //
+      dimensions_(root_->dimensions()) {
   }
 
   // Construct a leaf expression, creating Symbol
   Expression(unsigned char c, size_t j) :
-      root_(new LeafExpression<T>(Symbol(c, j))) {
+      root_(new LeafExpression<T>(Symbol(c, j))), //
+      dimensions_(root_->dimensions()) {
   }
 
   /// Construct a nullary method expression
   template<typename A>
   Expression(const Expression<A>& expression,
-      T (A::*method)(boost::optional<Matrix&>) const) {
+      T (A::*method)(typename Optional<T, A>::type) const) :
-    root_.reset(
+      root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)), //
-        new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression));
+      dimensions_(root_->dimensions()) {
   }
 
   /// Construct a unary function expression
   template<typename A>
   Expression(typename UnaryExpression<T, A>::Function function,
-      const Expression<A>& expression) {
+      const Expression<A>& expression) :
-    root_.reset(new UnaryExpression<T, A>(function, expression));
+      root_(new UnaryExpression<T, A>(function, expression)), //
+      dimensions_(root_->dimensions()) {
   }
 
   /// Construct a unary method expression
   template<typename A1, typename A2>
   Expression(const Expression<A1>& expression1,
-      T (A1::*method)(const A2&,
+      T (A1::*method)(const A2&, typename Optional<T, A1>::type,
-          boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA1&>,
+          typename Optional<T, A2>::type) const,
-          boost::optional<typename BinaryExpression<T, A1, A2>::JacobianTA2&>) const,
+      const Expression<A2>& expression2) :
-      const Expression<A2>& expression2) {
+      root_(
-    root_.reset(
+          new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
-        new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
+              expression1, expression2)), //
-            expression1, expression2));
+      dimensions_(root_->dimensions()) {
   }
 
   /// Construct a binary function expression
   template<typename A1, typename A2>
   Expression(typename BinaryExpression<T, A1, A2>::Function function,
-      const Expression<A1>& expression1, const Expression<A2>& expression2) {
+      const Expression<A1>& expression1, const Expression<A2>& expression2) :
-    root_.reset(
+      root_(
-        new BinaryExpression<T, A1, A2>(function, expression1, expression2));
+          new BinaryExpression<T, A1, A2>(function, expression1, expression2)), //
+      dimensions_(root_->dimensions()) {
   }
 
   /// Construct a ternary function expression
   template<typename A1, typename A2, typename A3>
   Expression(typename TernaryExpression<T, A1, A2, A3>::Function function,
       const Expression<A1>& expression1, const Expression<A2>& expression2,
-      const Expression<A3>& expression3) {
+      const Expression<A3>& expression3) :
-    root_.reset(
+      root_(
-        new TernaryExpression<T, A1, A2, A3>(function, expression1, expression2,
+          new TernaryExpression<T, A1, A2, A3>(function, expression1,
-            expression3));
+              expression2, expression3)), //
+      dimensions_(root_->dimensions()) {
   }
 
   /// Return keys that play in this expression
@@ -108,14 +118,9 @@ public:
     return root_->keys();
   }
 
-  /// Return value and optional derivatives
+  /// Return dimensions for each argument, must be same order as keys
-  T value(const Values& values) const {
+  const std::vector<size_t>& dimensions() const {
-    return root_->value(values);
+    return dimensions_;
-  }
-
-  /// Return value and derivatives, forward AD version
-  Augmented<T> forward(const Values& values) const {
-    return root_->forward(values);
   }
 
   // Return size needed for memory buffer in traceExecution
@@ -130,29 +135,31 @@ public:
   }
 
   /// Return value and derivatives, reverse AD version
-  Augmented<T> reverse(const Values& values) const {
+  T reverse(const Values& values, JacobianMap& jacobians) const {
     size_t size = traceSize();
     char raw[size];
     ExecutionTrace<T> trace;
     T value(traceExecution(values, trace, raw));
-    Augmented<T> augmented(value);
+    trace.startReverseAD(jacobians);
-    trace.startReverseAD(augmented.jacobians());
+    return value;
-    return augmented;
+  }
+
+  /// Return value
+  T value(const Values& values) const {
+    return root_->value(values);
   }
 
   /// Return value and derivatives
-  Augmented<T> augmented(const Values& values) const {
+  T value(const Values& values, JacobianMap& jacobians) const {
-#ifdef EXPRESSION_FORWARD_AD
+    return reverse(values, jacobians);
-    return forward(values);
-#else
-    return reverse(values);
-#endif
   }
 
   const boost::shared_ptr<ExpressionNode<T> >& root() const {
     return root_;
   }
 
+  /// Define type so we can apply it as a meta-function
+  typedef Expression<T> type;
 };
 
 // http://stackoverflow.com/questions/16260445/boost-bind-to-operator

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -39,6 +39,11 @@ public:
       const T& measurement, const Expression<T>& expression) :
       NoiseModelFactor(noiseModel, expression.keys()), //
       measurement_(measurement), expression_(expression) {
+    if (!noiseModel)
+      throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
+    if (noiseModel->dim() != T::dimension)
+      throw std::invalid_argument(
+          "ExpressionFactor was created with a NoiseModel of incorrect dimension.");
   }
 
   /**
@@ -49,17 +54,71 @@ public:
   virtual Vector unwhitenedError(const Values& x,
       boost::optional<std::vector<Matrix>&> H = boost::none) const {
     if (H) {
+      // H should be pre-allocated
       assert(H->size()==size());
-      Augmented<T> augmented = expression_.augmented(x);
+
-      // move terms to H, which is pre-allocated to correct size
+      // Get dimensions of Jacobian matrices
-      augmented.move(*H);
+      std::vector<size_t> dims = expression_.dimensions();
-      return measurement_.localCoordinates(augmented.value());
+
+      // Create and zero out blocks to be passed to expression_
+      JacobianMap blocks;
+      for(DenseIndex i=0;i<size();i++) {
+        Matrix& Hi = H->at(i);
+        Hi.resize(T::dimension, dims[i]);
+        Hi.setZero(); // zero out
+        Eigen::Block<Matrix> block = Hi.block(0,0,T::dimension, dims[i]);
+        blocks.insert(std::make_pair(keys_[i], block));
+      }
+
+      T value = expression_.value(x, blocks);
+      return measurement_.localCoordinates(value);
     } else {
       const T& value = expression_.value(x);
       return measurement_.localCoordinates(value);
     }
   }
 
+  // Internal function to allocate a VerticalBlockMatrix and
+  // create Eigen::Block<Matrix> views into it
+  VerticalBlockMatrix prepareBlocks(JacobianMap& blocks) const {
+
+    // Get dimensions of Jacobian matrices
+    std::vector<size_t> dims = expression_.dimensions();
+
+    // Construct block matrix, is of right size but un-initialized
+    VerticalBlockMatrix Ab(dims, T::dimension, true);
+    Ab.matrix().setZero(); // zero out
+
+    // Create blocks to be passed to expression_
+    for(DenseIndex i=0;i<size();i++)
+      blocks.insert(std::make_pair(keys_[i], Ab(i)));
+
+    return Ab;
+  }
+
+  virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
+
+    // Construct VerticalBlockMatrix and views into it
+    JacobianMap blocks;
+    VerticalBlockMatrix Ab = prepareBlocks(blocks);
+
+    // Evaluate error to get Jacobians and RHS vector b
+    T value = expression_.value(x, blocks);
+    Ab(size()).col(0) = -measurement_.localCoordinates(value);
+
+    // Whiten the corresponding system now
+    // TODO ! this->noiseModel_->WhitenSystem(Ab);
+
+    // TODO pass unwhitened + noise model to Gaussian factor
+    // For now, only linearized constrained factors have noise model at linear level!!!
+    noiseModel::Constrained::shared_ptr constrained = //
+        boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
+    if (constrained) {
+      return boost::make_shared<JacobianFactor>(this->keys(), Ab,
+          constrained->unit());
+    } else
+      return boost::make_shared<JacobianFactor>(this->keys(), Ab);
+  }
 };
 // ExpressionFactor
 

gtsam_unstable/nonlinear/tests/testExpression.cpp

@@ -26,6 +26,10 @@
 
 #include <CppUnitLite/TestHarness.h>
 
+#include <boost/assign/list_of.hpp>
+using boost::assign::list_of;
+using boost::assign::map_list_of;
+
 using namespace std;
 using namespace gtsam;
 
@@ -44,10 +48,11 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3);
 TEST(Expression, constant) {
   Expression<Rot3> R(someR);
   Values values;
-  Augmented<Rot3> a = R.augmented(values);
+  JacobianMap actualMap;
-  EXPECT(assert_equal(someR, a.value()));
+  Rot3 actual = R.value(values, actualMap);
+  EXPECT(assert_equal(someR, actual));
   JacobianMap expected;
-  EXPECT(a.jacobians() == expected);
+  EXPECT(actualMap == expected);
 }
 
 /* ************************************************************************* */
@@ -56,11 +61,16 @@ TEST(Expression, leaf) {
   Expression<Rot3> R(100);
   Values values;
   values.insert(100, someR);
-  Augmented<Rot3> a = R.augmented(values);
+
-  EXPECT(assert_equal(someR, a.value()));
   JacobianMap expected;
-  expected[100] = eye(3);
+  Matrix H = eye(3);
-  EXPECT(a.jacobians() == expected);
+  expected.insert(make_pair(100, H.block(0, 0, 3, 3)));
+
+  JacobianMap actualMap2;
+  actualMap2.insert(make_pair(100, H.block(0, 0, 3, 3)));
+  Rot3 actual2 = R.reverse(values, actualMap2);
+  EXPECT(assert_equal(someR, actual2));
+  EXPECT(actualMap2 == expected);
 }
 
 /* ************************************************************************* */
@@ -86,13 +96,16 @@ Expression<Point3> p_cam(x, &Pose3::transform_to, p);
 }
 /* ************************************************************************* */
 // keys
-TEST(Expression, keys_binary) {
+TEST(Expression, BinaryKeys) {
-
+  set<Key> expected = list_of(1)(2);
-  // Check keys
+  EXPECT(expected == binary::p_cam.keys());
-  set<Key> expectedKeys;
+}
-  expectedKeys.insert(1);
+/* ************************************************************************* */
-  expectedKeys.insert(2);
+// dimensions
-  EXPECT(expectedKeys == binary::p_cam.keys());
+TEST(Expression, BinaryDimensions) {
+  vector<size_t> expected = list_of(6)(3), //
+  actual = binary::p_cam.dimensions();
+  EXPECT(expected==actual);
 }
 /* ************************************************************************* */
 // Binary(Leaf,Unary(Binary(Leaf,Leaf)))
@@ -107,24 +120,16 @@ Expression<Point2> uv_hat(uncalibrate<Cal3_S2>, K, projection);
 }
 /* ************************************************************************* */
 // keys
-TEST(Expression, keys_tree) {
+TEST(Expression, TreeKeys) {
-
+  set<Key> expected = list_of(1)(2)(3);
-  // Check keys
+  EXPECT(expected == tree::uv_hat.keys());
-  set<Key> expectedKeys;
-  expectedKeys.insert(1);
-  expectedKeys.insert(2);
-  expectedKeys.insert(3);
-  EXPECT(expectedKeys == tree::uv_hat.keys());
 }
 /* ************************************************************************* */
-// keys
+// dimensions
-TEST(Expression, block_tree) {
+TEST(Expression, TreeDimensions) {
-//  // Check VerticalBlockMatrix
+  vector<size_t> expected = list_of(6)(3)(5), //
-//  size_t dimensions[3] = { 6, 3, 5 };
+  actual = tree::uv_hat.dimensions();
-//  Matrix matrix(2, 14);
+  EXPECT(expected==actual);
-//  VerticalBlockMatrix expected(dimensions, matrix), actual =
-//      tree::uv_hat.verticalBlockMatrix();
-//  EXPECT( assert_equal(expected, *jf, 1e-9));
 }
 /* ************************************************************************* */
 
@@ -135,10 +140,8 @@ TEST(Expression, compose1) {
   Expression<Rot3> R3 = R1 * R2;
 
   // Check keys
-  set<Key> expectedKeys;
+  set<Key> expected = list_of(1)(2);
-  expectedKeys.insert(1);
+  EXPECT(expected == R3.keys());
-  expectedKeys.insert(2);
-  EXPECT(expectedKeys == R3.keys());
 }
 
 /* ************************************************************************* */
@@ -150,9 +153,8 @@ TEST(Expression, compose2) {
   Expression<Rot3> R3 = R1 * R2;
 
   // Check keys
-  set<Key> expectedKeys;
+  set<Key> expected = list_of(1);
-  expectedKeys.insert(1);
+  EXPECT(expected == R3.keys());
-  EXPECT(expectedKeys == R3.keys());
 }
 
 /* ************************************************************************* */
@@ -164,9 +166,8 @@ TEST(Expression, compose3) {
   Expression<Rot3> R3 = R1 * R2;
 
   // Check keys
-  set<Key> expectedKeys;
+  set<Key> expected = list_of(3);
-  expectedKeys.insert(3);
+  EXPECT(expected == R3.keys());
-  EXPECT(expectedKeys == R3.keys());
 }
 
 /* ************************************************************************* */
@@ -191,11 +192,8 @@ TEST(Expression, ternary) {
   Expression<Rot3> ABC(composeThree, A, B, C);
 
   // Check keys
-  set<Key> expectedKeys;
+  set<Key> expected = list_of(1)(2)(3);
-  expectedKeys.insert(1);
+  EXPECT(expected == ABC.keys());
-  expectedKeys.insert(2);
-  expectedKeys.insert(3);
-  EXPECT(expectedKeys == ABC.keys());
 }
 
 /* ************************************************************************* */

gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp

@@ -21,6 +21,7 @@
 #include <gtsam_unstable/nonlinear/ExpressionFactor.h>
 #include <gtsam/slam/GeneralSFMFactor.h>
 #include <gtsam/slam/ProjectionFactor.h>
+#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/geometry/Cal3_S2.h>
 #include <gtsam/base/Testable.h>
@@ -33,85 +34,73 @@ using namespace gtsam;
 Point2 measured(-17, 30);
 SharedNoiseModel model = noiseModel::Unit::Create(2);
 
+namespace leaf {
+// Create some values
+struct MyValues: public Values {
+  MyValues() {
+    insert(2, Point2(3, 5));
+  }
+} values;
+
+// Create leaf
+Point2_ p(2);
+}
+
 /* ************************************************************************* */
 // Leaf
-TEST(ExpressionFactor, leaf) {
+TEST(ExpressionFactor, Leaf) {
+  using namespace leaf;
 
-  // Create some values
+  // Create old-style factor to create expected value and derivatives
-  Values values;
+  PriorFactor<Point2> old(2, Point2(0, 0), model);
-  values.insert(2, Point2(3, 5));
-
-  JacobianFactor expected( //
-      2, (Matrix(2, 2) << 1, 0, 0, 1), //
-      (Vector(2) << -3, -5));
-
-  // Create leaf
-  Point2_ p(2);
 
   // Concise version
   ExpressionFactor<Point2> f(model, Point2(0, 0), p);
+  EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f.dim());
-  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+  boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
-  boost::shared_ptr<JacobianFactor> jf = //
+  EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
-      boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf, 1e-9));
 }
 
-/* ************************************************************************* */
+///* ************************************************************************* */
-// non-zero noise model
+//// non-zero noise model
-TEST(ExpressionFactor, model) {
+//TEST(ExpressionFactor, Model) {
-
+//  using namespace leaf;
-  SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
+//
-
+//  SharedNoiseModel model = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.01));
-  // Create some values
+//
-  Values values;
+//  // Create old-style factor to create expected value and derivatives
-  values.insert(2, Point2(3, 5));
+//  PriorFactor<Point2> old(2, Point2(0, 0), model);
-
+//
-  JacobianFactor expected( //
+//  // Concise version
-      2, (Matrix(2, 2) << 10, 0, 0, 100), //
+//  ExpressionFactor<Point2> f(model, Point2(0, 0), p);
-      (Vector(2) << -30, -500));
+//  EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
-
+//  EXPECT_LONGS_EQUAL(2, f.dim());
-  // Create leaf
+//  boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
-  Point2_ p(2);
+//  EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
-
+//}
-  // Concise version
+//
-  ExpressionFactor<Point2> f(model, Point2(0, 0), p);
+///* ************************************************************************* */
-  EXPECT_LONGS_EQUAL(2, f.dim());
+//// Constrained noise model
-  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
+//TEST(ExpressionFactor, Constrained) {
-  boost::shared_ptr<JacobianFactor> jf = //
+//  using namespace leaf;
-      boost::dynamic_pointer_cast<JacobianFactor>(gf);
+//
-  EXPECT( assert_equal(expected, *jf, 1e-9));
+//  SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2, 0));
-}
+//
-
+//  // Create old-style factor to create expected value and derivatives
-/* ************************************************************************* */
+//  PriorFactor<Point2> old(2, Point2(0, 0), model);
-// Constrained noise model
+//
-TEST(ExpressionFactor, constrained) {
+//  // Concise version
-
+//  ExpressionFactor<Point2> f(model, Point2(0, 0), p);
-  SharedDiagonal model = noiseModel::Constrained::MixedSigmas(Vector2(0.2,0));
+//  EXPECT_DOUBLES_EQUAL(old.error(values), f.error(values), 1e-9);
-
+//  EXPECT_LONGS_EQUAL(2, f.dim());
-  // Create some values
+//  boost::shared_ptr<GaussianFactor> gf2 = f.linearize(values);
-  Values values;
+//  EXPECT( assert_equal(*old.linearize(values), *gf2, 1e-9));
-  values.insert(2, Point2(3, 5));
+//}
-
-  vector<pair<Key, Matrix> > terms;
-  terms.push_back(make_pair(2, (Matrix(2, 2) << 1, 0, 0, 1)));
-  JacobianFactor expected(terms, (Vector(2) << -3, -5), model);
-
-  // Create leaf
-  Point2_ p(2);
-
-  // Concise version
-  ExpressionFactor<Point2> f(model, Point2(0, 0), p);
-  EXPECT_LONGS_EQUAL(2, f.dim());
-  boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
-  boost::shared_ptr<JacobianFactor> jf = //
-      boost::dynamic_pointer_cast<JacobianFactor>(gf);
-  EXPECT( assert_equal(expected, *jf, 1e-9));
-}
 
 /* ************************************************************************* */
 // Unary(Leaf))
-TEST(ExpressionFactor, unary) {
+TEST(ExpressionFactor, Unary) {
 
   // Create some values
   Values values;
@@ -132,25 +121,21 @@ TEST(ExpressionFactor, unary) {
       boost::dynamic_pointer_cast<JacobianFactor>(gf);
   EXPECT( assert_equal(expected, *jf, 1e-9));
 }
+
 /* ************************************************************************* */
-struct TestBinaryExpression {
+static Point2 myUncal(const Cal3_S2& K, const Point2& p,
-  static Point2 myUncal(const Cal3_S2& K, const Point2& p,
+    boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) {
-      boost::optional<Matrix25&> Dcal, boost::optional<Matrix2&> Dp) {
+  return K.uncalibrate(p, Dcal, Dp);
-    return K.uncalibrate(p, Dcal, Dp);
+}
-  }
+
-  Cal3_S2_ K_;
-  Point2_ p_;
-  BinaryExpression<Point2, Cal3_S2, Point2> binary_;
-  TestBinaryExpression() :
-      K_(1), p_(2), binary_(myUncal, K_, p_) {
-  }
-};
-/* ************************************************************************* */
 // Binary(Leaf,Leaf)
-TEST(ExpressionFactor, binary) {
+TEST(ExpressionFactor, Binary) {
 
   typedef BinaryExpression<Point2, Cal3_S2, Point2> Binary;
-  TestBinaryExpression tester;
+
+  Cal3_S2_ K_(1);
+  Point2_ p_(2);
+  Binary binary(myUncal, K_, p_);
 
   // Create some values
   Values values;
@@ -159,22 +144,24 @@ TEST(ExpressionFactor, binary) {
 
   // traceRaw will fill raw with [Trace<Point2> | Binary::Record]
   EXPECT_LONGS_EQUAL(8, sizeof(double));
+  EXPECT_LONGS_EQUAL(24, sizeof(Point2));
+  EXPECT_LONGS_EQUAL(48, sizeof(Cal3_S2));
   EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace<Point2>));
   EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace<Cal3_S2>));
-  EXPECT_LONGS_EQUAL(2*5*8, sizeof(Binary::JacobianTA1));
+  EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian<Point2,Cal3_S2>::type));
-  EXPECT_LONGS_EQUAL(2*2*8, sizeof(Binary::JacobianTA2));
+  EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian<Point2,Point2>::type));
-  size_t expectedRecordSize = 16 + 2 * 16 + 80 + 32;
+  size_t expectedRecordSize = 24 + 24 + 48 + 2 * 16 + 80 + 32;
   EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record));
 
   // Check size
-  size_t size = tester.binary_.traceSize();
+  size_t size = binary.traceSize();
   CHECK(size);
   EXPECT_LONGS_EQUAL(expectedRecordSize, size);
   // Use Variable Length Array, allocated on stack by gcc
   // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla
   char raw[size];
   ExecutionTrace<Point2> trace;
-  Point2 value = tester.binary_.traceExecution(values, trace, raw);
+  Point2 value = binary.traceExecution(values, trace, raw);
   // trace.print();
 
   // Expected Jacobians
@@ -187,13 +174,12 @@ TEST(ExpressionFactor, binary) {
   boost::optional<Binary::Record*> r = trace.record<Binary::Record>();
   CHECK(r);
   EXPECT(
-      assert_equal(expected25, (Matrix) static_cast<Argument<Point2, Cal3_S2, 1>*> (*r)->dTdA, 1e-9));
+      assert_equal(expected25, (Matrix) (*r)-> jacobian<Cal3_S2, 1>(), 1e-9));
-  EXPECT(
+  EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian<Point2, 2>(), 1e-9));
-      assert_equal(expected22, (Matrix) static_cast<Argument<Point2, Point2, 2>*> (*r)->dTdA, 1e-9));
 }
 /* ************************************************************************* */
 // Unary(Binary(Leaf,Leaf))
-TEST(ExpressionFactor, shallow) {
+TEST(ExpressionFactor, Shallow) {
 
   // Create some values
   Values values;
@@ -216,10 +202,10 @@ TEST(ExpressionFactor, shallow) {
   // traceExecution of shallow tree
   typedef UnaryExpression<Point2, Point3> Unary;
   typedef BinaryExpression<Point3, Pose3, Point3> Binary;
-  EXPECT_LONGS_EQUAL(80, sizeof(Unary::Record));
+  EXPECT_LONGS_EQUAL(112, sizeof(Unary::Record));
-  EXPECT_LONGS_EQUAL(272, sizeof(Binary::Record));
+  EXPECT_LONGS_EQUAL(432, sizeof(Binary::Record));
   size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record);
-  LONGS_EQUAL(352, expectedTraceSize);
+  LONGS_EQUAL(112+432, expectedTraceSize);
   size_t size = expression.traceSize();
   CHECK(size);
   EXPECT_LONGS_EQUAL(expectedTraceSize, size);
@@ -235,8 +221,7 @@ TEST(ExpressionFactor, shallow) {
   // Check matrices
   boost::optional<Unary::Record*> r = trace.record<Unary::Record>();
   CHECK(r);
-  EXPECT(
+  EXPECT(assert_equal(expected23, (Matrix)(*r)->jacobian<Point3, 1>(), 1e-9));
-      assert_equal(expected23, (Matrix)static_cast<Argument<Point2, Point3, 1>*>(*r)->dTdA, 1e-9));
 
   // Linearization
   ExpressionFactor<Point2> f2(model, measured, expression);
@@ -298,7 +283,7 @@ TEST(ExpressionFactor, tree) {
 
 /* ************************************************************************* */
 
-TEST(ExpressionFactor, compose1) {
+TEST(ExpressionFactor, Compose1) {
 
   // Create expression
   Rot3_ R1(1), R2(2);
@@ -430,37 +415,6 @@ TEST(ExpressionFactor, composeTernary) {
   EXPECT( assert_equal(expected, *jf,1e-9));
 }
 
-/* ************************************************************************* */
-
-namespace mpl = boost::mpl;
-
-#include <boost/mpl/assert.hpp>
-template<class T> struct Incomplete;
-
-typedef mpl::vector<Numbered<Pose3, 1>, Numbered<Point3, 2>,
-    Numbered<Cal3_S2, 3> > MyTypes;
-typedef GenerateRecord<Point2, MyTypes>::type Generated;
-//Incomplete<Generated> incomplete;
-//BOOST_MPL_ASSERT((boost::is_same< Matrix25, Generated::JacobianTA >));
-BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Jacobian2T >));
-
-Generated generated;
-
-typedef mpl::vector1<Point3> OneType;
-typedef mpl::pop_front<OneType>::type Empty;
-typedef mpl::pop_front<Empty>::type Bad;
-//typedef ProtoTrace<OneType> UnaryTrace;
-//BOOST_MPL_ASSERT((boost::is_same< UnaryTrace::A, Point3 >));
-
-#include <boost/static_assert.hpp>
-#include <boost/mpl/plus.hpp>
-#include <boost/mpl/int.hpp>
-//#include <boost/mpl/print.hpp>
-
-typedef struct {
-} Expected0;
-BOOST_MPL_ASSERT((boost::is_same< Expected0, Expected0 >));
-
 /* ************************************************************************* */
 int main() {
   TestResult tr;

gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp

@@ -0,0 +1,158 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file testExpressionMeta.cpp
+ * @date October 14, 2014
+ * @author Frank Dellaert
+ * @brief Test meta-programming constructs for Expressions
+ */
+
+#include <gtsam_unstable/nonlinear/ExpressionFactor.h>
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Cal3_S2.h>
+#include <gtsam/base/Testable.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+
+/* ************************************************************************* */
+namespace mpl = boost::mpl;
+
+#include <boost/mpl/assert.hpp>
+#include <boost/mpl/equal.hpp>
+template<class T> struct Incomplete;
+
+// Check generation of FunctionalNode
+typedef mpl::vector<Pose3, Point3> MyTypes;
+typedef FunctionalNode<Point2, MyTypes>::type Generated;
+//Incomplete<Generated> incomplete;
+BOOST_MPL_ASSERT((boost::is_same< Matrix2, Generated::Record::Jacobian2T >));
+
+// Try generating vectors of ExecutionTrace
+typedef mpl::vector<ExecutionTrace<Pose3>, ExecutionTrace<Point3> > ExpectedTraces;
+
+typedef mpl::transform<MyTypes, ExecutionTrace<MPL::_1> >::type MyTraces;
+BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces >));
+
+template<class T>
+struct MakeTrace {
+  typedef ExecutionTrace<T> type;
+};
+typedef mpl::transform<MyTypes, MakeTrace<MPL::_1> >::type MyTraces1;
+BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces1 >));
+
+// Try generating vectors of Expression types
+typedef mpl::vector<Expression<Pose3>, Expression<Point3> > ExpectedExpressions;
+typedef mpl::transform<MyTypes, Expression<MPL::_1> >::type Expressions;
+BOOST_MPL_ASSERT((mpl::equal< ExpectedExpressions, Expressions >));
+
+// Try generating vectors of Jacobian types
+typedef mpl::vector<Matrix26, Matrix23> ExpectedJacobians;
+typedef mpl::transform<MyTypes, Jacobian<Point2, MPL::_1> >::type Jacobians;
+BOOST_MPL_ASSERT((mpl::equal< ExpectedJacobians, Jacobians >));
+
+// Try accessing a Jacobian
+typedef mpl::at_c<Jacobians, 1>::type Jacobian23; // base zero !
+BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>));
+
+/* ************************************************************************* */
+// Boost Fusion includes allow us to create/access values from MPL vectors
+#include <boost/fusion/include/mpl.hpp>
+#include <boost/fusion/include/at_c.hpp>
+
+// Create a value and access it
+TEST(ExpressionFactor, JacobiansValue) {
+  using boost::fusion::at_c;
+  Matrix23 expected;
+  Jacobians jacobians;
+
+  at_c<1>(jacobians) << 1, 2, 3, 4, 5, 6;
+
+  Matrix23 actual = at_c<1>(jacobians);
+  CHECK(actual.cols() == expected.cols());
+  CHECK(actual.rows() == expected.rows());
+}
+
+/* ************************************************************************* */
+// Test out polymorphic transform
+
+#include <boost/fusion/include/make_vector.hpp>
+#include <boost/fusion/include/transform.hpp>
+#include <boost/utility/result_of.hpp>
+
+struct triple {
+  template<class> struct result; // says we will provide result
+
+  template<class F>
+  struct result<F(int)> {
+      typedef double type; // result for int argument
+  };
+
+  template<class F>
+  struct result<F(const int&)> {
+      typedef double type; // result for int argument
+  };
+
+  template<class F>
+  struct result<F(const double &)> {
+      typedef double type; // result for double argument
+  };
+
+  template<class F>
+  struct result<F(double)> {
+      typedef double type; // result for double argument
+  };
+
+  // actual function
+  template<typename T>
+  typename result<triple(T)>::type operator()(const T& x) const {
+    return (double) x;
+  }
+};
+
+TEST(ExpressionFactor, Triple) {
+  typedef boost::fusion::vector<int, double> IntDouble;
+  IntDouble H = boost::fusion::make_vector(1, 2.0);
+
+  // Only works if I use Double2
+  typedef boost::fusion::result_of::transform<IntDouble, triple>::type Result;
+  typedef boost::fusion::vector<double, double> Double2;
+  Double2 D = boost::fusion::transform(H, triple());
+
+  using boost::fusion::at_c;
+  DOUBLES_EQUAL(1.0, at_c<0>(D), 1e-9);
+  DOUBLES_EQUAL(2.0, at_c<1>(D), 1e-9);
+}
+
+/* ************************************************************************* */
+#include <boost/fusion/include/invoke.hpp>
+#include <boost/functional/value_factory.hpp>
+
+// Test out polymorphic transform
+TEST(ExpressionFactor, Invoke) {
+  std::plus<int> add;
+  assert(invoke(add,boost::fusion::make_vector(1,1)) == 2);
+
+  // Creating a Pose3 (is there another way?)
+  boost::fusion::vector<Rot3, Point3> pair;
+  Pose3 pose = boost::fusion::invoke(boost::value_factory<Pose3>(), pair);
+}
+
+/* ************************************************************************* */
+int main() {
+  TestResult tr;
+  return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */
+