diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 7e21db45e..29c6def8f 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -21,9 +21,10 @@ #include #include +#include #include #include - +#include // for placement new struct TestBinaryExpression; namespace gtsam { @@ -43,10 +44,7 @@ typedef std::map JacobianMap; */ template struct CallRecord { - - /// Make sure destructor is virtual - virtual ~CallRecord() { - } + 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; @@ -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 boost::optional record() { @@ -158,14 +162,6 @@ struct Select<2, A> { } }; -//template -//struct TypedTrace { -// virtual void startReverseAD(JacobianMap& jacobians) const = 0; -// virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; -//// template -//// 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 ExpressionNode { protected: + ExpressionNode() { } @@ -309,9 +306,14 @@ public: /// Return value and derivatives virtual Augmented 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& p) const = 0; + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const = 0; }; //----------------------------------------------------------------------------- @@ -348,7 +350,8 @@ public: } /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& p) const { + virtual T traceExecution(const Values& values, ExecutionTrace& 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& p) const { - p.setLeaf(key_); + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const { + trace.setLeaf(key_); return values.at(key_); } @@ -443,7 +447,12 @@ public: struct Record: public CallRecord { ExecutionTrace 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::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& p) const { - Record* record = new Record(); - p.setFunction(record); - A1 a = this->expressionA1_->traceExecution(values, record->trace1); - return function_(a, record->dTdA1); + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const { + Record* record = new (raw) Record(); + trace.setFunction(record); + raw = (char*) (record + 1); + A1 a1 = this->expressionA1_->traceExecution(values, record->trace1, raw); + return function_(a1, record->dTdA1); } }; @@ -534,7 +550,14 @@ public: ExecutionTrace 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::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& p) const { - Record* record = new Record(); - p.setFunction(record); - A1 a1 = this->expressionA1_->traceExecution(values, record->trace1); - A2 a2 = this->expressionA2_->traceExecution(values, record->trace2); + /// The raw buffer is [Record | A1 raw | A2 raw] + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const { + Record* record = new (raw) Record(); + 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::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& p) const { - 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); + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const { + Record* record = new (raw) Record(); + 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); + raw = raw + expressionA2_->traceSize(); + A3 a3 = this->expressionA3_->traceExecution(values, record->trace3, raw); return function_(a1, a2, a3, record->dTdA1, record->dTdA2, record->dTdA3); } diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index b79fdceef..afd5a60e0 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -113,17 +113,39 @@ public: return root_->value(values); } - /// Return value and derivatives - Augmented augmented(const Values& values) const { -#define REVERSE_AD -#ifdef REVERSE_AD + /// Return value and derivatives, forward AD version + Augmented forward(const Values& values) const { + 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& trace, + char* raw) const { + return root_->traceExecution(values, trace, raw); + } + + /// Return value and derivatives, reverse AD version + Augmented reverse(const Values& values) const { + size_t size = traceSize(); + char raw[size]; ExecutionTrace trace; - T value = root_->traceExecution(values, trace); + T value(traceExecution(values, trace, raw)); Augmented augmented(value); trace.startReverseAD(augmented.jacobians()); return augmented; + } + + /// Return value and derivatives + Augmented augmented(const Values& values) const { +#ifdef EXPRESSION_FORWARD_AD + return forward(values); #else - return root_->forward(values); + return reverse(values); #endif } diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index e1694f59a..a6dbe842b 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/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 | Binary::Record] + EXPECT_LONGS_EQUAL(8, sizeof(double)); + 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 = 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 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 trace; - tester.binary_.traceExecution(values, trace); - // Check matrices boost::optional p = trace.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 p = trace.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); - Point3_ p(2); + Pose3_ x_(1); + Point3_ p_(2); - // Concise version - ExpressionFactor f2(model, measured, project(transform_to(x, p))); + // Construct expression, concise evrsion + Point2_ expression = project(transform_to(x_, p_)); + + // traceExecution of shallow tree + typedef UnaryExpression Unary; + typedef BinaryExpression 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 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 p = trace.record(); + CHECK(p); + EXPECT( assert_equal(expected23, (Matrix)(*p)->dTdA1, 1e-9)); + + // Linearization + ExpressionFactor f2(model, measured, expression); EXPECT_DOUBLES_EQUAL(expected_error, f2.error(values), 1e-9); EXPECT_LONGS_EQUAL(2, f2.dim()); boost::shared_ptr gf2 = f2.linearize(values);