diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 7f59dcb7d..58f6de6b4 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; @@ -193,6 +214,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 @@ -239,7 +265,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; }; //----------------------------------------------------------------------------- @@ -266,7 +292,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_; } }; @@ -310,7 +336,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_)); } @@ -355,7 +382,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_); } @@ -450,7 +477,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 } }; @@ -530,17 +558,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(); } }; @@ -605,15 +634,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; @@ -640,7 +671,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 ; @@ -654,9 +685,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(), @@ -689,7 +720,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 ; @@ -707,9 +738,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(), @@ -745,7 +776,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 ; @@ -763,9 +794,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 c673dd9a7..4343e7085 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -161,9 +161,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(); @@ -217,9 +217,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();