From e20a704a96ce28dc99f57650840b1bed9e8bc5fa Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 4 May 2015 10:06:55 -0700 Subject: [PATCH] Moved a lot of things to Expression-inl.h, made interface cleaner and more encapsulated. With Jing on skype... --- gtsam/nonlinear/Expression-inl.h | 293 ++++++++++++++++++++++--------- gtsam/nonlinear/Expression.h | 184 +++++++++---------- 2 files changed, 294 insertions(+), 183 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 3fe33f2d1..3267e20be 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -40,7 +40,9 @@ class ExpressionFactorBinaryTest; namespace gtsam { -const unsigned TraceAlignment = 16; +//----------------------------------------------------------------------------- +// ExecutionTrace.h +//----------------------------------------------------------------------------- template T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { @@ -60,30 +62,6 @@ T upAligned(T value, unsigned requiredAlignment = TraceAlignment) { return upAlign(value, requiredAlignment); } -template -class Expression; - -/** - * Expressions are designed to write their derivatives into an already allocated - * Jacobian of the correct size, of type VerticalBlockMatrix. - * The JacobianMap provides a mapping from keys to the underlying blocks. - */ -class JacobianMap { - const FastVector& keys_; - VerticalBlockMatrix& Ab_; -public: - JacobianMap(const FastVector& keys, VerticalBlockMatrix& Ab) : - keys_(keys), Ab_(Ab) { - } - /// Access via key - VerticalBlockMatrix::Block operator()(Key key) { - FastVector::const_iterator it = std::find(keys_.begin(), keys_.end(), - key); - DenseIndex block = it - keys_.begin(); - return Ab_(block); - } -}; - //----------------------------------------------------------------------------- namespace internal { @@ -215,12 +193,10 @@ 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; - //----------------------------------------------------------------------------- +// ExpressionNode.h +//----------------------------------------------------------------------------- + /** * Expression node. The superclass for objects that do the heavy lifting * An Expression has a pointer to an ExpressionNode underneath @@ -247,10 +223,12 @@ public: } /// Streaming - GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, + GTSAM_EXPORT + friend std::ostream &operator<<(std::ostream &os, const ExpressionNode& node) { os << "Expression of type " << typeid(T).name(); - if (node.traceSize_>0) os << ", trace size = " << node.traceSize_; + if (node.traceSize_ > 0) + os << ", trace size = " << node.traceSize_; os << "\n"; return os; } @@ -307,11 +285,10 @@ public: } }; - //----------------------------------------------------------------------------- /// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value template -class LeafExpression : public ExpressionNode { +class LeafExpression: public ExpressionNode { typedef T value_type; /// The key into values @@ -412,15 +389,7 @@ public: /// meta-function to generate fixed-size JacobianTA type template struct Jacobian { - typedef Eigen::Matrix::dimension, - traits::dimension> type; -}; - -/// meta-function to generate JacobianTA optional reference -template -struct MakeOptionalJacobian { - typedef OptionalJacobian::dimension, - traits::dimension> type; + typedef Eigen::Matrix::dimension, traits::dimension> type; }; /** @@ -524,8 +493,7 @@ struct GenerateFunctionalNode: Argument, Base { /// 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 { + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { Base::Record::reverseAD4(dFdT, jacobians); This::trace.reverseAD1(dFdT * This::dTdA, jacobians); } @@ -629,16 +597,9 @@ struct FunctionalNode { /// Unary Function Expression template -class UnaryExpression : public ExpressionNode { - - typedef typename MakeOptionalJacobian::type OJ1; - -public: - - typedef boost::function Function; - -private: +class UnaryExpression: public ExpressionNode { + typedef typename UnaryFunction::type Function; Function function_; boost::shared_ptr > expression1_; @@ -660,6 +621,20 @@ public: return function_(this->expression1_->value(values), boost::none); } + /// 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 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 @@ -706,8 +681,7 @@ public: /// 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 { + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { This::trace.reverseAD1(dFdT * This::dTdA, jacobians); } }; @@ -748,7 +722,6 @@ public: ExecutionTraceStorage* traceStorage) const { Record* record = this->trace(values, traceStorage); - record->print("record: "); trace.setFunction(record); return function_(record->template value(), @@ -760,20 +733,15 @@ public: /// Binary Expression template -class BinaryExpression: - public FunctionalNode >::type { - - typedef typename MakeOptionalJacobian::type OJ1; - typedef typename MakeOptionalJacobian::type OJ2; +class BinaryExpression: public FunctionalNode >::type { + typedef typename FunctionalNode >::type Base; public: - - typedef boost::function Function; - typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; private: + typedef typename BinaryFunction::type Function; Function function_; /// Constructor with a ternary function f, and three input arguments @@ -801,14 +769,14 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { + ExecutionTraceStorage* traceStorage) const { Record* record = Base::trace(values, traceStorage); trace.setFunction(record); return function_(record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian()); + record->template value(), record->template jacobian(), + record->template jacobian()); } }; @@ -816,21 +784,14 @@ public: /// Ternary Expression template -class TernaryExpression: - public FunctionalNode >::type { +class TernaryExpression: public FunctionalNode >::type { - typedef typename MakeOptionalJacobian::type OJ1; - typedef typename MakeOptionalJacobian::type OJ2; - typedef typename MakeOptionalJacobian::type OJ3; - -public: - - typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; private: + typedef typename TernaryFunction::type Function; Function function_; /// Constructor with a ternary function f, and three input arguments @@ -860,17 +821,187 @@ public: /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { + ExecutionTraceStorage* traceStorage) const { Record* record = Base::trace(values, traceStorage); trace.setFunction(record); return function_( - record->template value(), record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian(), record->template jacobian()); + record->template value(), record->template value(), + record->template value(), record->template jacobian(), + record->template jacobian(), record->template jacobian()); } }; + //----------------------------------------------------------------------------- +// Esxpression-inl.h +//----------------------------------------------------------------------------- + +/// Print +template +void Expression::print(const std::string& s) const { + std::cout << s << *root_ << std::endl; +} + +// Construct a constant expression +template +Expression::Expression(const T& value) : + root_(new ConstantExpression(value)) { +} + +// Construct a leaf expression, with Key +template +Expression::Expression(const Key& key) : + root_(new LeafExpression(key)) { +} + +// Construct a leaf expression, with Symbol +template +Expression::Expression(const Symbol& symbol) : + root_(new LeafExpression(symbol)) { +} + +// Construct a leaf expression, creating Symbol +template +Expression::Expression(unsigned char c, size_t j) : + root_(new LeafExpression(Symbol(c, j))) { +} + +/// Construct a nullary method expression +template +template +Expression::Expression(const Expression& expression, + T (A::*method)(typename MakeOptionalJacobian::type) const) : + root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { +} + +/// Construct a unary function expression +template +template +Expression::Expression(typename UnaryFunction::type function, + const Expression& expression) : + root_(new UnaryExpression(function, expression)) { +} + +/// Construct a unary method expression +template +template +Expression::Expression(const Expression& expression1, + T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2) : + root_( + new BinaryExpression(boost::bind(method, _1, _2, _3, _4), + expression1, expression2)) { +} + +/// Construct a binary function expression +template +template +Expression::Expression(typename BinaryFunction::type function, + const Expression& expression1, const Expression& expression2) : + root_(new BinaryExpression(function, expression1, expression2)) { +} + +/// Construct a binary method expression +template +template +Expression::Expression(const Expression& expression1, + T (A1::*method)(const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2, const Expression& expression3) : + root_( + new TernaryExpression( + boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, + expression2, expression3)) { +} + +/// Construct a ternary function expression +template +template +Expression::Expression( + typename TernaryFunction::type function, + const Expression& expression1, const Expression& expression2, + const Expression& expression3) : + root_( + new TernaryExpression(function, expression1, expression2, + expression3)) { +} + +/// private version that takes keys and dimensions, returns derivatives +template +T Expression::value(const Values& values, const FastVector& keys, + const FastVector& dims, std::vector& H) const { + + // H should be pre-allocated + assert(H.size()==keys.size()); + + // Pre-allocate and zero VerticalBlockMatrix + static const int Dim = traits::dimension; + VerticalBlockMatrix Ab(dims, Dim); + Ab.matrix().setZero(); + JacobianMap jacobianMap(keys, Ab); + + // Call unsafe version + T result = value(values, jacobianMap); + + // Copy blocks into the vector of jacobians passed in + for (DenseIndex i = 0; i < static_cast(keys.size()); i++) + H[i] = Ab(i); + + return result; +} + +template +T Expression::traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const { + return root_->traceExecution(values, trace, traceStorage); +} + +template +T Expression::value(const Values& values, JacobianMap& jacobians) const { + // The following piece of code is absolutely crucial for performance. + // We allocate a block of memory on the stack, which can be done at runtime + // with modern C++ compilers. The traceExecution then fills this memory + // with an execution trace, made up entirely of "Record" structs, see + // the FunctionalNode class in expression-inl.h + size_t size = traceSize(); + + // Windows does not support variable length arrays, so memory must be dynamically + // allocated on Visual Studio. For more information see the issue below + // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio +#ifdef _MSC_VER + ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size]; +#else + ExecutionTraceStorage traceStorage[size]; +#endif + + ExecutionTrace trace; + T value(this->traceExecution(values, trace, traceStorage)); + trace.startReverseAD1(jacobians); + +#ifdef _MSC_VER + delete[] traceStorage; +#endif + + return value; +} + +// JacobianMap: +JacobianMap::JacobianMap(const FastVector& keys, VerticalBlockMatrix& Ab) : + keys_(keys), Ab_(Ab) { +} + +VerticalBlockMatrix::Block JacobianMap::operator()(Key key) { + FastVector::const_iterator it = std::find(keys_.begin(), keys_.end(), + key); + DenseIndex block = it - keys_.begin(); + return Ab_(block); +} + +//----------------------------------------------------------------------------- + } diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index a39b1557c..502579cf7 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -19,9 +19,10 @@ #pragma once -#include #include +#include #include +#include #include #include @@ -31,9 +32,46 @@ class ExpressionFactorShallowTest; namespace gtsam { -// Forward declare +// Forward declares +class Values; +template class ExecutionTrace; +template class ExpressionNode; template class ExpressionFactor; +// A JacobianMap is the primary mechanism by which derivatives are returned. +// For clarity, it is forward declared here but implemented at the end of this header. +class JacobianMap; + +// Expressions wrap trees of functions that can evaluate their own derivatives. +// The meta-functions below provide a handy to specify the type of those functions +template +struct UnaryFunction { + typedef boost::function< + T(const A1&, typename MakeOptionalJacobian::type)> type; +}; + +template +struct BinaryFunction { + typedef boost::function< + T(const A1&, const A2&, typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> type; +}; + +template +struct TernaryFunction { + typedef boost::function< + T(const A1&, const A2&, const A3&, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type)> 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. +const unsigned TraceAlignment = 16; +typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; + /** * Expression class that supports automatic differentiation */ @@ -53,85 +91,56 @@ private: public: /// Print - void print(const std::string& s) const { - std::cout << s << *root_ << std::endl; - } + void print(const std::string& s) const; - // Construct a constant expression - Expression(const T& value) : - root_(new ConstantExpression(value)) { - } + /// Construct a constant expression + Expression(const T& value); - // Construct a leaf expression, with Key - Expression(const Key& key) : - root_(new LeafExpression(key)) { - } + /// Construct a leaf expression, with Key + Expression(const Key& key); - // Construct a leaf expression, with Symbol - Expression(const Symbol& symbol) : - root_(new LeafExpression(symbol)) { - } + /// Construct a leaf expression, with Symbol + Expression(const Symbol& symbol); - // Construct a leaf expression, creating Symbol - Expression(unsigned char c, size_t j) : - root_(new LeafExpression(Symbol(c, j))) { - } + /// Construct a leaf expression, creating Symbol + Expression(unsigned char c, size_t j); /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(typename MakeOptionalJacobian::type) const) : - root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { - } + T (A::*method)(typename MakeOptionalJacobian::type) const); /// Construct a unary function expression template - Expression(typename UnaryExpression::Function function, - const Expression& expression) : - root_(new UnaryExpression(function, expression)) { - } + Expression(typename UnaryFunction::type function, + const Expression& expression); /// Construct a unary method expression template Expression(const Expression& expression1, T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, typename MakeOptionalJacobian::type) const, - const Expression& expression2) : - root_( - new BinaryExpression(boost::bind(method, _1, _2, _3, _4), - expression1, expression2)) { - } + const Expression& expression2); /// Construct a binary function expression template - Expression(typename BinaryExpression::Function function, - const Expression& expression1, const Expression& expression2) : - root_(new BinaryExpression(function, expression1, expression2)) { - } + Expression(typename BinaryFunction::type function, + const Expression& expression1, const Expression& expression2); /// Construct a binary method expression template Expression(const Expression& expression1, T (A1::*method)(const A2&, const A3&, - typename TernaryExpression::OJ1, - typename TernaryExpression::OJ2, - typename TernaryExpression::OJ3) const, - const Expression& expression2, const Expression& expression3) : - root_( - new TernaryExpression( - boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, - expression2, expression3)) { - } + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type, + typename MakeOptionalJacobian::type) const, + const Expression& expression2, const Expression& expression3); /// Construct a ternary function expression template - Expression(typename TernaryExpression::Function function, + Expression(typename TernaryFunction::type function, const Expression& expression1, const Expression& expression2, - const Expression& expression3) : - root_( - new TernaryExpression(function, expression1, - expression2, expression3)) { - } + const Expression& expression3); /// Return root const boost::shared_ptr >& root() const { @@ -195,65 +204,18 @@ private: /// private version that takes keys and dimensions, returns derivatives T value(const Values& values, const FastVector& keys, - const FastVector& dims, std::vector& H) const { - - // H should be pre-allocated - assert(H.size()==keys.size()); - - // Pre-allocate and zero VerticalBlockMatrix - static const int Dim = traits::dimension; - VerticalBlockMatrix Ab(dims, Dim); - Ab.matrix().setZero(); - JacobianMap jacobianMap(keys, Ab); - - // Call unsafe version - T result = value(values, jacobianMap); - - // Copy blocks into the vector of jacobians passed in - for (DenseIndex i = 0; i < static_cast(keys.size()); i++) - H[i] = Ab(i); - - return result; - } + const FastVector& dims, std::vector& H) const; /// trace execution, very unsafe T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - return root_->traceExecution(values, trace, traceStorage); - } + ExecutionTraceStorage* traceStorage) const; /** * @brief Return value and derivatives, reverse AD version * This very unsafe method needs a JacobianMap with correctly allocated * and initialized VerticalBlockMatrix, hence is declared private. */ - T value(const Values& values, JacobianMap& jacobians) const { - // The following piece of code is absolutely crucial for performance. - // We allocate a block of memory on the stack, which can be done at runtime - // with modern C++ compilers. The traceExecution then fills this memory - // with an execution trace, made up entirely of "Record" structs, see - // the FunctionalNode class in expression-inl.h - size_t size = traceSize(); - - // Windows does not support variable length arrays, so memory must be dynamically - // allocated on Visual Studio. For more information see the issue below - // https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio -#ifdef _MSC_VER - ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size]; -#else - ExecutionTraceStorage traceStorage[size]; -#endif - - ExecutionTrace trace; - T value(traceExecution(values, trace, traceStorage)); - trace.startReverseAD1(jacobians); - -#ifdef _MSC_VER - delete[] traceStorage; -#endif - - return value; - } + T value(const Values& values, JacobianMap& jacobians) const; // be very selective on who can access these private methods: friend class ExpressionFactor ; @@ -261,6 +223,22 @@ private: }; +// Expressions are designed to write their derivatives into an already allocated +// Jacobian of the correct size, of type VerticalBlockMatrix. +// The JacobianMap provides a mapping from keys to the underlying blocks. +class JacobianMap { +private: + const FastVector& keys_; + VerticalBlockMatrix& Ab_; + +public: + /// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab + JacobianMap(const FastVector& keys, VerticalBlockMatrix& Ab); + + /// Access blocks of via key + VerticalBlockMatrix::Block operator()(Key key); +}; + // http://stackoverflow.com/questions/16260445/boost-bind-to-operator template struct apply_compose { @@ -292,3 +270,5 @@ std::vector > createUnknowns(size_t n, char c, size_t start = 0) { } +#include +