diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index a98ab349f..73d475a24 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -27,6 +27,7 @@ #include #include #include +#include // template meta-programming headers #include @@ -39,6 +40,26 @@ class ExpressionFactorBinaryTest; namespace gtsam { +const unsigned TraceAlignment = 16; + +template +T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment){ + // right now only word sized types are supported. + // Easy to extend if needed, + // by somehow inferring the unsigned integer of same size + BOOST_STATIC_ASSERT(sizeof(T) == sizeof(size_t)); + size_t & uiValue = reinterpret_cast(value); + size_t misAlignment = uiValue % requiredAlignment; + if(misAlignment) { + uiValue += requiredAlignment - misAlignment; + } + return value; +} +template +T upAligned(T value, unsigned requiredAlignment = TraceAlignment){ + return upAlign(value, requiredAlignment); +} + template class Expression; @@ -175,6 +196,11 @@ public: typedef ExecutionTrace type; }; +/// Storage type for the execution trace. +/// It enforces the proper alignment in a portable way. +/// Provide a traceSize() sized array of this type to traceExecution as traceStorage. +typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; + //----------------------------------------------------------------------------- /** * Expression node. The superclass for objects that do the heavy lifting @@ -221,7 +247,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const = 0; + ExecutionTraceStorage* traceStorage) const = 0; }; //----------------------------------------------------------------------------- @@ -248,7 +274,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + ExecutionTraceStorage* traceStorage) const { return constant_; } }; @@ -292,7 +318,8 @@ public: /// Construct an execution trace for reverse AD virtual const value_type& traceExecution(const Values& values, - ExecutionTrace& trace, char* raw) const { + ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const { trace.setLeaf(key_); return dynamic_cast(values.at(key_)); } @@ -337,7 +364,7 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + ExecutionTraceStorage* traceStorage) const { trace.setLeaf(key_); return values.at(key_); } @@ -432,7 +459,8 @@ struct FunctionalBase: ExpressionNode { } }; /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, char*& raw) const { + void trace(const Values& values, Record* record, + ExecutionTraceStorage*& traceStorage) const { // base case: does not do anything } }; @@ -512,17 +540,18 @@ struct GenerateFunctionalNode: Argument, Base { }; /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, char*& raw) const { - Base::trace(values, record, raw); // recurse + void trace(const Values& values, Record* record, + ExecutionTraceStorage*& traceStorage) const { + Base::trace(values, record, traceStorage); // recurse // Write an Expression execution trace in record->trace - // Iff Constant or Leaf, this will not write to raw, only to trace. - // Iff the expression is functional, write all Records in raw buffer + // 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::expression->traceExecution(values, - record->Record::This::trace, raw); - // raw is never modified by traceExecution, but if traceExecution has + record->Record::This::trace, traceStorage); + // traceStorage is never modified by traceExecution, but if traceExecution has // written in the buffer, the next caller expects we advance the pointer - raw += This::expression->traceSize(); + traceStorage += This::expression->traceSize(); } }; @@ -587,15 +616,17 @@ struct FunctionalNode { }; /// Construct an execution trace for reverse AD - Record* trace(const Values& values, char* raw) const { + Record* trace(const Values& values, + ExecutionTraceStorage* traceStorage) const { + assert(reinterpret_cast(traceStorage) % TraceAlignment == 0); // Create the record and advance the pointer - Record* record = new (raw) Record(); - raw = (char*) (record + 1); + Record* record = new (traceStorage) Record(); + traceStorage += upAligned(sizeof(Record)); // Record the traces for all arguments - // After this, the raw pointer is set to after what was written - Base::trace(values, record, raw); + // After this, the traceStorage pointer is set to after what was written + Base::trace(values, record, traceStorage); // Return the record for this function evaluation return record; @@ -622,7 +653,7 @@ private: UnaryExpression(Function f, const Expression& e1) : function_(f) { this->template reset(e1.root()); - ExpressionNode::traceSize_ = sizeof(Record) + e1.traceSize(); + ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); } friend class Expression ; @@ -636,9 +667,9 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + ExecutionTraceStorage* traceStorage) const { - Record* record = Base::trace(values, raw); + Record* record = Base::trace(values, traceStorage); trace.setFunction(record); return function_(record->template value(), @@ -671,7 +702,7 @@ private: this->template reset(e1.root()); this->template reset(e2.root()); ExpressionNode::traceSize_ = // - sizeof(Record) + e1.traceSize() + e2.traceSize(); + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); } friend class Expression ; @@ -689,9 +720,9 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + ExecutionTraceStorage* traceStorage) const { - Record* record = Base::trace(values, raw); + Record* record = Base::trace(values, traceStorage); trace.setFunction(record); return function_(record->template value(), @@ -727,7 +758,7 @@ private: this->template reset(e2.root()); this->template reset(e3.root()); ExpressionNode::traceSize_ = // - sizeof(Record) + e1.traceSize() + e2.traceSize() + e3.traceSize(); + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + e3.traceSize(); } friend class Expression ; @@ -745,9 +776,9 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { + ExecutionTraceStorage* traceStorage) const { - Record* record = Base::trace(values, raw); + Record* record = Base::trace(values, traceStorage); trace.setFunction(record); return function_( diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index e8bd8bbe7..5e1d425ed 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -118,8 +118,8 @@ public: /// trace execution, very unsafe, for testing purposes only //TODO this is not only used for testing, but in value() below! T traceExecution(const Values& values, ExecutionTrace& trace, - char* raw) const { - return root_->traceExecution(values, trace, raw); + ExecutionTraceStorage* traceStorage) const { + return root_->traceExecution(values, trace, traceStorage); } /// Return value and derivatives, reverse AD version @@ -130,9 +130,9 @@ public: // with an execution trace, made up entirely of "Record" structs, see // the FunctionalNode class in expression-inl.h size_t size = traceSize(); - char raw[size]; + ExecutionTraceStorage traceStorage[size]; ExecutionTrace trace; - T value(traceExecution(values, trace, raw)); + T value(traceExecution(values, trace, traceStorage)); trace.startReverseAD(jacobians); return value; } diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index c63bfeb6f..e4448fbf7 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -162,9 +162,9 @@ TEST(ExpressionFactor, Binary) { EXPECT_LONGS_EQUAL(expectedRecordSize + 8, size); // Use Variable Length Array, allocated on stack by gcc // Note unclear for Clang: http://clang.llvm.org/compatibility.html#vla - char raw[size]; + ExecutionTraceStorage traceStorage[size]; ExecutionTrace trace; - Point2 value = binary.traceExecution(values, trace, raw); + Point2 value = binary.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); @@ -218,9 +218,9 @@ TEST(ExpressionFactor, Shallow) { size_t size = expression.traceSize(); CHECK(size); EXPECT_LONGS_EQUAL(expectedTraceSize, size); - char raw[size]; + ExecutionTraceStorage traceStorage[size]; ExecutionTrace trace; - Point2 value = expression.traceExecution(values, trace, raw); + Point2 value = expression.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print();