From 599e232d1de82cface68e864e2d804065b37248a Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sat, 11 Oct 2014 12:11:22 +0200
Subject: [PATCH] traceSize, two tests work

---
 gtsam_unstable/nonlinear/Expression-inl.h     | 25 ++++++++++++++++++-
 gtsam_unstable/nonlinear/Expression.h         | 10 ++++++--
 .../nonlinear/tests/testExpressionFactor.cpp  | 13 +++++++---
 3 files changed, 42 insertions(+), 6 deletions(-)

diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h
index ae4224182..1cbc11da5 100644
--- a/gtsam_unstable/nonlinear/Expression-inl.h
+++ b/gtsam_unstable/nonlinear/Expression-inl.h
@@ -257,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() << ") ";
@@ -287,6 +287,7 @@ template<class T>
 class ExpressionNode {
 
 protected:
+
   ExpressionNode() {
   }
 
@@ -305,6 +306,11 @@ 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, ExecutionTrace<T>& trace,
       void* raw) const = 0;
@@ -463,6 +469,11 @@ 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>& trace,
       void* raw) const {
@@ -566,6 +577,12 @@ 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
   /// The raw buffer is [Record | A1 raw | A2 raw]
   virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
@@ -689,6 +706,12 @@ 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>& trace,
       void* raw) const {
diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h
index ea2e6280d..3f31d0517 100644
--- a/gtsam_unstable/nonlinear/Expression.h
+++ b/gtsam_unstable/nonlinear/Expression.h
@@ -118,6 +118,11 @@ public:
     return root_->forward(values);
   }
 
+  // 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,
       void* raw) const {
@@ -126,9 +131,10 @@ public:
 
   /// Return value and derivatives, reverse AD version
   Augmented<T> reverse(const Values& values) const {
-    char raw[352];
+    size_t size = traceSize();
+    char raw[size];
     ExecutionTrace<T> trace;
-    T value(root_->traceExecution(values, trace, raw));
+    T value(traceExecution(values, trace, raw));
     Augmented<T> augmented(value);
     trace.startReverseAD(augmented.jacobians());
     return augmented;
diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
index f64f2a11a..b1ea646a8 100644
--- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
+++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
@@ -139,7 +139,11 @@ TEST(ExpressionFactor, binary) {
   EXPECT_LONGS_EQUAL(2*2*8, sizeof(Binary::JacobianTA2));
   size_t expectedRecordSize = 16 + 2 * 16 + 80 + 32;
   EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record));
-  size_t size = 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];
@@ -186,8 +190,11 @@ TEST(ExpressionFactor, shallow) {
   typedef BinaryExpression<Point3, Pose3, Point3> Binary;
   EXPECT_LONGS_EQUAL(80, sizeof(Unary::Record));
   EXPECT_LONGS_EQUAL(272, sizeof(Binary::Record));
-  size_t size = sizeof(Unary::Record) + sizeof(Binary::Record);
-  LONGS_EQUAL(352, size);
+  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);