From b8024b10b54686e66fbcf5c6bfc25396eb09e575 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 23:37:50 -0700 Subject: [PATCH] Simplifying UnaryExpression --- gtsam/nonlinear/internal/ExpressionNode.h | 94 ++++++------------- .../nonlinear/tests/testExpressionFactor.cpp | 6 +- 2 files changed, 30 insertions(+), 70 deletions(-) diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 40b071b00..d3c736e9d 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -325,7 +325,6 @@ struct GenerateFunctionalNode: Argument, Base { // case to this unique signature to retrieve the value/trace at any level struct Record: JacobianTrace, Base::Record { - typedef T return_type; typedef JacobianTrace This; /// Print to std::cout @@ -460,22 +459,18 @@ template class UnaryExpression: public ExpressionNode { typedef typename Expression::template UnaryFunction::type Function; - Function function_; boost::shared_ptr > expression1_; + Function function_; - typedef Argument This; ///< The storage we have direct access to +public: - /// Constructor with a unary function f, and input argument e + /// Constructor with a unary function f, and input argument e1 UnaryExpression(Function f, const Expression& e1) : function_(f) { this->expression1_ = e1.root(); ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); } - friend class Expression ; - -public: - /// Return value virtual T value(const Values& values) const { return function_(this->expression1_->value(values), boost::none); @@ -483,45 +478,27 @@ public: /// Return keys that play in this expression virtual std::set keys() const { - std::set keys; // = Base::keys(); - std::set myKeys = this->expression1_->keys(); - keys.insert(myKeys.begin(), myKeys.end()); - return keys; + return this->expression1_->keys(); } /// Return dimensions for each argument virtual void dims(std::map& map) const { - // Base::dims(map); this->expression1_->dims(map); } // Inner Record Class - // The reason we inherit from JacobianTrace is because we can then - // case to this unique signature to retrieve the value/trace at any level - struct Record: public CallRecordImplementor::dimension>, - JacobianTrace { + struct Record: public CallRecordImplementor::dimension> { - typedef T return_type; - typedef JacobianTrace This; - - /// Access Jacobian - template - typename Jacobian::type& jacobian() { - return static_cast&>(*this).dTdA; - } - - /// Access Value - template - const A& value() const { - return static_cast const &>(*this).value; - } + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; /// Print to std::cout void print(const std::string& indent) const { std::cout << indent << "UnaryExpression::Record {" << std::endl; static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << indent << This::dTdA.format(matlab) << std::endl; - This::trace.print(indent); + std::cout << indent << dTdA1.format(matlab) << std::endl; + trace1.print(indent); std::cout << indent << "}" << std::endl; } @@ -535,57 +512,40 @@ public: // ExecutionTrace::reverseAD1 just passes this on to CallRecord::reverseAD2 // which calls the correctly sized CallRecord::reverseAD3, which in turn // calls reverseAD4 below. - This::trace.reverseAD1(This::dTdA, jacobians); + trace1.reverseAD1(dTdA1, jacobians); } /// Given df/dT, multiply in dT/dA and continue reverse AD process // Cols is always known at compile time template void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { - This::trace.reverseAD1(dFdT * This::dTdA, jacobians); + trace1.reverseAD1(dFdT * dTdA1, jacobians); } }; /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, - ExecutionTraceStorage*& traceStorage) const { + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + + // Create the record at the start of the traceStorage and advance the pointer + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + + // Record the traces for all arguments + // After this, the traceStorage pointer is set to after what was written // Write an Expression execution trace in record->trace // Iff Constant or Leaf, this will not write to traceStorage, only to trace. // Iff the expression is functional, write all Records in traceStorage buffer // Return value of type T is recorded in record->value - record->Record::This::value = this->expression1_->traceExecution(values, - record->Record::This::trace, traceStorage); - // traceStorage is never modified by traceExecution, but if traceExecution has + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + + // ptr is never modified by traceExecution, but if traceExecution has // written in the buffer, the next caller expects we advance the pointer - traceStorage += this->expression1_->traceSize(); - } - - /// Construct an execution trace for reverse AD - Record* trace(const Values& values, - ExecutionTraceStorage* traceStorage) const { - assert(reinterpret_cast(traceStorage) % TraceAlignment == 0); - - // Create the record and advance the pointer - Record* record = new (traceStorage) Record(); - traceStorage += upAligned(sizeof(Record)); - - // Record the traces for all arguments - // After this, the traceStorage pointer is set to after what was written - this->trace(values, record, traceStorage); - - // Return the record for this function evaluation - return record; - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - - Record* record = this->trace(values, traceStorage); + ptr += expression1_->traceSize(); trace.setFunction(record); - return function_(record->template value(), - record->template jacobian()); + return function_(record->value1, record->dTdA1); } }; diff --git a/gtsam/nonlinear/tests/testExpressionFactor.cpp b/gtsam/nonlinear/tests/testExpressionFactor.cpp index ead6e0176..3573378ed 100644 --- a/gtsam/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam/nonlinear/tests/testExpressionFactor.cpp @@ -253,10 +253,10 @@ TEST(ExpressionFactor, Shallow) { typedef internal::UnaryExpression Unary; typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record); - EXPECT_LONGS_EQUAL(112, sizeof(Unary::Record)); + EXPECT_LONGS_EQUAL(96, sizeof(Unary::Record)); #ifdef GTSAM_USE_QUATERNIONS EXPECT_LONGS_EQUAL(352, sizeof(Binary::Record)); - LONGS_EQUAL(112+352, expectedTraceSize); + LONGS_EQUAL(96+352, expectedTraceSize); #else EXPECT_LONGS_EQUAL(400, sizeof(Binary::Record)); LONGS_EQUAL(112+400, expectedTraceSize); @@ -277,7 +277,7 @@ TEST(ExpressionFactor, Shallow) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT(assert_equal(expected23, (Matrix)(*r)->jacobian(), 1e-9)); + EXPECT(assert_equal(expected23, (Matrix)(*r)->dTdA1, 1e-9)); // Linearization ExpressionFactor f2(model, measured, expression);