Merge branch 'feature/BAD_raw' into feature/BAD: the entire allocation tree is now allocated all at once, in Expression::reverse, on the stack. the traceExecution method then takes a raw pointer, and the "placement new" is used to grab that existing memory, avoiding all mallocs altogether. The only mallocs that still happen - that are responsible for almost all time within reverse - are the leaves inserting general matrices into a map to pass back to linearize.

gtsam_unstable/nonlinear/Expression-inl.h

@@ -21,9 +21,10 @@
 
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/base/Matrix.h>
+#include <gtsam/base/Testable.h>
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
-
+#include <new> // for placement new
 struct TestBinaryExpression;
 
 namespace gtsam {
@@ -43,10 +44,7 @@ typedef std::map<Key, Matrix> JacobianMap;
  */
 template<int COLS>
 struct CallRecord {
-
+  virtual void print(const std::string& indent) const = 0;
-  /// Make sure destructor is virtual
-  virtual ~CallRecord() {
-  }
   virtual void startReverseAD(JacobianMap& jacobians) const = 0;
   virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0;
   typedef Eigen::Matrix<double, 2, COLS> Jacobian2T;
@@ -76,11 +74,6 @@ public:
   ExecutionTrace() :
       type(Constant) {
   }
-  /// Destructor cleans up pointer if Function
-  ~ExecutionTrace() {
-    if (type == Function)
-      delete content.ptr;
-  }
   /// Change pointer to a Leaf Record
   void setLeaf(Key key) {
     type = Leaf;
@@ -91,6 +84,17 @@ public:
     type = Function;
     content.ptr = record;
   }
+  /// Print
+  void print(const std::string& indent = "") const {
+    if (type == Constant)
+      std::cout << indent << "Constant" << std::endl;
+    else if (type == Leaf)
+      std::cout << indent << "Leaf, key = " << content.key << std::endl;
+    else if (type == Function) {
+      std::cout << indent << "Function" << std::endl;
+      content.ptr->print(indent + "  ");
+    }
+  }
   /// Return record pointer, quite unsafe, used only for testing
   template<class Record>
   boost::optional<Record*> record() {
@@ -158,14 +162,6 @@ struct Select<2, A> {
   }
 };
 
-//template <class Derived>
-//struct TypedTrace {
-//  virtual void startReverseAD(JacobianMap& jacobians) const = 0;
-//  virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0;
-////  template<class JacobianFT>
-////  void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const {
-//};
-
 //-----------------------------------------------------------------------------
 /**
  * Value and Jacobians
@@ -261,7 +257,7 @@ public:
   }
 
   /// debugging
-  void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) {
+  virtual void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) {
     BOOST_FOREACH(const Pair& term, jacobians_)
       std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows()
           << "x" << term.second.cols() << ") ";
@@ -291,6 +287,7 @@ template<class T>
 class ExpressionNode {
 
 protected:
+
   ExpressionNode() {
   }
 
@@ -309,9 +306,14 @@ public:
   /// 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,
+  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-      ExecutionTrace<T>& p) const = 0;
+      char* raw) const = 0;
 };
 
 //-----------------------------------------------------------------------------
@@ -348,7 +350,8 @@ public:
   }
 
   /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& p) const {
+  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+      char* raw) const {
     return constant_;
   }
 };
@@ -388,8 +391,9 @@ public:
   }
 
   /// Construct an execution trace for reverse AD
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& p) const {
+  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-    p.setLeaf(key_);
+      char* raw) const {
+    trace.setLeaf(key_);
     return values.at<T>(key_);
   }
 
@@ -443,7 +447,12 @@ public:
   struct Record: public CallRecord<T::dimension> {
     ExecutionTrace<A1> trace1;
     JacobianTA dTdA1;
-
+    /// print to std::cout
+    virtual void print(const std::string& indent) const {
+      static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
+      std::cout << dTdA1.format(matlab) << std::endl;
+      trace1.print(indent);
+    }
     /// Start the reverse AD process
     virtual void startReverseAD(JacobianMap& jacobians) const {
       Select<T::dimension, A1>::reverseAD(trace1, dTdA1, jacobians);
@@ -460,12 +469,19 @@ public:
     }
   };
 
+  // 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>& p) const {
+  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-    Record* record = new Record();
+      char* raw) const {
-    p.setFunction(record);
+    Record* record = new (raw) Record();
-    A1 a = this->expressionA1_->traceExecution(values, record->trace1);
+    trace.setFunction(record);
-    return function_(a, record->dTdA1);
+    raw = (char*) (record + 1);
+    A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw);
+    return function_(a1, record->dTdA1);
   }
 };
 
@@ -534,7 +550,14 @@ public:
     ExecutionTrace<A2> trace2;
     JacobianTA1 dTdA1;
     JacobianTA2 dTdA2;
-
+    /// print to std::cout
+    virtual void print(const std::string& indent) const {
+      static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
+      std::cout << indent << dTdA1.format(matlab) << std::endl;
+      trace1.print(indent);
+      std::cout << indent << dTdA2.format(matlab) << std::endl;
+      trace2.print(indent);
+    }
     /// Start the reverse AD process
     virtual void startReverseAD(JacobianMap& jacobians) const {
       Select<T::dimension, A1>::reverseAD(trace1, dTdA1, jacobians);
@@ -554,12 +577,22 @@ public:
     }
   };
 
+  // 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
-  virtual T traceExecution(const Values& values, ExecutionTrace<T>& p) const {
+  /// The raw buffer is [Record | A1 raw | A2 raw]
-    Record* record = new Record();
+  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-    p.setFunction(record);
+      char* raw) const {
-    A1 a1 = this->expressionA1_->traceExecution(values, record->trace1);
+    Record* record = new (raw) Record();
-    A2 a2 = this->expressionA2_->traceExecution(values, record->trace2);
+    trace.setFunction(record);
+    raw = (char*) (record + 1);
+    A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw);
+    raw = raw + expressionA1_->traceSize();
+    A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw);
     return function_(a1, a2, record->dTdA1, record->dTdA2);
   }
 
@@ -643,7 +676,16 @@ public:
     JacobianTA1 dTdA1;
     JacobianTA2 dTdA2;
     JacobianTA3 dTdA3;
-
+    /// print to std::cout
+    virtual void print(const std::string& indent) const {
+      static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]");
+      std::cout << dTdA1.format(matlab) << std::endl;
+      trace1.print(indent);
+      std::cout << dTdA2.format(matlab) << std::endl;
+      trace2.print(indent);
+      std::cout << dTdA3.format(matlab) << std::endl;
+      trace3.print(indent);
+    }
     /// Start the reverse AD process
     virtual void startReverseAD(JacobianMap& jacobians) const {
       Select<T::dimension, A1>::reverseAD(trace1, dTdA1, jacobians);
@@ -666,13 +708,23 @@ public:
     }
   };
 
+  // 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>& p) const {
+  virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
-    Record* record = new Record();
+      char* raw) const {
-    p.setFunction(record);
+    Record* record = new (raw) Record();
-    A1 a1 = this->expressionA1_->traceExecution(values, record->trace1);
+    trace.setFunction(record);
-    A2 a2 = this->expressionA2_->traceExecution(values, record->trace2);
+    raw = (char*) (record + 1);
-    A3 a3 = this->expressionA3_->traceExecution(values, record->trace3);
+    A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw);
+    raw = raw + expressionA1_->traceSize();
+    A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw);
+    raw = raw + expressionA2_->traceSize();
+    A3 a3 = this->expressionA3_->traceExecution(values, record->trace3, raw);
     return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3);
   }
 

gtsam_unstable/nonlinear/Expression.h

@@ -113,17 +113,39 @@ public:
     return root_->value(values);
   }
 
-  /// Return value and derivatives
+  /// Return value and derivatives, forward AD version
-  Augmented<T> augmented(const Values& values) const {
+  Augmented<T> forward(const Values& values) const {
-#define REVERSE_AD
+    return root_->forward(values);
-#ifdef REVERSE_AD
+  }
+
+  // Return size needed for memory buffer in traceExecution
+  size_t traceSize() const {
+    return root_->traceSize();
+  }
+
+  /// trace execution, very unsafe, for testing purposes only
+  T traceExecution(const Values& values, ExecutionTrace<T>& trace,
+      char* raw) const {
+    return root_->traceExecution(values, trace, raw);
+  }
+
+  /// Return value and derivatives, reverse AD version
+  Augmented<T> reverse(const Values& values) const {
+    size_t size = traceSize();
+    char raw[size];
     ExecutionTrace<T> trace;
-    T value = root_->traceExecution(values, trace);
+    T value(traceExecution(values, trace, raw));
     Augmented<T> augmented(value);
     trace.startReverseAD(augmented.jacobians());
     return augmented;
+  }
+
+  /// Return value and derivatives
+  Augmented<T> augmented(const Values& values) const {
+#ifdef EXPRESSION_FORWARD_AD
+    return forward(values);
 #else
-    return root_->forward(values);
+    return reverse(values);
 #endif
   }
 

gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp

@@ -131,31 +131,37 @@ TEST(ExpressionFactor, binary) {
   values.insert(1, Cal3_S2());
   values.insert(2, Point2(0, 0));
 
+  // traceRaw will fill raw with [Trace<Point2> | Binary::Record]
+  EXPECT_LONGS_EQUAL(8, sizeof(double));
+  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*2*8, sizeof(Binary::JacobianTA2));
+  size_t expectedRecordSize = 16 + 2 * 16 + 80 + 32;
+  EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record));
+
+  // Check size
+  size_t size = tester.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);
+  // trace.print();
+
   // Expected Jacobians
   Matrix25 expected25;
   expected25 << 0, 0, 0, 1, 0, 0, 0, 0, 0, 1;
   Matrix2 expected22;
   expected22 << 1, 0, 0, 1;
 
-  // Do old trace
-  ExecutionTrace<Point2> trace;
-  tester.binary_.traceExecution(values, trace);
-
   // Check matrices
   boost::optional<Binary::Record*> p = trace.record<Binary::Record>();
   CHECK(p);
   EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9));
   EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9));
-
-//  // Check raw memory trace
-//  double raw[10];
-//  tester.binary_.traceRaw(values, 0);
-//
-//  // Check matrices
-//  boost::optional<Binary::Record*> p = trace.record<Binary::Record>();
-//  CHECK(p);
-//  EXPECT( assert_equal(expected25, (Matrix)(*p)->dTdA1, 1e-9));
-//  EXPECT( assert_equal(expected22, (Matrix)(*p)->dTdA2, 1e-9));
 }
 /* ************************************************************************* */
 // Unary(Binary(Leaf,Leaf))
@@ -173,11 +179,38 @@ TEST(ExpressionFactor, shallow) {
   GaussianFactor::shared_ptr expected = old.linearize(values);
 
   // Create leaves
-  Pose3_ x(1);
+  Pose3_ x_(1);
-  Point3_ p(2);
+  Point3_ p_(2);
 
-  // Concise version
+  // Construct expression, concise evrsion
-  ExpressionFactor<Point2> f2(model, measured, project(transform_to(x, p)));
+  Point2_ expression = project(transform_to(x_, p_));
+
+  // 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(272, sizeof(Binary::Record));
+  size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record);
+  LONGS_EQUAL(352, expectedTraceSize);
+  size_t size = expression.traceSize();
+  CHECK(size);
+  EXPECT_LONGS_EQUAL(expectedTraceSize, size);
+  char raw[size];
+  ExecutionTrace<Point2> trace;
+  Point2 value = expression.traceExecution(values, trace, raw);
+  // trace.print();
+
+  // Expected Jacobians
+  Matrix23 expected23;
+  expected23 << 1, 0, 0, 0, 1, 0;
+
+  // Check matrices
+  boost::optional<Unary::Record*> p = trace.record<Unary::Record>();
+  CHECK(p);
+  EXPECT( assert_equal(expected23, (Matrix)(*p)->dTdA1, 1e-9));
+
+  // Linearization
+  ExpressionFactor<Point2> f2(model, measured, expression);
   EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9);
   EXPECT_LONGS_EQUAL(2, f2.dim());
   boost::shared_ptr<GaussianFactor> gf2 = f2.linearize(values);