diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 08de1ab5b..8e62ab708 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -24,7 +24,7 @@ #include #include #include - +#include // for placement new struct TestBinaryExpression; namespace gtsam { @@ -44,7 +44,7 @@ typedef std::map JacobianMap; */ template struct CallRecord { - virtual void print() const = 0; + virtual void print(const std::string& indent) const = 0; virtual void startReverseAD(JacobianMap& jacobians) const = 0; virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; typedef Eigen::Matrix Jacobian2T; @@ -70,7 +70,6 @@ class ExecutionTrace { CallRecord* ptr; } content; public: - T value; /// Pointer always starts out as a Constant ExecutionTrace() : type(Constant) { @@ -86,12 +85,15 @@ public: content.ptr = record; } /// Print - virtual void print() const { - GTSAM_PRINT(value); - if (type == Leaf) - std::cout << "Leaf, key = " << content.key << std::endl; - else if (type == Function) - content.ptr->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 @@ -304,7 +306,7 @@ public: virtual Augmented forward(const Values& values) const = 0; /// Construct an execution trace for reverse AD - virtual ExecutionTrace traceExecution(const Values& values, + virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const = 0; }; @@ -342,11 +344,9 @@ public: } /// Construct an execution trace for reverse AD - virtual ExecutionTrace traceExecution(const Values& values, + virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { - ExecutionTrace trace; - trace.value = constant_; - return trace; + return constant_; } }; @@ -385,12 +385,10 @@ public: } /// Construct an execution trace for reverse AD - virtual ExecutionTrace traceExecution(const Values& values, + virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { - ExecutionTrace trace; trace.setLeaf(key_); - trace.value = values.at(key_); - return trace; + return values.at(key_); } }; @@ -444,9 +442,10 @@ public: ExecutionTrace trace1; JacobianTA dTdA1; /// print to std::cout - virtual void print() const { - std::cout << dTdA1 << std::endl; - trace1.print(); + 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 { @@ -465,14 +464,13 @@ public: }; /// Construct an execution trace for reverse AD - virtual ExecutionTrace traceExecution(const Values& values, + virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { - ExecutionTrace trace; // Record* record = new Record(); // p.setFunction(record); // A1 a = this->expressionA1_->traceExecution(values, record->trace1); // return function_(a, record->dTdA1); - return trace; + return T(); } }; @@ -542,11 +540,12 @@ public: JacobianTA1 dTdA1; JacobianTA2 dTdA2; /// print to std::cout - virtual void print() const { - std::cout << dTdA1 << std::endl; - trace1.print(); - std::cout << dTdA2 << std::endl; - trace2.print(); + 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 { @@ -569,17 +568,13 @@ public: /// Construct an execution trace for reverse AD /// The raw buffer is [Record | A1 raw | A2 raw] - virtual ExecutionTrace traceExecution(const Values& values, + virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { - ExecutionTrace trace; - Record* record = static_cast(raw); + Record* record = new (raw) Record(); trace.setFunction(record); - record->trace1 = this->expressionA1_->traceExecution(values, raw); - record->trace2 = this->expressionA2_->traceExecution(values, raw); - trace.value = function_(record->trace1.value, record->trace2.value, - record->dTdA1, record->dTdA2); - trace.print(); - return trace; + A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw); + A2 a2 = this->expressionA2_->traceExecution(values, record->trace2, raw); + return function_(a1, a2, record->dTdA1, record->dTdA2); } }; @@ -663,13 +658,14 @@ public: JacobianTA2 dTdA2; JacobianTA3 dTdA3; /// print to std::cout - virtual void print() const { - std::cout << dTdA1 << std::endl; - trace1.print(); - std::cout << dTdA2 << std::endl; - trace2.print(); - std::cout << dTdA3 << std::endl; - trace3.print(); + 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 { @@ -694,16 +690,15 @@ public: }; /// Construct an execution trace for reverse AD - virtual ExecutionTrace traceExecution(const Values& values, + virtual T traceExecution(const Values& values, ExecutionTrace& trace, void* raw) const { - ExecutionTrace trace; // Record* record = new Record(); // p.setFunction(record); // A1 a1 = this->expressionA1_->traceExecution(values, record->trace1); // A2 a2 = this->expressionA2_->traceExecution(values, record->trace2); // A3 a3 = this->expressionA3_->traceExecution(values, record->trace3); // return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3); - return trace; + return T(); } }; diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index e784d1c2f..322692c27 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -118,8 +118,9 @@ public: #define REVERSE_AD #ifdef REVERSE_AD char raw[10]; - ExecutionTrace trace = root_->traceExecution(values, raw); - Augmented augmented(trace.value); + ExecutionTrace trace; + T value (root_->traceExecution(values, trace, raw)); + Augmented augmented(value); trace.startReverseAD(augmented.jacobians()); return augmented; #else diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 7a1afcf18..535bdba74 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -139,17 +139,19 @@ TEST(ExpressionFactor, binary) { // traceRaw will fill raw with [Trace | Binary::Record] EXPECT_LONGS_EQUAL(8, sizeof(double)); - EXPECT_LONGS_EQUAL(48, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(72, sizeof(ExecutionTrace)); + EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); + EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); EXPECT_LONGS_EQUAL(2*5*8, sizeof(Binary::JacobianTA1)); EXPECT_LONGS_EQUAL(2*2*8, sizeof(Binary::JacobianTA2)); - size_t expectedRecordSize = 8 + 48 + 72 + 80 + 32; // 240 + size_t expectedRecordSize = 16 + 2*16 + 80 + 32; EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record)); size_t size = sizeof(Binary::Record); // Use Variable Length Array, allocated on stack by gcc // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla char raw[size]; - ExecutionTrace trace = tester.binary_.traceExecution(values, raw); + ExecutionTrace trace; + Point2 value = tester.binary_.traceExecution(values, trace, raw); + trace.print(); // Check matrices // boost::optional p = trace.record();