From 660acec58ef0f7c180cee8c9842591d07869d4bc Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 3 May 2015 20:40:13 -0700 Subject: [PATCH 01/35] Removed MPL complexity from UnaryExpression. --- gtsam/nonlinear/Expression-inl.h | 95 ++++++++++++++++++++++++++++++-- 1 file changed, 89 insertions(+), 6 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index a86e7312a..9893d8370 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -629,24 +629,25 @@ struct FunctionalNode { /// Unary Function Expression template -class UnaryExpression: public FunctionalNode >::type { +class UnaryExpression : public ExpressionNode { typedef typename MakeOptionalJacobian::type OJ1; public: typedef boost::function Function; - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; private: Function function_; + boost::shared_ptr > expression1_; + + typedef Argument This; ///< The storage we have direct access to /// Constructor with a unary function f, and input argument e UnaryExpression(Function f, const Expression& e1) : function_(f) { - this->template reset(e1.root()); + this->expression1_ = e1.root(); ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); } @@ -656,14 +657,96 @@ public: /// Return value virtual T value(const Values& values) const { - return function_(this->template expression()->value(values), boost::none); + return function_(this->expression1_->value(values), boost::none); + } + + // 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 internal::CallRecordImplementor::dimension>, JacobianTrace { + + 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; + } + + /// Print to std::cout + void print(const std::string& indent) const { + static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); + std::cout << This::dTdA.format(matlab) << std::endl; + This::trace.print(indent); + } + + /// Start the reverse AD process + void startReverseAD4(JacobianMap& jacobians) const { + // This is the crucial point where the size of the AD pipeline is selected. + // One pipeline is started for each argument, but the number of rows in each + // pipeline is the same, namely the dimension of the output argument T. + // For example, if the entire expression is rooted by a binary function + // yielding a 2D result, then the matrix dTdA will have 2 rows. + // 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); + } + + /// 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); + } + }; + + /// Construct an execution trace for reverse AD + void trace(const Values& values, Record* record, + ExecutionTraceStorage*& traceStorage) const { + // 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 + // 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 = Base::trace(values, traceStorage); + Record* record = this->trace(values, traceStorage); + record->print("record: "); trace.setFunction(record); return function_(record->template value(), From 11de86cc1e64eb83f1d13257475a6c8988511fba Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 3 May 2015 20:45:52 -0700 Subject: [PATCH 02/35] Better print --- gtsam/nonlinear/Expression-inl.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 9893d8370..3fe33f2d1 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -683,9 +683,11 @@ public: /// 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 << This::dTdA.format(matlab) << std::endl; + std::cout << indent << This::dTdA.format(matlab) << std::endl; This::trace.print(indent); + std::cout << indent << "}" << std::endl; } /// Start the reverse AD process From e2e6d1b1167737f351749d0b4dc6d71c6ca58d86 Mon Sep 17 00:00:00 2001 From: dellaert Date: Sun, 3 May 2015 20:46:15 -0700 Subject: [PATCH 03/35] Slight refactor of tests, added (commented out) dynamic test --- gtsam/nonlinear/tests/testExpression.cpp | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 80100af5e..a6b9386ac 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -74,18 +74,21 @@ TEST(Expression, Leaves) { /* ************************************************************************* */ // Unary(Leaf) namespace unary { -Point2 f0(const Point3& p, OptionalJacobian<2,3> H) { +Point2 f1(const Point3& p, OptionalJacobian<2,3> H) { return Point2(); } double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } +Vector f3(const Point3& p, OptionalJacobian H) { + return p.vector(); +} Expression p(1); set expected = list_of(1); } -TEST(Expression, Unary0) { +TEST(Expression, Unary1) { using namespace unary; - Expression e(f0, p); + Expression e(f1, p); EXPECT(expected == e.keys()); } TEST(Expression, Unary2) { @@ -94,6 +97,12 @@ TEST(Expression, Unary2) { EXPECT(expected == e.keys()); } /* ************************************************************************* */ +// Unary(Leaf), dynamic +TEST(Expression, Unary3) { + using namespace unary; +// Expression e(f3, p); +} +/* ************************************************************************* */ //Nullary Method TEST(Expression, NullaryMethod) { From 0a19c078e7d2f146219d8919296c4573afb36867 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 4 May 2015 10:04:42 -0700 Subject: [PATCH 04/35] Added traceSize checks --- gtsam/nonlinear/tests/testExpression.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index a6b9386ac..84f180609 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -89,11 +89,13 @@ set expected = list_of(1); TEST(Expression, Unary1) { using namespace unary; Expression e(f1, p); + EXPECT_LONGS_EQUAL(112,e.traceSize()); EXPECT(expected == e.keys()); } TEST(Expression, Unary2) { using namespace unary; Expression e(f2, p); + EXPECT_LONGS_EQUAL(80,e.traceSize()); EXPECT(expected == e.keys()); } /* ************************************************************************* */ From e20a704a96ce28dc99f57650840b1bed9e8bc5fa Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 4 May 2015 10:06:55 -0700 Subject: [PATCH 05/35] 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 + From 3a99e74bb7bb1dc857073e7f53526605033dc599 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 4 May 2015 10:07:10 -0700 Subject: [PATCH 06/35] Use public meta-function now --- gtsam/nonlinear/tests/testExpressionFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/tests/testExpressionFactor.cpp b/gtsam/nonlinear/tests/testExpressionFactor.cpp index 99b8120e3..94e32448c 100644 --- a/gtsam/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam/nonlinear/tests/testExpressionFactor.cpp @@ -327,7 +327,7 @@ TEST(ExpressionFactor, tree) { boost::shared_ptr gf2 = f2.linearize(values); EXPECT( assert_equal(*expected, *gf2, 1e-9)); - TernaryExpression::Function fff = project6; + TernaryFunction::type fff = project6; // Try ternary version ExpressionFactor f3(model, measured, project3(x, p, K)); From 9a0f973e7175f4a0c9170203e4a53bdc4673521b Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 4 May 2015 10:11:02 -0700 Subject: [PATCH 07/35] forward declare traits and moved MakeOptionalJacobian here... --- gtsam/base/OptionalJacobian.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index a83333caa..9ab8bc598 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -168,5 +168,20 @@ public: Jacobian* operator->(){ return pointer_; } }; +// forward declate +template struct traits; + +/** + * @brief: meta-function to generate JacobianTA optional reference + * Used mainly by Expressions + * @param T return type + * @param A argument type + */ +template +struct MakeOptionalJacobian { + typedef OptionalJacobian::dimension, + traits::dimension> type; +}; + } // namespace gtsam From 949efebe72ff0870e301d3172934a903f7dbddca Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Wed, 6 May 2015 12:02:46 -0400 Subject: [PATCH 08/35] fix missing boost header under linux --- gtsam/nonlinear/Expression.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 502579cf7..1c4331de8 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -27,6 +27,7 @@ #include #include #include +#include class ExpressionFactorShallowTest; From 376c9e409d8a2efd6fa57b4a3dd77c2c72b80e57 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Wed, 6 May 2015 14:50:01 -0400 Subject: [PATCH 09/35] move some Expression implementations to include header --- gtsam/nonlinear/Expression-inl.h | 42 ++++++++++++++++++++++++++++++++ gtsam/nonlinear/Expression.h | 27 ++++---------------- 2 files changed, 47 insertions(+), 22 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 3267e20be..936691166 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -931,6 +931,48 @@ Expression::Expression( expression3)) { } + +/// Return root +template +const boost::shared_ptr >& Expression::root() const { + return root_; +} + +// Return size needed for memory buffer in traceExecution +template +size_t Expression::traceSize() const { + return root_->traceSize(); +} + +/// Return keys that play in this expression +template +std::set Expression::keys() const { + return root_->keys(); +} + +/// Return dimensions for each argument, as a map +template +void Expression::dims(std::map& map) const { + root_->dims(map); +} + +/** + * @brief Return value and optional derivatives, reverse AD version + * Notes: this is not terribly efficient, and H should have correct size. + * The order of the Jacobians is same as keys in either keys() or dims() + */ +template +T Expression::value(const Values& values, boost::optional&> H) const { + + if (H) { + // Call private version that returns derivatives in H + KeysAndDims pair = keysAndDims(); + return value(values, pair.first, pair.second, *H); + } else + // no derivatives needed, just return value + return root_->value(values); +} + /// private version that takes keys and dimensions, returns derivatives template T Expression::value(const Values& values, const FastVector& keys, diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 1c4331de8..c400fbe12 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -144,24 +144,16 @@ public: const Expression& expression3); /// Return root - const boost::shared_ptr >& root() const { - return root_; - } + const boost::shared_ptr >& root() const; // Return size needed for memory buffer in traceExecution - size_t traceSize() const { - return root_->traceSize(); - } + size_t traceSize() const; /// Return keys that play in this expression - std::set keys() const { - return root_->keys(); - } + std::set keys() const; /// Return dimensions for each argument, as a map - void dims(std::map& map) const { - root_->dims(map); - } + void dims(std::map& map) const; /** * @brief Return value and optional derivatives, reverse AD version @@ -169,16 +161,7 @@ public: * The order of the Jacobians is same as keys in either keys() or dims() */ T value(const Values& values, boost::optional&> H = - boost::none) const { - - if (H) { - // Call private version that returns derivatives in H - KeysAndDims pair = keysAndDims(); - return value(values, pair.first, pair.second, *H); - } else - // no derivatives needed, just return value - return root_->value(values); - } + boost::none) const; /** * @return a "deep" copy of this Expression From 3bce2344032e1807793332176fc6b50e95fdaa4c Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Sun, 10 May 2015 02:23:52 -0400 Subject: [PATCH 10/35] move ExpressionNode to seperate .f file --- gtsam/nonlinear/Expression-inl.h | 64 +---------------------- gtsam/nonlinear/ExpressionNode.h | 87 ++++++++++++++++++++++++++++++++ 2 files changed, 88 insertions(+), 63 deletions(-) create mode 100644 gtsam/nonlinear/ExpressionNode.h diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 936691166..174fc31a6 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -19,6 +19,7 @@ #pragma once +#include #include #include #include @@ -193,69 +194,6 @@ public: typedef ExecutionTrace type; }; -//----------------------------------------------------------------------------- -// ExpressionNode.h -//----------------------------------------------------------------------------- - -/** - * Expression node. The superclass for objects that do the heavy lifting - * An Expression has a pointer to an ExpressionNode underneath - * allowing Expressions to have polymorphic behaviour even though they - * are passed by value. This is the same way boost::function works. - * http://loki-lib.sourceforge.net/html/a00652.html - */ -template -class ExpressionNode { - -protected: - - size_t traceSize_; - - /// Constructor, traceSize is size of the execution trace of expression rooted here - ExpressionNode(size_t traceSize = 0) : - traceSize_(traceSize) { - } - -public: - - /// Destructor - virtual ~ExpressionNode() { - } - - /// Streaming - 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_; - os << "\n"; - return os; - } - - /// Return keys that play in this expression as a set - virtual std::set keys() const { - std::set keys; - return keys; - } - - /// Return dimensions for each argument, as a map - virtual void dims(std::map& map) const { - } - - // Return size needed for memory buffer in traceExecution - size_t traceSize() const { - return traceSize_; - } - - /// Return value - virtual T value(const Values& values) const = 0; - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const = 0; -}; - //----------------------------------------------------------------------------- /// Constant Expression template diff --git a/gtsam/nonlinear/ExpressionNode.h b/gtsam/nonlinear/ExpressionNode.h new file mode 100644 index 000000000..465cab013 --- /dev/null +++ b/gtsam/nonlinear/ExpressionNode.h @@ -0,0 +1,87 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ExpressionNode.h + * @date May 10, 2015 + * @author Frank Dellaert + * @author Paul Furgale + * @brief ExpressionNode class + */ + +#pragma once + +#include +#include + + +namespace gtsam { + +/** + * Expression node. The superclass for objects that do the heavy lifting + * An Expression has a pointer to an ExpressionNode underneath + * allowing Expressions to have polymorphic behaviour even though they + * are passed by value. This is the same way boost::function works. + * http://loki-lib.sourceforge.net/html/a00652.html + */ +template +class ExpressionNode { + +protected: + + size_t traceSize_; + + /// Constructor, traceSize is size of the execution trace of expression rooted here + ExpressionNode(size_t traceSize = 0) : + traceSize_(traceSize) { + } + +public: + + /// Destructor + virtual ~ExpressionNode() { + } + + /// Streaming + 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_; + os << "\n"; + return os; + } + + /// Return keys that play in this expression as a set + virtual std::set keys() const { + std::set keys; + return keys; + } + + /// Return dimensions for each argument, as a map + virtual void dims(std::map& map) const { + } + + // Return size needed for memory buffer in traceExecution + size_t traceSize() const { + return traceSize_; + } + + /// Return value + virtual T value(const Values& values) const = 0; + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const = 0; +}; + +} From a99aaf892c371b8d866602ba1075eed4b05da664 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Sun, 10 May 2015 02:36:42 -0400 Subject: [PATCH 11/35] move meta functions to Expression class scope --- gtsam/nonlinear/Expression-inl.h | 12 ++--- gtsam/nonlinear/Expression.h | 54 +++++++++---------- .../nonlinear/tests/testExpressionFactor.cpp | 2 +- 3 files changed, 34 insertions(+), 34 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 174fc31a6..5e262977a 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -537,7 +537,7 @@ struct FunctionalNode { template class UnaryExpression: public ExpressionNode { - typedef typename UnaryFunction::type Function; + typedef typename Expression::template UnaryFunction::type Function; Function function_; boost::shared_ptr > expression1_; @@ -679,7 +679,7 @@ public: private: - typedef typename BinaryFunction::type Function; + typedef typename Expression::template BinaryFunction::type Function; Function function_; /// Constructor with a ternary function f, and three input arguments @@ -729,7 +729,7 @@ class TernaryExpression: public FunctionalNode private: - typedef typename TernaryFunction::type Function; + typedef typename Expression::template TernaryFunction::type Function; Function function_; /// Constructor with a ternary function f, and three input arguments @@ -817,7 +817,7 @@ Expression::Expression(const Expression& expression, /// Construct a unary function expression template template -Expression::Expression(typename UnaryFunction::type function, +Expression::Expression(typename UnaryFunction::type function, const Expression& expression) : root_(new UnaryExpression(function, expression)) { } @@ -837,7 +837,7 @@ Expression::Expression(const Expression& expression1, /// Construct a binary function expression template template -Expression::Expression(typename BinaryFunction::type function, +Expression::Expression(typename BinaryFunction::type function, const Expression& expression1, const Expression& expression2) : root_(new BinaryExpression(function, expression1, expression2)) { } @@ -861,7 +861,7 @@ Expression::Expression(const Expression& expression1, template template Expression::Expression( - typename TernaryFunction::type function, + typename TernaryFunction::type function, const Expression& expression1, const Expression& expression2, const Expression& expression3) : root_( diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index c400fbe12..df575dbbf 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -43,30 +43,6 @@ template class ExpressionFactor; // 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. @@ -91,6 +67,30 @@ private: public: + // 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; + }; + /// Print void print(const std::string& s) const; @@ -113,7 +113,7 @@ public: /// Construct a unary function expression template - Expression(typename UnaryFunction::type function, + Expression(typename UnaryFunction::type function, const Expression& expression); /// Construct a unary method expression @@ -125,7 +125,7 @@ public: /// Construct a binary function expression template - Expression(typename BinaryFunction::type function, + Expression(typename BinaryFunction::type function, const Expression& expression1, const Expression& expression2); /// Construct a binary method expression @@ -139,7 +139,7 @@ public: /// Construct a ternary function expression template - Expression(typename TernaryFunction::type function, + Expression(typename TernaryFunction::type function, const Expression& expression1, const Expression& expression2, const Expression& expression3); diff --git a/gtsam/nonlinear/tests/testExpressionFactor.cpp b/gtsam/nonlinear/tests/testExpressionFactor.cpp index 94e32448c..900293261 100644 --- a/gtsam/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam/nonlinear/tests/testExpressionFactor.cpp @@ -327,7 +327,7 @@ TEST(ExpressionFactor, tree) { boost::shared_ptr gf2 = f2.linearize(values); EXPECT( assert_equal(*expected, *gf2, 1e-9)); - TernaryFunction::type fff = project6; + Expression::TernaryFunction::type fff = project6; // Try ternary version ExpressionFactor f3(model, measured, project3(x, p, K)); From c6d723baad41a595ccfaabe406a762136155a8ef Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 18:42:49 -0700 Subject: [PATCH 12/35] Moved entire ExpressionNode hierarchy now --- gtsam/nonlinear/Expression-inl.h | 610 +------------------------------ gtsam/nonlinear/ExpressionNode.h | 610 ++++++++++++++++++++++++++++++- 2 files changed, 610 insertions(+), 610 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 5e262977a..102b2ff8f 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -20,8 +20,6 @@ #pragma once #include -#include -#include #include #include @@ -29,42 +27,14 @@ #include #include -// template meta-programming headers -#include -namespace MPL = boost::mpl::placeholders; - -#include // operator typeid #include -class ExpressionFactorBinaryTest; -// Forward declare for testing - namespace gtsam { //----------------------------------------------------------------------------- // ExecutionTrace.h //----------------------------------------------------------------------------- -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); -} - -//----------------------------------------------------------------------------- - namespace internal { template @@ -195,585 +165,7 @@ public: }; //----------------------------------------------------------------------------- -/// Constant Expression -template -class ConstantExpression: public ExpressionNode { - - /// The constant value - T constant_; - - /// Constructor with a value, yielding a constant - ConstantExpression(const T& value) : - constant_(value) { - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - return constant_; - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - return constant_; - } -}; - -//----------------------------------------------------------------------------- -/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value -template -class LeafExpression: public ExpressionNode { - typedef T value_type; - - /// The key into values - Key key_; - - /// Constructor with a single key - LeafExpression(Key key) : - key_(key) { - } - // todo: do we need a virtual destructor here too? - - friend class Expression ; - -public: - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys; - keys.insert(key_); - return keys; - } - - /// Return dimensions for each argument - virtual void dims(std::map& map) const { - map[key_] = traits::dimension; - } - - /// Return value - virtual T value(const Values& values) const { - return values.at(key_); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - trace.setLeaf(key_); - return values.at(key_); - } - -}; - -//----------------------------------------------------------------------------- -// Below we use the "Class Composition" technique described in the book -// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost -// and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. -// to recursively generate a class, that will be the base for function nodes. -// -// The class generated, for three arguments A1, A2, and A3 will be -// -// struct Base1 : Argument, FunctionalBase { -// ... storage related to A1 ... -// ... methods that work on A1 ... -// }; -// -// struct Base2 : Argument, Base1 { -// ... storage related to A2 ... -// ... methods that work on A2 and (recursively) on A1 ... -// }; -// -// struct Base3 : Argument, Base2 { -// ... storage related to A3 ... -// ... methods that work on A3 and (recursively) on A2 and A1 ... -// }; -// -// struct FunctionalNode : Base3 { -// Provides convenience access to storage in hierarchy by using -// static_cast &>(*this) -// } -// -// All this magic happens when we generate the Base3 base class of FunctionalNode -// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode -// -// Similarly, the inner Record struct will be -// -// struct Record1 : JacobianTrace, CallRecord::value> { -// ... storage related to A1 ... -// ... methods that work on A1 ... -// }; -// -// struct Record2 : JacobianTrace, Record1 { -// ... storage related to A2 ... -// ... methods that work on A2 and (recursively) on A1 ... -// }; -// -// struct Record3 : JacobianTrace, Record2 { -// ... storage related to A3 ... -// ... methods that work on A3 and (recursively) on A2 and A1 ... -// }; -// -// struct Record : Record3 { -// Provides convenience access to storage in hierarchy by using -// static_cast &>(*this) -// } -// - -//----------------------------------------------------------------------------- - -/// meta-function to generate fixed-size JacobianTA type -template -struct Jacobian { - typedef Eigen::Matrix::dimension, traits::dimension> type; -}; - -/** - * Base case for recursive FunctionalNode class - */ -template -struct FunctionalBase: ExpressionNode { - static size_t const N = 0; // number of arguments - - struct Record { - void print(const std::string& indent) const { - } - void startReverseAD4(JacobianMap& jacobians) const { - } - template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { - } - }; - /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, - ExecutionTraceStorage*& traceStorage) const { - // base case: does not do anything - } -}; - -/** - * Building block for recursive FunctionalNode class - * The integer argument N is to guarantee a unique type signature, - * so we are guaranteed to be able to extract their values by static cast. - */ -template -struct Argument { - /// Expression that will generate value/derivatives for argument - boost::shared_ptr > expression; -}; - -/** - * Building block for Recursive Record Class - * Records the evaluation of a single argument in a functional expression - */ -template -struct JacobianTrace { - A value; - ExecutionTrace trace; - typename Jacobian::type dTdA; -}; - -// Recursive Definition of Functional ExpressionNode -// The reason we inherit from Argument is because we can then -// case to this unique signature to retrieve the expression at any level -template -struct GenerateFunctionalNode: Argument, Base { - - static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy - typedef Argument This; ///< The storage we have direct access to - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys = Base::keys(); - std::set myKeys = This::expression->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::expression->dims(map); - } - - // Recursive Record Class for Functional Expressions - // 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: JacobianTrace, Base::Record { - - typedef T return_type; - typedef JacobianTrace This; - - /// Print to std::cout - void print(const std::string& indent) const { - Base::Record::print(indent); - static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << This::dTdA.format(matlab) << std::endl; - This::trace.print(indent); - } - - /// Start the reverse AD process - void startReverseAD4(JacobianMap& jacobians) const { - Base::Record::startReverseAD4(jacobians); - // This is the crucial point where the size of the AD pipeline is selected. - // One pipeline is started for each argument, but the number of rows in each - // pipeline is the same, namely the dimension of the output argument T. - // For example, if the entire expression is rooted by a binary function - // yielding a 2D result, then the matrix dTdA will have 2 rows. - // 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); - } - - /// 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 { - Base::Record::reverseAD4(dFdT, jacobians); - This::trace.reverseAD1(dFdT * This::dTdA, jacobians); - } - }; - - /// Construct an execution trace for reverse AD - 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 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, traceStorage); - // traceStorage is never modified by traceExecution, but if traceExecution has - // written in the buffer, the next caller expects we advance the pointer - traceStorage += This::expression->traceSize(); - } -}; - -/** - * Recursive GenerateFunctionalNode class Generator - */ -template -struct FunctionalNode { - - /// The following typedef generates the recursively defined Base class - typedef typename boost::mpl::fold, - GenerateFunctionalNode >::type Base; - - /** - * The type generated by this meta-function derives from Base - * and adds access functions as well as the crucial [trace] function - */ - struct type: public Base { - - // Argument types and derived, note these are base 0 ! - // These are currently not used - useful for Phoenix in future -#ifdef EXPRESSIONS_PHOENIX - typedef TYPES Arguments; - typedef typename boost::mpl::transform >::type Jacobians; - typedef typename boost::mpl::transform >::type Optionals; -#endif - - /// Reset expression shared pointer - template - void reset(const boost::shared_ptr >& ptr) { - static_cast &>(*this).expression = ptr; - } - - /// Access Expression shared pointer - template - boost::shared_ptr > expression() const { - return static_cast const &>(*this).expression; - } - - /// Provide convenience access to Record storage and implement - /// the virtual function based interface of CallRecord using the CallRecordImplementor - struct Record: public internal::CallRecordImplementor::dimension>, public Base::Record { - using Base::Record::print; - using Base::Record::startReverseAD4; - using Base::Record::reverseAD4; - - virtual ~Record() { - } - - /// Access Value - template - const A& value() const { - return static_cast const &>(*this).value; - } - - /// Access Jacobian - template - typename Jacobian::type& jacobian() { - return static_cast&>(*this).dTdA; - } - }; - - /// 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 - Base::trace(values, record, traceStorage); - - // Return the record for this function evaluation - return record; - } - }; -}; -//----------------------------------------------------------------------------- - -/// Unary Function Expression -template -class UnaryExpression: public ExpressionNode { - - typedef typename Expression::template UnaryFunction::type Function; - Function function_; - boost::shared_ptr > expression1_; - - typedef Argument This; ///< The storage we have direct access to - - /// Constructor with a unary function f, and input argument e - 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); - } - - /// 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 - struct Record: public internal::CallRecordImplementor::dimension>, JacobianTrace { - - 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; - } - - /// 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 << "}" << std::endl; - } - - /// Start the reverse AD process - void startReverseAD4(JacobianMap& jacobians) const { - // This is the crucial point where the size of the AD pipeline is selected. - // One pipeline is started for each argument, but the number of rows in each - // pipeline is the same, namely the dimension of the output argument T. - // For example, if the entire expression is rooted by a binary function - // yielding a 2D result, then the matrix dTdA will have 2 rows. - // 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); - } - - /// 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); - } - }; - - /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, - ExecutionTraceStorage*& traceStorage) const { - // 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 - // 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); - trace.setFunction(record); - - return function_(record->template value(), - record->template jacobian()); - } -}; - -//----------------------------------------------------------------------------- -/// Binary Expression - -template -class BinaryExpression: public FunctionalNode >::type { - typedef typename FunctionalNode >::type Base; - -public: - typedef typename Base::Record Record; - -private: - - typedef typename Expression::template BinaryFunction::type Function; - Function function_; - - /// Constructor with a ternary function f, and three input arguments - BinaryExpression(Function f, const Expression& e1, - const Expression& e2) : - function_(f) { - this->template reset(e1.root()); - this->template reset(e2.root()); - ExpressionNode::traceSize_ = // - upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); - } - - friend class Expression ; - friend class ::ExpressionFactorBinaryTest; - -public: - - /// Return value - virtual T value(const Values& values) const { - using boost::none; - return function_(this->template expression()->value(values), - this->template expression()->value(values), - none, none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - 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()); - } -}; - -//----------------------------------------------------------------------------- -/// Ternary Expression - -template -class TernaryExpression: public FunctionalNode >::type { - - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: - - typedef typename Expression::template TernaryFunction::type Function; - Function function_; - - /// Constructor with a ternary function f, and three input arguments - TernaryExpression(Function f, const Expression& e1, - const Expression& e2, const Expression& e3) : - function_(f) { - this->template reset(e1.root()); - this->template reset(e2.root()); - this->template reset(e3.root()); - ExpressionNode::traceSize_ = // - upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() - + e3.traceSize(); - } - - friend class Expression ; - -public: - - /// Return value - virtual T value(const Values& values) const { - using boost::none; - return function_(this->template expression()->value(values), - this->template expression()->value(values), - this->template expression()->value(values), - none, none, none); - } - - /// Construct an execution trace for reverse AD - virtual T traceExecution(const Values& values, ExecutionTrace& trace, - 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()); - } - -}; - -//----------------------------------------------------------------------------- -// Esxpression-inl.h +// Expression-inl.h //----------------------------------------------------------------------------- /// Print diff --git a/gtsam/nonlinear/ExpressionNode.h b/gtsam/nonlinear/ExpressionNode.h index 465cab013..259d3ab5d 100644 --- a/gtsam/nonlinear/ExpressionNode.h +++ b/gtsam/nonlinear/ExpressionNode.h @@ -19,12 +19,42 @@ #pragma once +#include +#include + +// template meta-programming headers +#include +namespace MPL = boost::mpl::placeholders; + +#include // operator typeid #include #include +class ExpressionFactorBinaryTest; +// Forward declare for testing namespace gtsam { +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); +} + +//----------------------------------------------------------------------------- + /** * Expression node. The superclass for objects that do the heavy lifting * An Expression has a pointer to an ExpressionNode underneath @@ -84,4 +114,582 @@ public: ExecutionTraceStorage* traceStorage) const = 0; }; -} +//----------------------------------------------------------------------------- +/// Constant Expression +template +class ConstantExpression: public ExpressionNode { + + /// The constant value + T constant_; + + /// Constructor with a value, yielding a constant + ConstantExpression(const T& value) : + constant_(value) { + } + + friend class Expression ; + +public: + + /// Return value + virtual T value(const Values& values) const { + return constant_; + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const { + return constant_; + } +}; + +//----------------------------------------------------------------------------- +/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value +template +class LeafExpression: public ExpressionNode { + typedef T value_type; + + /// The key into values + Key key_; + + /// Constructor with a single key + LeafExpression(Key key) : + key_(key) { + } + // todo: do we need a virtual destructor here too? + + friend class Expression ; + +public: + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys; + keys.insert(key_); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + map[key_] = traits::dimension; + } + + /// Return value + virtual T value(const Values& values) const { + return values.at(key_); + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + ExecutionTraceStorage* traceStorage) const { + trace.setLeaf(key_); + return values.at(key_); + } + +}; + +//----------------------------------------------------------------------------- +// Below we use the "Class Composition" technique described in the book +// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost +// and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. +// to recursively generate a class, that will be the base for function nodes. +// +// The class generated, for three arguments A1, A2, and A3 will be +// +// struct Base1 : Argument, FunctionalBase { +// ... storage related to A1 ... +// ... methods that work on A1 ... +// }; +// +// struct Base2 : Argument, Base1 { +// ... storage related to A2 ... +// ... methods that work on A2 and (recursively) on A1 ... +// }; +// +// struct Base3 : Argument, Base2 { +// ... storage related to A3 ... +// ... methods that work on A3 and (recursively) on A2 and A1 ... +// }; +// +// struct FunctionalNode : Base3 { +// Provides convenience access to storage in hierarchy by using +// static_cast &>(*this) +// } +// +// All this magic happens when we generate the Base3 base class of FunctionalNode +// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode +// +// Similarly, the inner Record struct will be +// +// struct Record1 : JacobianTrace, CallRecord::value> { +// ... storage related to A1 ... +// ... methods that work on A1 ... +// }; +// +// struct Record2 : JacobianTrace, Record1 { +// ... storage related to A2 ... +// ... methods that work on A2 and (recursively) on A1 ... +// }; +// +// struct Record3 : JacobianTrace, Record2 { +// ... storage related to A3 ... +// ... methods that work on A3 and (recursively) on A2 and A1 ... +// }; +// +// struct Record : Record3 { +// Provides convenience access to storage in hierarchy by using +// static_cast &>(*this) +// } +// + +//----------------------------------------------------------------------------- + +/// meta-function to generate fixed-size JacobianTA type +template +struct Jacobian { + typedef Eigen::Matrix::dimension, traits::dimension> type; +}; + +/** + * Base case for recursive FunctionalNode class + */ +template +struct FunctionalBase: ExpressionNode { + static size_t const N = 0; // number of arguments + + struct Record { + void print(const std::string& indent) const { + } + void startReverseAD4(JacobianMap& jacobians) const { + } + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + } + }; + /// Construct an execution trace for reverse AD + void trace(const Values& values, Record* record, + ExecutionTraceStorage*& traceStorage) const { + // base case: does not do anything + } +}; + +/** + * Building block for recursive FunctionalNode class + * The integer argument N is to guarantee a unique type signature, + * so we are guaranteed to be able to extract their values by static cast. + */ +template +struct Argument { + /// Expression that will generate value/derivatives for argument + boost::shared_ptr > expression; +}; + +/** + * Building block for Recursive Record Class + * Records the evaluation of a single argument in a functional expression + */ +template +struct JacobianTrace { + A value; + ExecutionTrace trace; + typename Jacobian::type dTdA; +}; + +// Recursive Definition of Functional ExpressionNode +// The reason we inherit from Argument is because we can then +// case to this unique signature to retrieve the expression at any level +template +struct GenerateFunctionalNode: Argument, Base { + + static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy + typedef Argument This; ///< The storage we have direct access to + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys = Base::keys(); + std::set myKeys = This::expression->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::expression->dims(map); + } + + // Recursive Record Class for Functional Expressions + // 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: JacobianTrace, Base::Record { + + typedef T return_type; + typedef JacobianTrace This; + + /// Print to std::cout + void print(const std::string& indent) const { + Base::Record::print(indent); + static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); + std::cout << This::dTdA.format(matlab) << std::endl; + This::trace.print(indent); + } + + /// Start the reverse AD process + void startReverseAD4(JacobianMap& jacobians) const { + Base::Record::startReverseAD4(jacobians); + // This is the crucial point where the size of the AD pipeline is selected. + // One pipeline is started for each argument, but the number of rows in each + // pipeline is the same, namely the dimension of the output argument T. + // For example, if the entire expression is rooted by a binary function + // yielding a 2D result, then the matrix dTdA will have 2 rows. + // 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); + } + + /// 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 { + Base::Record::reverseAD4(dFdT, jacobians); + This::trace.reverseAD1(dFdT * This::dTdA, jacobians); + } + }; + + /// Construct an execution trace for reverse AD + 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 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, traceStorage); + // traceStorage is never modified by traceExecution, but if traceExecution has + // written in the buffer, the next caller expects we advance the pointer + traceStorage += This::expression->traceSize(); + } +}; + +/** + * Recursive GenerateFunctionalNode class Generator + */ +template +struct FunctionalNode { + + /// The following typedef generates the recursively defined Base class + typedef typename boost::mpl::fold, + GenerateFunctionalNode >::type Base; + + /** + * The type generated by this meta-function derives from Base + * and adds access functions as well as the crucial [trace] function + */ + struct type: public Base { + + // Argument types and derived, note these are base 0 ! + // These are currently not used - useful for Phoenix in future +#ifdef EXPRESSIONS_PHOENIX + typedef TYPES Arguments; + typedef typename boost::mpl::transform >::type Jacobians; + typedef typename boost::mpl::transform >::type Optionals; +#endif + + /// Reset expression shared pointer + template + void reset(const boost::shared_ptr >& ptr) { + static_cast &>(*this).expression = ptr; + } + + /// Access Expression shared pointer + template + boost::shared_ptr > expression() const { + return static_cast const &>(*this).expression; + } + + /// Provide convenience access to Record storage and implement + /// the virtual function based interface of CallRecord using the CallRecordImplementor + struct Record: public internal::CallRecordImplementor::dimension>, public Base::Record { + using Base::Record::print; + using Base::Record::startReverseAD4; + using Base::Record::reverseAD4; + + virtual ~Record() { + } + + /// Access Value + template + const A& value() const { + return static_cast const &>(*this).value; + } + + /// Access Jacobian + template + typename Jacobian::type& jacobian() { + return static_cast&>(*this).dTdA; + } + }; + + /// 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 + Base::trace(values, record, traceStorage); + + // Return the record for this function evaluation + return record; + } + }; +}; +//----------------------------------------------------------------------------- + +/// Unary Function Expression +template +class UnaryExpression: public ExpressionNode { + + typedef typename Expression::template UnaryFunction::type Function; + Function function_; + boost::shared_ptr > expression1_; + + typedef Argument This; ///< The storage we have direct access to + + /// Constructor with a unary function f, and input argument e + 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); + } + + /// 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 + struct Record: public internal::CallRecordImplementor::dimension>, JacobianTrace { + + 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; + } + + /// 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 << "}" << std::endl; + } + + /// Start the reverse AD process + void startReverseAD4(JacobianMap& jacobians) const { + // This is the crucial point where the size of the AD pipeline is selected. + // One pipeline is started for each argument, but the number of rows in each + // pipeline is the same, namely the dimension of the output argument T. + // For example, if the entire expression is rooted by a binary function + // yielding a 2D result, then the matrix dTdA will have 2 rows. + // 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); + } + + /// 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); + } + }; + + /// Construct an execution trace for reverse AD + void trace(const Values& values, Record* record, + ExecutionTraceStorage*& traceStorage) const { + // 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 + // 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); + trace.setFunction(record); + + return function_(record->template value(), + record->template jacobian()); + } +}; + +//----------------------------------------------------------------------------- +/// Binary Expression + +template +class BinaryExpression: public FunctionalNode >::type { + typedef typename FunctionalNode >::type Base; + +public: + typedef typename Base::Record Record; + +private: + + typedef typename Expression::template BinaryFunction::type Function; + Function function_; + + /// Constructor with a ternary function f, and three input arguments + BinaryExpression(Function f, const Expression& e1, + const Expression& e2) : + function_(f) { + this->template reset(e1.root()); + this->template reset(e2.root()); + ExpressionNode::traceSize_ = // + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); + } + + friend class Expression ; + friend class ::ExpressionFactorBinaryTest; + +public: + + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return function_(this->template expression()->value(values), + this->template expression()->value(values), + none, none); + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + 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()); + } +}; + +//----------------------------------------------------------------------------- +/// Ternary Expression + +template +class TernaryExpression: public FunctionalNode >::type { + + typedef typename FunctionalNode >::type Base; + typedef typename Base::Record Record; + +private: + + typedef typename Expression::template TernaryFunction::type Function; + Function function_; + + /// Constructor with a ternary function f, and three input arguments + TernaryExpression(Function f, const Expression& e1, + const Expression& e2, const Expression& e3) : + function_(f) { + this->template reset(e1.root()); + this->template reset(e2.root()); + this->template reset(e3.root()); + ExpressionNode::traceSize_ = // + upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() + + e3.traceSize(); + } + + friend class Expression ; + +public: + + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return function_(this->template expression()->value(values), + this->template expression()->value(values), + this->template expression()->value(values), + none, none, none); + } + + /// Construct an execution trace for reverse AD + virtual T traceExecution(const Values& values, ExecutionTrace& trace, + 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()); + } + +}; + +} // namespace gtsam From a753f071f48f597868761396a7cee3f3723ab3d5 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 19:04:45 -0700 Subject: [PATCH 13/35] Try separating out ExecutionTrace --- gtsam/nonlinear/ExecutionTrace.h | 166 +++++++++++++++++++ gtsam/nonlinear/Expression-inl.h | 142 +--------------- gtsam/nonlinear/tests/testExecutionTrace.cpp | 38 +++++ 3 files changed, 206 insertions(+), 140 deletions(-) create mode 100644 gtsam/nonlinear/ExecutionTrace.h create mode 100644 gtsam/nonlinear/tests/testExecutionTrace.cpp diff --git a/gtsam/nonlinear/ExecutionTrace.h b/gtsam/nonlinear/ExecutionTrace.h new file mode 100644 index 000000000..c0f1657be --- /dev/null +++ b/gtsam/nonlinear/ExecutionTrace.h @@ -0,0 +1,166 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ExecutionTrace.h + * @date September 18, 2014 + * @author Frank Dellaert + * @brief Used in Expression.h, not for general consumption + */ + +#pragma once + +#include +#include +//#include +//#include +//#include +// +//#include +//#include +//#include +//#include +// +//#include + +namespace gtsam { + +template struct CallRecord; + +namespace internal { + +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + // block makes HUGE difference + jacobians(key).block( + 0, 0) += dTdA; + } +}; + +/// Handle Leaf Case for Dynamic Matrix type (slower) +template +struct UseBlockIf { + static void addToJacobian(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + jacobians(key) += dTdA; + } +}; +} + +/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians +template +void handleLeafCase(const Eigen::MatrixBase& dTdA, + JacobianMap& jacobians, Key key) { + internal::UseBlockIf< + Derived::RowsAtCompileTime != Eigen::Dynamic + && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( + dTdA, jacobians, key); +} + +/** + * The ExecutionTrace class records a tree-structured expression's execution. + * + * The class looks a bit complicated but it is so for performance. + * It is a tagged union that obviates the need to create + * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead + * the key for the leaf is stored in the space normally used to store a + * CallRecord*. Nothing is stored for a Constant. + * + * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be: + * Trace(Function) -> + * BinaryRecord with two traces in it + * trace1(Function) -> + * UnaryRecord with one trace in it + * trace1(Function) -> + * BinaryRecord with two traces in it + * trace1(Leaf) + * trace2(Constant) + * trace2(Leaf) + * Hence, there are three Record structs, written to memory by traceExecution + */ +template +class ExecutionTrace { + static const int Dim = traits::dimension; + enum { + Constant, Leaf, Function + } kind; + union { + Key key; + CallRecord* ptr; + } content; +public: + /// Pointer always starts out as a Constant + ExecutionTrace() : + kind(Constant) { + } + /// Change pointer to a Leaf Record + void setLeaf(Key key) { + kind = Leaf; + content.key = key; + } + /// Take ownership of pointer to a Function Record + void setFunction(CallRecord* record) { + kind = Function; + content.ptr = record; + } + /// Print + void print(const std::string& indent = "") const { + if (kind == Constant) + std::cout << indent << "Constant" << std::endl; + else if (kind == Leaf) + std::cout << indent << "Leaf, key = " << content.key << std::endl; + else if (kind == Function) { + std::cout << indent << "Function" << std::endl; + content.ptr->print(indent + " "); + } + } + /// Return record pointer, quite unsafe, used only for testing + template + boost::optional record() { + if (kind != Function) + return boost::none; + else { + Record* p = dynamic_cast(content.ptr); + return p ? boost::optional(p) : boost::none; + } + } + /** + * *** This is the main entry point for reverse AD, called from Expression *** + * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) + */ + typedef Eigen::Matrix JacobianTT; + void startReverseAD1(JacobianMap& jacobians) const { + if (kind == Leaf) { + // This branch will only be called on trivial Leaf expressions, i.e. Priors + static const JacobianTT I = JacobianTT::Identity(); + handleLeafCase(I, jacobians, content.key); + } else if (kind == Function) + // This is the more typical entry point, starting the AD pipeline + // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. + content.ptr->startReverseAD2(jacobians); + } + // Either add to Jacobians (Leaf) or propagate (Function) + template + void reverseAD1(const Eigen::MatrixBase & dTdA, + JacobianMap& jacobians) const { + if (kind == Leaf) + handleLeafCase(dTdA, jacobians, content.key); + else if (kind == Function) + content.ptr->reverseAD2(dTdA, jacobians); + } + + /// Define type so we can apply it as a meta-function + typedef ExecutionTrace type; +}; + +} // namespace gtsam diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 102b2ff8f..7db3fd191 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -19,6 +19,7 @@ #pragma once +#include #include #include @@ -31,143 +32,6 @@ namespace gtsam { -//----------------------------------------------------------------------------- -// ExecutionTrace.h -//----------------------------------------------------------------------------- - -namespace internal { - -template -struct UseBlockIf { - static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { - // block makes HUGE difference - jacobians(key).block( - 0, 0) += dTdA; - } - ; -}; -/// Handle Leaf Case for Dynamic Matrix type (slower) -template -struct UseBlockIf { - static void addToJacobian(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { - jacobians(key) += dTdA; - } -}; -} - -/// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians -template -void handleLeafCase(const Eigen::MatrixBase& dTdA, - JacobianMap& jacobians, Key key) { - internal::UseBlockIf< - Derived::RowsAtCompileTime != Eigen::Dynamic - && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( - dTdA, jacobians, key); -} - -//----------------------------------------------------------------------------- -/** - * The ExecutionTrace class records a tree-structured expression's execution. - * - * The class looks a bit complicated but it is so for performance. - * It is a tagged union that obviates the need to create - * a ExecutionTrace subclass for Constants and Leaf Expressions. Instead - * the key for the leaf is stored in the space normally used to store a - * CallRecord*. Nothing is stored for a Constant. - * - * A full execution trace of a Binary(Unary(Binary(Leaf,Constant)),Leaf) would be: - * Trace(Function) -> - * BinaryRecord with two traces in it - * trace1(Function) -> - * UnaryRecord with one trace in it - * trace1(Function) -> - * BinaryRecord with two traces in it - * trace1(Leaf) - * trace2(Constant) - * trace2(Leaf) - * Hence, there are three Record structs, written to memory by traceExecution - */ -template -class ExecutionTrace { - static const int Dim = traits::dimension; - enum { - Constant, Leaf, Function - } kind; - union { - Key key; - CallRecord* ptr; - } content; -public: - /// Pointer always starts out as a Constant - ExecutionTrace() : - kind(Constant) { - } - /// Change pointer to a Leaf Record - void setLeaf(Key key) { - kind = Leaf; - content.key = key; - } - /// Take ownership of pointer to a Function Record - void setFunction(CallRecord* record) { - kind = Function; - content.ptr = record; - } - /// Print - void print(const std::string& indent = "") const { - if (kind == Constant) - std::cout << indent << "Constant" << std::endl; - else if (kind == Leaf) - std::cout << indent << "Leaf, key = " << content.key << std::endl; - else if (kind == Function) { - std::cout << indent << "Function" << std::endl; - content.ptr->print(indent + " "); - } - } - /// Return record pointer, quite unsafe, used only for testing - template - boost::optional record() { - if (kind != Function) - return boost::none; - else { - Record* p = dynamic_cast(content.ptr); - return p ? boost::optional(p) : boost::none; - } - } - /** - * *** This is the main entry point for reverse AD, called from Expression *** - * Called only once, either inserts I into Jacobians (Leaf) or starts AD (Function) - */ - typedef Eigen::Matrix JacobianTT; - void startReverseAD1(JacobianMap& jacobians) const { - if (kind == Leaf) { - // This branch will only be called on trivial Leaf expressions, i.e. Priors - static const JacobianTT I = JacobianTT::Identity(); - handleLeafCase(I, jacobians, content.key); - } else if (kind == Function) - // This is the more typical entry point, starting the AD pipeline - // Inside startReverseAD2 the correctly dimensioned pipeline is chosen. - content.ptr->startReverseAD2(jacobians); - } - // Either add to Jacobians (Leaf) or propagate (Function) - template - void reverseAD1(const Eigen::MatrixBase & dTdA, - JacobianMap& jacobians) const { - if (kind == Leaf) - handleLeafCase(dTdA, jacobians, content.key); - else if (kind == Function) - content.ptr->reverseAD2(dTdA, jacobians); - } - - /// Define type so we can apply it as a meta-function - typedef ExecutionTrace type; -}; - -//----------------------------------------------------------------------------- -// Expression-inl.h -//----------------------------------------------------------------------------- - /// Print template void Expression::print(const std::string& s) const { @@ -374,6 +238,4 @@ VerticalBlockMatrix::Block JacobianMap::operator()(Key key) { return Ab_(block); } -//----------------------------------------------------------------------------- - -} +} // namespace gtsam diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp new file mode 100644 index 000000000..b1b0038bd --- /dev/null +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -0,0 +1,38 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------1------------------------------------------- */ + +/** + * @file testExecutionTrace.cpp + * @date May 11, 2015 + * @author Frank Dellaert + * @brief unit tests for Expression internals + */ + +#include + +#include + +using namespace std; +using namespace gtsam; + +/* ************************************************************************* */ +// Constant +TEST(ExecutionTrace, construct) { + ExecutionTrace trace; +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + From 0e764fadb3509420118d60e59abf6ccd0d40a5b9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 20:25:03 -0700 Subject: [PATCH 14/35] new target --- .cproject | 42 ++++++++++++++++++++++++++++++------------ 1 file changed, 30 insertions(+), 12 deletions(-) diff --git a/.cproject b/.cproject index 9726fec60..b6b1aec87 100644 --- a/.cproject +++ b/.cproject @@ -532,14 +532,6 @@ true true - - make - -j4 - testSimilarity3.run - true - true - true - make -j2 @@ -755,6 +747,14 @@ true true + + make + -j4 + testSimilarity3.run + true + true + true + make -j5 @@ -1301,7 +1301,6 @@ make - testSimulated2DOriented.run true false @@ -1341,7 +1340,6 @@ make - testSimulated2D.run true false @@ -1349,7 +1347,6 @@ make - testSimulated3D.run true false @@ -1453,6 +1450,7 @@ make + testErrors.run true false @@ -1763,6 +1761,7 @@ cpack + -G DEB true false @@ -1770,6 +1769,7 @@ cpack + -G RPM true false @@ -1777,6 +1777,7 @@ cpack + -G TGZ true false @@ -1784,6 +1785,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -1975,7 +1977,6 @@ make - tests/testGaussianISAM2 true false @@ -2127,6 +2128,7 @@ make + tests/testBayesTree.run true false @@ -2134,6 +2136,7 @@ make + testBinaryBayesNet.run true false @@ -2181,6 +2184,7 @@ make + testSymbolicBayesNet.run true false @@ -2188,6 +2192,7 @@ make + tests/testSymbolicFactor.run true false @@ -2195,6 +2200,7 @@ make + testSymbolicFactorGraph.run true false @@ -2210,6 +2216,7 @@ make + tests/testBayesTree true false @@ -2783,6 +2790,14 @@ true true + + make + -j4 + testExecutionTrace.run + true + true + true + make -j5 @@ -3329,6 +3344,7 @@ make + testGraph.run true false @@ -3336,6 +3352,7 @@ make + testJunctionTree.run true false @@ -3343,6 +3360,7 @@ make + testSymbolicBayesNetB.run true false From 8d8f270b608874fc20a44219ee62a0deb06ada47 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 20:33:15 -0700 Subject: [PATCH 15/35] JacobianMap, header discipline --- gtsam/nonlinear/ExecutionTrace.h | 27 +++++----- gtsam/nonlinear/Expression-inl.h | 40 +++++++-------- gtsam/nonlinear/Expression.h | 48 ++---------------- gtsam/nonlinear/ExpressionNode.h | 1 + gtsam/nonlinear/JacobianMap.h | 52 ++++++++++++++++++++ gtsam/nonlinear/tests/testExecutionTrace.cpp | 3 +- 6 files changed, 91 insertions(+), 80 deletions(-) create mode 100644 gtsam/nonlinear/JacobianMap.h diff --git a/gtsam/nonlinear/ExecutionTrace.h b/gtsam/nonlinear/ExecutionTrace.h index c0f1657be..292ad7719 100644 --- a/gtsam/nonlinear/ExecutionTrace.h +++ b/gtsam/nonlinear/ExecutionTrace.h @@ -11,30 +11,31 @@ /** * @file ExecutionTrace.h - * @date September 18, 2014 + * @date May 11, 2015 * @author Frank Dellaert - * @brief Used in Expression.h, not for general consumption + * @brief Execution trace for expressions */ #pragma once -#include +#include #include -//#include -//#include -//#include -// -//#include -//#include -//#include -//#include -// -//#include +#include + +#include + +#include namespace gtsam { template struct CallRecord; +/// 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. +static const unsigned TraceAlignment = 16; +typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; + namespace internal { template diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 7db3fd191..9c69cf0f7 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -19,16 +19,11 @@ #pragma once -#include #include -#include -#include #include -#include -#include - -#include +#include +#include namespace gtsam { @@ -116,8 +111,7 @@ Expression::Expression(const Expression& expression1, /// Construct a ternary function expression template template -Expression::Expression( - typename TernaryFunction::type function, +Expression::Expression(typename TernaryFunction::type function, const Expression& expression1, const Expression& expression2, const Expression& expression3) : root_( @@ -125,7 +119,6 @@ Expression::Expression( expression3)) { } - /// Return root template const boost::shared_ptr >& Expression::root() const { @@ -156,7 +149,8 @@ void Expression::dims(std::map& map) const { * The order of the Jacobians is same as keys in either keys() or dims() */ template -T Expression::value(const Values& values, boost::optional&> H) const { +T Expression::value(const Values& values, + boost::optional&> H) const { if (H) { // Call private version that returns derivatives in H @@ -193,8 +187,9 @@ T Expression::value(const Values& values, const FastVector& keys, template T Expression::traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - return root_->traceExecution(values, trace, traceStorage); + void* traceStorage) const { + return root_->traceExecution(values, trace, + static_cast(traceStorage)); } template @@ -226,16 +221,15 @@ T Expression::value(const Values& values, JacobianMap& jacobians) const { 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); +template +typename Expression::KeysAndDims Expression::keysAndDims() const { + std::map map; + dims(map); + size_t n = map.size(); + KeysAndDims pair = std::make_pair(FastVector < Key > (n), FastVector(n)); + boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); + boost::copy(map | boost::adaptors::map_values, pair.second.begin()); + return pair; } } // namespace gtsam diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index df575dbbf..1ea9e7f49 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -19,15 +19,12 @@ #pragma once +#include #include #include -#include -#include #include -#include -#include -#include +#include class ExpressionFactorShallowTest; @@ -39,16 +36,6 @@ 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; - -/// 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 */ @@ -176,15 +163,7 @@ private: /// Vaguely unsafe keys and dimensions in same order typedef std::pair, FastVector > KeysAndDims; - KeysAndDims keysAndDims() const { - std::map map; - dims(map); - size_t n = map.size(); - KeysAndDims pair = std::make_pair(FastVector(n), FastVector(n)); - boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); - boost::copy(map | boost::adaptors::map_values, pair.second.begin()); - return pair; - } + KeysAndDims keysAndDims() const; /// private version that takes keys and dimensions, returns derivatives T value(const Values& values, const FastVector& keys, @@ -192,7 +171,7 @@ private: /// trace execution, very unsafe T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const; + void* traceStorage) const; /** * @brief Return value and derivatives, reverse AD version @@ -204,23 +183,6 @@ private: // be very selective on who can access these private methods: friend class ExpressionFactor ; friend class ::ExpressionFactorShallowTest; - -}; - -// 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 @@ -252,7 +214,7 @@ std::vector > createUnknowns(size_t n, char c, size_t start = 0) { return unknowns; } -} +} // namespace gtsam #include diff --git a/gtsam/nonlinear/ExpressionNode.h b/gtsam/nonlinear/ExpressionNode.h index 259d3ab5d..26585ce4d 100644 --- a/gtsam/nonlinear/ExpressionNode.h +++ b/gtsam/nonlinear/ExpressionNode.h @@ -19,6 +19,7 @@ #pragma once +#include #include #include diff --git a/gtsam/nonlinear/JacobianMap.h b/gtsam/nonlinear/JacobianMap.h new file mode 100644 index 000000000..43e22fc16 --- /dev/null +++ b/gtsam/nonlinear/JacobianMap.h @@ -0,0 +1,52 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file JacobianMap.h + * @date May 11, 2015 + * @author Frank Dellaert + * @brief JacobianMap for returning derivatives from expressions + */ + +#pragma once + +#include +#include +#include + +namespace gtsam { + +// A JacobianMap is the primary mechanism by which derivatives are returned. +// 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: + typedef FastVector Keys; + const FastVector& keys_; + VerticalBlockMatrix& Ab_; + +public: + /// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab + JacobianMap(const Keys& keys, VerticalBlockMatrix& Ab) : + keys_(keys), Ab_(Ab) { + } + + /// Access blocks of via key + VerticalBlockMatrix::Block operator()(Key key) { + Keys::const_iterator it = std::find(keys_.begin(), keys_.end(), key); + DenseIndex block = it - keys_.begin(); + return Ab_(block); + } +}; + +} // namespace gtsam + diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp index b1b0038bd..cb4dfd2e7 100644 --- a/gtsam/nonlinear/tests/testExecutionTrace.cpp +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -17,6 +17,7 @@ */ #include +#include #include @@ -26,7 +27,7 @@ using namespace gtsam; /* ************************************************************************* */ // Constant TEST(ExecutionTrace, construct) { - ExecutionTrace trace; + ExecutionTrace trace; } /* ************************************************************************* */ From 134730eeeea0d2bbb5e0a990ca90effa81a431e3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 20:53:41 -0700 Subject: [PATCH 16/35] Moved utility functions to inl.h --- gtsam/nonlinear/Expression-inl.h | 29 +++++++++++++++++++++++++ gtsam/nonlinear/Expression.h | 37 +++++++++++--------------------- 2 files changed, 41 insertions(+), 25 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 9c69cf0f7..15ef269f2 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -232,4 +232,33 @@ typename Expression::KeysAndDims Expression::keysAndDims() const { return pair; } +// http://stackoverflow.com/questions/16260445/boost-bind-to-operator +template +struct apply_compose { + typedef T result_type; + static const int Dim = traits::dimension; + T operator()(const T& x, const T& y, OptionalJacobian H1 = + boost::none, OptionalJacobian H2 = boost::none) const { + return x.compose(y, H1, H2); + } +}; + +/// Construct a product expression, assumes T::compose(T) -> T +template +Expression operator*(const Expression& expression1, + const Expression& expression2) { + return Expression(boost::bind(apply_compose(), _1, _2, _3, _4), + expression1, expression2); +} + +/// Construct an array of leaves +template +std::vector > createUnknowns(size_t n, char c, size_t start) { + std::vector > unknowns; + unknowns.reserve(n); + for (size_t i = start; i < start + n; i++) + unknowns.push_back(Expression(c, i)); + return unknowns; +} + } // namespace gtsam diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 1ea9e7f49..f81a17c96 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -185,34 +185,21 @@ private: friend class ::ExpressionFactorShallowTest; }; -// http://stackoverflow.com/questions/16260445/boost-bind-to-operator -template -struct apply_compose { - typedef T result_type; - static const int Dim = traits::dimension; - T operator()(const T& x, const T& y, OptionalJacobian H1 = - boost::none, OptionalJacobian H2 = boost::none) const { - return x.compose(y, H1, H2); - } -}; - -/// Construct a product expression, assumes T::compose(T) -> T +/** + * Construct a product expression, assumes T::compose(T) -> T + * Example: + * Expression a(0), b(1), c = a*b; + */ template -Expression operator*(const Expression& expression1, - const Expression& expression2) { - return Expression(boost::bind(apply_compose(), _1, _2, _3, _4), - expression1, expression2); -} +Expression operator*(const Expression& e1, const Expression& e2); -/// Construct an array of leaves +/** + * Construct an array of unknown expressions with successive symbol keys + * Example: + * createUnknowns(3,'x') creates unknown expressions for x0,x1,x2 + */ template -std::vector > createUnknowns(size_t n, char c, size_t start = 0) { - std::vector > unknowns; - unknowns.reserve(n); - for (size_t i = start; i < start + n; i++) - unknowns.push_back(Expression(c, i)); - return unknowns; -} +std::vector > createUnknowns(size_t n, char c, size_t start = 0); } // namespace gtsam From 81b38609913e51bc171183ba59325e507e0ff0ad Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 21:16:57 -0700 Subject: [PATCH 17/35] Moved all internal data structures to internal namespace --- .cproject | 8 ---- gtsam/nonlinear/CallRecord.h | 19 ++------ gtsam/nonlinear/ExecutionTrace.h | 7 ++- gtsam/nonlinear/Expression-inl.h | 44 ++++++++++--------- gtsam/nonlinear/Expression.h | 11 +++-- gtsam/nonlinear/ExpressionNode.h | 17 ++++--- gtsam/nonlinear/expressionTesting.h | 4 +- gtsam/nonlinear/tests/testCallRecord.cpp | 2 +- gtsam/nonlinear/tests/testExecutionTrace.cpp | 2 +- gtsam/nonlinear/tests/testExpression.cpp | 8 ++-- .../nonlinear/tests/testExpressionFactor.cpp | 22 +++++----- .../nonlinear/tests/testExpressionMeta.cpp | 1 + 12 files changed, 67 insertions(+), 78 deletions(-) diff --git a/.cproject b/.cproject index b6b1aec87..5f9d5d349 100644 --- a/.cproject +++ b/.cproject @@ -2046,14 +2046,6 @@ true true - - make - -j4 - testCustomChartExpression.run - true - true - true - make -j5 diff --git a/gtsam/nonlinear/CallRecord.h b/gtsam/nonlinear/CallRecord.h index 159a841e5..88ba0fb2d 100644 --- a/gtsam/nonlinear/CallRecord.h +++ b/gtsam/nonlinear/CallRecord.h @@ -20,18 +20,10 @@ #pragma once -#include -#include -#include - +#include #include namespace gtsam { - -class JacobianMap; -// forward declaration - -//----------------------------------------------------------------------------- namespace internal { /** @@ -64,8 +56,6 @@ struct ConvertToVirtualFunctionSupportedMatrixType { } }; -} // namespace internal - /** * The CallRecord is an abstract base class for the any class that stores * the Jacobians of applying a function with respect to each of its arguments, @@ -94,9 +84,8 @@ struct CallRecord { inline void reverseAD2(const Eigen::MatrixBase & dFdT, JacobianMap& jacobians) const { _reverseAD3( - internal::ConvertToVirtualFunctionSupportedMatrixType< - (Derived::RowsAtCompileTime > 5)>::convert(dFdT), - jacobians); + ConvertToVirtualFunctionSupportedMatrixType< + (Derived::RowsAtCompileTime > 5)>::convert(dFdT), jacobians); } // This overload supports matrices with both rows and columns dynamically sized. @@ -140,7 +129,6 @@ private: */ const int CallRecordMaxVirtualStaticRows = 5; -namespace internal { /** * The CallRecordImplementor implements the CallRecord interface for a Derived class by * delegating to its corresponding (templated) non-virtual methods. @@ -193,5 +181,4 @@ private: }; } // namespace internal - } // gtsam diff --git a/gtsam/nonlinear/ExecutionTrace.h b/gtsam/nonlinear/ExecutionTrace.h index 292ad7719..ca7d78d81 100644 --- a/gtsam/nonlinear/ExecutionTrace.h +++ b/gtsam/nonlinear/ExecutionTrace.h @@ -27,6 +27,7 @@ #include namespace gtsam { +namespace internal { template struct CallRecord; @@ -36,8 +37,6 @@ template struct CallRecord; static const unsigned TraceAlignment = 16; typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage; -namespace internal { - template struct UseBlockIf { static void addToJacobian(const Eigen::MatrixBase& dTdA, @@ -56,13 +55,12 @@ struct UseBlockIf { jacobians(key) += dTdA; } }; -} /// Handle Leaf Case: reverse AD ends here, by writing a matrix into Jacobians template void handleLeafCase(const Eigen::MatrixBase& dTdA, JacobianMap& jacobians, Key key) { - internal::UseBlockIf< + UseBlockIf< Derived::RowsAtCompileTime != Eigen::Dynamic && Derived::ColsAtCompileTime != Eigen::Dynamic, Derived>::addToJacobian( dTdA, jacobians, key); @@ -164,4 +162,5 @@ public: typedef ExecutionTrace type; }; +} // namespace internal } // namespace gtsam diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 15ef269f2..3ab0bfe4d 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -36,25 +36,25 @@ void Expression::print(const std::string& s) const { // Construct a constant expression template Expression::Expression(const T& value) : - root_(new ConstantExpression(value)) { + root_(new internal::ConstantExpression(value)) { } // Construct a leaf expression, with Key template Expression::Expression(const Key& key) : - root_(new LeafExpression(key)) { + root_(new internal::LeafExpression(key)) { } // Construct a leaf expression, with Symbol template Expression::Expression(const Symbol& symbol) : - root_(new LeafExpression(symbol)) { + root_(new internal::LeafExpression(symbol)) { } // Construct a leaf expression, creating Symbol template Expression::Expression(unsigned char c, size_t j) : - root_(new LeafExpression(Symbol(c, j))) { + root_(new internal::LeafExpression(Symbol(c, j))) { } /// Construct a nullary method expression @@ -62,7 +62,9 @@ template template Expression::Expression(const Expression& expression, T (A::*method)(typename MakeOptionalJacobian::type) const) : - root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { + root_( + new internal::UnaryExpression(boost::bind(method, _1, _2), + expression)) { } /// Construct a unary function expression @@ -70,7 +72,7 @@ template template Expression::Expression(typename UnaryFunction::type function, const Expression& expression) : - root_(new UnaryExpression(function, expression)) { + root_(new internal::UnaryExpression(function, expression)) { } /// Construct a unary method expression @@ -81,8 +83,8 @@ Expression::Expression(const Expression& expression1, typename MakeOptionalJacobian::type) const, const Expression& expression2) : root_( - new BinaryExpression(boost::bind(method, _1, _2, _3, _4), - expression1, expression2)) { + new internal::BinaryExpression( + boost::bind(method, _1, _2, _3, _4), expression1, expression2)) { } /// Construct a binary function expression @@ -90,7 +92,9 @@ template template Expression::Expression(typename BinaryFunction::type function, const Expression& expression1, const Expression& expression2) : - root_(new BinaryExpression(function, expression1, expression2)) { + root_( + new internal::BinaryExpression(function, expression1, + expression2)) { } /// Construct a binary method expression @@ -103,7 +107,7 @@ Expression::Expression(const Expression& expression1, typename MakeOptionalJacobian::type) const, const Expression& expression2, const Expression& expression3) : root_( - new TernaryExpression( + new internal::TernaryExpression( boost::bind(method, _1, _2, _3, _4, _5, _6), expression1, expression2, expression3)) { } @@ -115,13 +119,13 @@ Expression::Expression(typename TernaryFunction::type function, const Expression& expression1, const Expression& expression2, const Expression& expression3) : root_( - new TernaryExpression(function, expression1, expression2, - expression3)) { + new internal::TernaryExpression(function, expression1, + expression2, expression3)) { } /// Return root template -const boost::shared_ptr >& Expression::root() const { +const boost::shared_ptr >& Expression::root() const { return root_; } @@ -186,10 +190,10 @@ T Expression::value(const Values& values, const FastVector& keys, } template -T Expression::traceExecution(const Values& values, ExecutionTrace& trace, - void* traceStorage) const { +T Expression::traceExecution(const Values& values, + internal::ExecutionTrace& trace, void* traceStorage) const { return root_->traceExecution(values, trace, - static_cast(traceStorage)); + static_cast(traceStorage)); } template @@ -205,12 +209,12 @@ T Expression::value(const Values& values, JacobianMap& jacobians) const { // 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]; + internal::ExecutionTraceStorage* traceStorage = new internal::ExecutionTraceStorage[size]; #else - ExecutionTraceStorage traceStorage[size]; + internal::ExecutionTraceStorage traceStorage[size]; #endif - ExecutionTrace trace; + internal::ExecutionTrace trace; T value(this->traceExecution(values, trace, traceStorage)); trace.startReverseAD1(jacobians); @@ -226,7 +230,7 @@ typename Expression::KeysAndDims Expression::keysAndDims() const { std::map map; dims(map); size_t n = map.size(); - KeysAndDims pair = std::make_pair(FastVector < Key > (n), FastVector(n)); + KeysAndDims pair = std::make_pair(FastVector(n), FastVector(n)); boost::copy(map | boost::adaptors::map_keys, pair.first.begin()); boost::copy(map | boost::adaptors::map_values, pair.second.begin()); return pair; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index f81a17c96..a7a3326c3 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -32,9 +32,12 @@ namespace gtsam { // Forward declares class Values; +template class ExpressionFactor; + +namespace internal { template class ExecutionTrace; template class ExpressionNode; -template class ExpressionFactor; +} /** * Expression class that supports automatic differentiation @@ -50,7 +53,7 @@ public: private: // Paul's trick shared pointer, polymorphic root of entire expression tree - boost::shared_ptr > root_; + boost::shared_ptr > root_; public: @@ -131,7 +134,7 @@ public: const Expression& expression3); /// Return root - const boost::shared_ptr >& root() const; + const boost::shared_ptr >& root() const; // Return size needed for memory buffer in traceExecution size_t traceSize() const; @@ -170,7 +173,7 @@ private: const FastVector& dims, std::vector& H) const; /// trace execution, very unsafe - T traceExecution(const Values& values, ExecutionTrace& trace, + T traceExecution(const Values& values, internal::ExecutionTrace& trace, void* traceStorage) const; /** diff --git a/gtsam/nonlinear/ExpressionNode.h b/gtsam/nonlinear/ExpressionNode.h index 26585ce4d..1cbfd488c 100644 --- a/gtsam/nonlinear/ExpressionNode.h +++ b/gtsam/nonlinear/ExpressionNode.h @@ -35,6 +35,7 @@ class ExpressionFactorBinaryTest; // Forward declare for testing namespace gtsam { +namespace internal { template T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) { @@ -412,8 +413,8 @@ struct FunctionalNode { /// Provide convenience access to Record storage and implement /// the virtual function based interface of CallRecord using the CallRecordImplementor - struct Record: public internal::CallRecordImplementor::dimension>, public Base::Record { + struct Record: public CallRecordImplementor::dimension>, + public Base::Record { using Base::Record::print; using Base::Record::startReverseAD4; using Base::Record::reverseAD4; @@ -497,8 +498,8 @@ public: // 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 internal::CallRecordImplementor::dimension>, JacobianTrace { + struct Record: public CallRecordImplementor::dimension>, + JacobianTrace { typedef T return_type; typedef JacobianTrace This; @@ -600,7 +601,7 @@ public: private: - typedef typename Expression::template BinaryFunction::type Function; + typedef typename Expression::template BinaryFunction::type Function; Function function_; /// Constructor with a ternary function f, and three input arguments @@ -650,7 +651,7 @@ class TernaryExpression: public FunctionalNode private: - typedef typename Expression::template TernaryFunction::type Function; + typedef typename Expression::template TernaryFunction::type Function; Function function_; /// Constructor with a ternary function f, and three input arguments @@ -693,4 +694,6 @@ public: }; -} // namespace gtsam +} + // namespace internal +}// namespace gtsam diff --git a/gtsam/nonlinear/expressionTesting.h b/gtsam/nonlinear/expressionTesting.h index ab6703f3a..92fff9e04 100644 --- a/gtsam/nonlinear/expressionTesting.h +++ b/gtsam/nonlinear/expressionTesting.h @@ -88,7 +88,7 @@ void testFactorJacobians(TestResult& result_, const std::string& name_, // Check cast result and then equality CHECK(actual); - EXPECT( assert_equal(expected, *actual, tolerance)); + EXPECT(assert_equal(expected, *actual, tolerance)); } } @@ -112,7 +112,7 @@ void testExpressionJacobians(TestResult& result_, const std::string& name_, expression.value(values), expression); testFactorJacobians(result_, name_, f, values, nd_step, tolerance); } -} +} // namespace internal } // namespace gtsam /// \brief Check the Jacobians produced by an expression against finite differences. diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 483b5ffa9..376ef56e4 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -32,7 +32,7 @@ static const int Cols = 3; int dynamicIfAboveMax(int i){ - if(i > CallRecordMaxVirtualStaticRows){ + if(i > internal::CallRecordMaxVirtualStaticRows){ return Eigen::Dynamic; } else return i; diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp index cb4dfd2e7..5d0f33966 100644 --- a/gtsam/nonlinear/tests/testExecutionTrace.cpp +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -27,7 +27,7 @@ using namespace gtsam; /* ************************************************************************* */ // Constant TEST(ExecutionTrace, construct) { - ExecutionTrace trace; + internal::ExecutionTrace trace; } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 84f180609..c47079db3 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -165,7 +165,7 @@ TEST(Expression, BinaryDimensions) { /* ************************************************************************* */ // dimensions TEST(Expression, BinaryTraceSize) { - typedef BinaryExpression Binary; + typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Binary::Record); EXPECT_LONGS_EQUAL(expectedTraceSize, binary::p_cam.traceSize()); } @@ -196,9 +196,9 @@ TEST(Expression, TreeDimensions) { /* ************************************************************************* */ // TraceSize TEST(Expression, TreeTraceSize) { - typedef UnaryExpression Unary; - typedef BinaryExpression Binary1; - typedef BinaryExpression Binary2; + typedef internal::UnaryExpression Unary; + typedef internal::BinaryExpression Binary1; + typedef internal::BinaryExpression Binary2; size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary1::Record) + sizeof(Binary2::Record); EXPECT_LONGS_EQUAL(expectedTraceSize, tree::uv_hat.traceSize()); diff --git a/gtsam/nonlinear/tests/testExpressionFactor.cpp b/gtsam/nonlinear/tests/testExpressionFactor.cpp index 900293261..ead6e0176 100644 --- a/gtsam/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam/nonlinear/tests/testExpressionFactor.cpp @@ -169,7 +169,7 @@ static Point2 myUncal(const Cal3_S2& K, const Point2& p, // Binary(Leaf,Leaf) TEST(ExpressionFactor, Binary) { - typedef BinaryExpression Binary; + typedef internal::BinaryExpression Binary; Cal3_S2_ K_(1); Point2_ p_(2); @@ -184,10 +184,10 @@ TEST(ExpressionFactor, Binary) { EXPECT_LONGS_EQUAL(8, sizeof(double)); EXPECT_LONGS_EQUAL(16, sizeof(Point2)); EXPECT_LONGS_EQUAL(40, sizeof(Cal3_S2)); - EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian::type)); - EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian::type)); + EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); + EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); + EXPECT_LONGS_EQUAL(2*5*8, sizeof(internal::Jacobian::type)); + EXPECT_LONGS_EQUAL(2*2*8, sizeof(internal::Jacobian::type)); size_t expectedRecordSize = 16 + 16 + 40 + 2 * 16 + 80 + 32; EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record)); @@ -197,8 +197,8 @@ 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 - ExecutionTraceStorage traceStorage[size]; - ExecutionTrace trace; + internal::ExecutionTraceStorage traceStorage[size]; + internal::ExecutionTrace trace; Point2 value = binary.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); @@ -250,8 +250,8 @@ TEST(ExpressionFactor, Shallow) { LONGS_EQUAL(3,dims[1]); // traceExecution of shallow tree - typedef UnaryExpression Unary; - typedef BinaryExpression Binary; + typedef internal::UnaryExpression Unary; + typedef internal::BinaryExpression Binary; size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary::Record); EXPECT_LONGS_EQUAL(112, sizeof(Unary::Record)); #ifdef GTSAM_USE_QUATERNIONS @@ -264,8 +264,8 @@ TEST(ExpressionFactor, Shallow) { size_t size = expression.traceSize(); CHECK(size); EXPECT_LONGS_EQUAL(expectedTraceSize, size); - ExecutionTraceStorage traceStorage[size]; - ExecutionTrace trace; + internal::ExecutionTraceStorage traceStorage[size]; + internal::ExecutionTrace trace; Point2 value = expression.traceExecution(values, trace, traceStorage); EXPECT(assert_equal(Point2(),value, 1e-9)); // trace.print(); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp index 7c2f9d9b9..211d07329 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp @@ -26,6 +26,7 @@ using namespace std; using namespace gtsam; +using namespace gtsam::internal; /* ************************************************************************* */ namespace mpl = boost::mpl; From 0090e07df941d4224f1d8a1fdb2027de8303bdf2 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 21:26:14 -0700 Subject: [PATCH 18/35] Moved to internal subdirectory --- gtsam/nonlinear/Expression-inl.h | 2 +- gtsam/nonlinear/{ => internal}/CallRecord.h | 0 gtsam/nonlinear/{ => internal}/ExecutionTrace.h | 0 gtsam/nonlinear/{ => internal}/ExpressionNode.h | 4 ++-- gtsam/nonlinear/tests/testCallRecord.cpp | 2 +- gtsam/nonlinear/tests/testExecutionTrace.cpp | 2 +- 6 files changed, 5 insertions(+), 5 deletions(-) rename gtsam/nonlinear/{ => internal}/CallRecord.h (100%) rename gtsam/nonlinear/{ => internal}/ExecutionTrace.h (100%) rename gtsam/nonlinear/{ => internal}/ExpressionNode.h (99%) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 3ab0bfe4d..f9f81c598 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -19,7 +19,7 @@ #pragma once -#include +#include #include #include diff --git a/gtsam/nonlinear/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h similarity index 100% rename from gtsam/nonlinear/CallRecord.h rename to gtsam/nonlinear/internal/CallRecord.h diff --git a/gtsam/nonlinear/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h similarity index 100% rename from gtsam/nonlinear/ExecutionTrace.h rename to gtsam/nonlinear/internal/ExecutionTrace.h diff --git a/gtsam/nonlinear/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h similarity index 99% rename from gtsam/nonlinear/ExpressionNode.h rename to gtsam/nonlinear/internal/ExpressionNode.h index 1cbfd488c..40b071b00 100644 --- a/gtsam/nonlinear/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -19,8 +19,8 @@ #pragma once -#include -#include +#include +#include #include // template meta-programming headers diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 376ef56e4..09099ba1b 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -18,7 +18,7 @@ * @brief unit tests for CallRecord class */ -#include +#include #include #include diff --git a/gtsam/nonlinear/tests/testExecutionTrace.cpp b/gtsam/nonlinear/tests/testExecutionTrace.cpp index 5d0f33966..731f69816 100644 --- a/gtsam/nonlinear/tests/testExecutionTrace.cpp +++ b/gtsam/nonlinear/tests/testExecutionTrace.cpp @@ -16,7 +16,7 @@ * @brief unit tests for Expression internals */ -#include +#include #include #include From 686c920d9f903ed0e46b55f97e7548ddd31a0e38 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 22:11:26 -0700 Subject: [PATCH 19/35] Removed unsafe test --- gtsam/nonlinear/tests/testExpression.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index c47079db3..5d443e073 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -42,7 +42,7 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3); /* ************************************************************************* */ // Constant -TEST(Expression, constant) { +TEST(Expression, Constant) { Expression R(someR); Values values; Rot3 actual = R.value(values); @@ -89,13 +89,11 @@ set expected = list_of(1); TEST(Expression, Unary1) { using namespace unary; Expression e(f1, p); - EXPECT_LONGS_EQUAL(112,e.traceSize()); EXPECT(expected == e.keys()); } TEST(Expression, Unary2) { using namespace unary; Expression e(f2, p); - EXPECT_LONGS_EQUAL(80,e.traceSize()); EXPECT(expected == e.keys()); } /* ************************************************************************* */ From b213e6419a2137a90d7207aa9aabcb6091c73ed9 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 22:12:00 -0700 Subject: [PATCH 20/35] Re-ordered for clarity --- gtsam/nonlinear/Expression-inl.h | 104 ++++++++++++++----------------- gtsam/nonlinear/Expression.h | 58 ++++++++--------- 2 files changed, 77 insertions(+), 85 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index f9f81c598..b3ead11c4 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -27,36 +27,60 @@ namespace gtsam { -/// 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 internal::ConstantExpression(value)) { } -// Construct a leaf expression, with Key template Expression::Expression(const Key& key) : root_(new internal::LeafExpression(key)) { } -// Construct a leaf expression, with Symbol template Expression::Expression(const Symbol& symbol) : root_(new internal::LeafExpression(symbol)) { } -// Construct a leaf expression, creating Symbol template Expression::Expression(unsigned char c, size_t j) : root_(new internal::LeafExpression(Symbol(c, j))) { } +/// Construct a unary function expression +template +template +Expression::Expression(typename UnaryFunction::type function, + const Expression& expression) : + root_(new internal::UnaryExpression(function, expression)) { +} + +/// Construct a binary function expression +template +template +Expression::Expression(typename BinaryFunction::type function, + const Expression& expression1, const Expression& expression2) : + root_( + new internal::BinaryExpression(function, expression1, + expression2)) { +} + +/// Construct a ternary function expression +template +template +Expression::Expression(typename TernaryFunction::type function, + const Expression& expression1, const Expression& expression2, + const Expression& expression3) : + root_( + new internal::TernaryExpression(function, expression1, + expression2, expression3)) { +} + /// Construct a nullary method expression template template @@ -67,14 +91,6 @@ Expression::Expression(const Expression& expression, expression)) { } -/// Construct a unary function expression -template -template -Expression::Expression(typename UnaryFunction::type function, - const Expression& expression) : - root_(new internal::UnaryExpression(function, expression)) { -} - /// Construct a unary method expression template template @@ -87,16 +103,6 @@ Expression::Expression(const Expression& expression1, 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 internal::BinaryExpression(function, expression1, - expression2)) { -} - /// Construct a binary method expression template template @@ -112,46 +118,16 @@ Expression::Expression(const Expression& 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 internal::TernaryExpression(function, expression1, - expression2, expression3)) { -} - -/// Return root -template -const boost::shared_ptr >& Expression::root() const { - return root_; -} - -// Return size needed for memory buffer in traceExecution -template -size_t Expression::traceSize() const { - return root_->traceSize(); -} - -/// Return keys that play in this expression template std::set Expression::keys() const { return root_->keys(); } -/// Return dimensions for each argument, as a map template void Expression::dims(std::map& map) const { root_->dims(map); } -/** - * @brief Return value and optional derivatives, reverse AD version - * Notes: this is not terribly efficient, and H should have correct size. - * The order of the Jacobians is same as keys in either keys() or dims() - */ template T Expression::value(const Values& values, boost::optional&> H) const { @@ -165,7 +141,18 @@ T Expression::value(const Values& values, return root_->value(values); } -/// private version that takes keys and dimensions, returns derivatives +template +const boost::shared_ptr >& Expression::root() const { + return root_; +} + +template +size_t Expression::traceSize() const { + return root_->traceSize(); +} + +// Private methods: + template T Expression::value(const Values& values, const FastVector& keys, const FastVector& dims, std::vector& H) const { @@ -236,6 +223,7 @@ typename Expression::KeysAndDims Expression::keysAndDims() const { return pair; } +namespace internal { // http://stackoverflow.com/questions/16260445/boost-bind-to-operator template struct apply_compose { @@ -246,13 +234,17 @@ struct apply_compose { return x.compose(y, H1, H2); } }; +} + +// Global methods: /// Construct a product expression, assumes T::compose(T) -> T template Expression operator*(const Expression& expression1, const Expression& expression2) { - return Expression(boost::bind(apply_compose(), _1, _2, _3, _4), - expression1, expression2); + return Expression( + boost::bind(internal::apply_compose(), _1, _2, _3, _4), expression1, + expression2); } /// Construct an array of leaves diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index a7a3326c3..2c503dce9 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -26,6 +26,7 @@ #include #include +// Forward declare tests class ExpressionFactorShallowTest; namespace gtsam { @@ -96,16 +97,27 @@ public: /// 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); - /// Construct a unary function expression template Expression(typename UnaryFunction::type function, const Expression& expression); + /// Construct a binary function expression + template + Expression(typename BinaryFunction::type function, + const Expression& expression1, const Expression& expression2); + + /// Construct a ternary function expression + template + Expression(typename TernaryFunction::type function, + const Expression& expression1, const Expression& expression2, + const Expression& expression3); + + /// Construct a nullary method expression + template + Expression(const Expression& expression, + T (A::*method)(typename MakeOptionalJacobian::type) const); + /// Construct a unary method expression template Expression(const Expression& expression1, @@ -113,11 +125,6 @@ public: typename MakeOptionalJacobian::type) const, const Expression& expression2); - /// Construct a binary function expression - template - Expression(typename BinaryFunction::type function, - const Expression& expression1, const Expression& expression2); - /// Construct a binary method expression template Expression(const Expression& expression1, @@ -127,18 +134,6 @@ public: typename MakeOptionalJacobian::type) const, const Expression& expression2, const Expression& expression3); - /// Construct a ternary function expression - template - Expression(typename TernaryFunction::type function, - const Expression& expression1, const Expression& expression2, - const Expression& expression3); - - /// Return root - const boost::shared_ptr >& root() const; - - // Return size needed for memory buffer in traceExecution - size_t traceSize() const; - /// Return keys that play in this expression std::set keys() const; @@ -162,9 +157,15 @@ public: return boost::make_shared(*this); } + /// Return root + const boost::shared_ptr >& root() const; + + /// Return size needed for memory buffer in traceExecution + size_t traceSize() const; + private: - /// Vaguely unsafe keys and dimensions in same order + /// Keys and dimensions in same order typedef std::pair, FastVector > KeysAndDims; KeysAndDims keysAndDims() const; @@ -176,15 +177,14 @@ private: T traceExecution(const Values& values, internal::ExecutionTrace& trace, void* 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. - */ + /// brief Return value and derivatives, reverse AD version T value(const Values& values, JacobianMap& jacobians) const; // be very selective on who can access these private methods: - friend class ExpressionFactor ; + friend class ExpressionFactor; + friend class internal::ExpressionNode; + + // and add tests friend class ::ExpressionFactorShallowTest; }; From 69c31d20e1a72f4ad72f00b3e724a4c7219d5f19 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 22:21:52 -0700 Subject: [PATCH 21/35] Made JacobianMap internal --- gtsam/nonlinear/Expression-inl.h | 4 ++-- gtsam/nonlinear/Expression.h | 4 ++-- gtsam/nonlinear/ExpressionFactor.h | 2 +- gtsam/nonlinear/internal/CallRecord.h | 2 +- gtsam/nonlinear/internal/ExecutionTrace.h | 2 +- gtsam/nonlinear/{ => internal}/JacobianMap.h | 2 ++ gtsam/nonlinear/tests/testCallRecord.cpp | 6 +++--- 7 files changed, 12 insertions(+), 10 deletions(-) rename gtsam/nonlinear/{ => internal}/JacobianMap.h (97%) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index b3ead11c4..06a25ac23 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -164,7 +164,7 @@ T Expression::value(const Values& values, const FastVector& keys, static const int Dim = traits::dimension; VerticalBlockMatrix Ab(dims, Dim); Ab.matrix().setZero(); - JacobianMap jacobianMap(keys, Ab); + internal::JacobianMap jacobianMap(keys, Ab); // Call unsafe version T result = value(values, jacobianMap); @@ -184,7 +184,7 @@ T Expression::traceExecution(const Values& values, } template -T Expression::value(const Values& values, JacobianMap& jacobians) const { +T Expression::value(const Values& values, internal::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 diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 2c503dce9..faa884562 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -19,7 +19,7 @@ #pragma once -#include +#include #include #include @@ -178,7 +178,7 @@ private: void* traceStorage) const; /// brief Return value and derivatives, reverse AD version - T value(const Values& values, JacobianMap& jacobians) const; + T value(const Values& values, internal::JacobianMap& jacobians) const; // be very selective on who can access these private methods: friend class ExpressionFactor; diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index 4e0467a3b..afb487ff4 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -89,7 +89,7 @@ public: // Wrap keys and VerticalBlockMatrix into structure passed to expression_ VerticalBlockMatrix& Ab = factor->matrixObject(); - JacobianMap jacobianMap(keys_, Ab); + internal::JacobianMap jacobianMap(keys_, Ab); // Zero out Jacobian so we can simply add to it Ab.matrix().setZero(); diff --git a/gtsam/nonlinear/internal/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h index 88ba0fb2d..50d08cb8e 100644 --- a/gtsam/nonlinear/internal/CallRecord.h +++ b/gtsam/nonlinear/internal/CallRecord.h @@ -20,7 +20,7 @@ #pragma once -#include +#include #include namespace gtsam { diff --git a/gtsam/nonlinear/internal/ExecutionTrace.h b/gtsam/nonlinear/internal/ExecutionTrace.h index ca7d78d81..37ce4dfd5 100644 --- a/gtsam/nonlinear/internal/ExecutionTrace.h +++ b/gtsam/nonlinear/internal/ExecutionTrace.h @@ -18,7 +18,7 @@ #pragma once -#include +#include #include #include diff --git a/gtsam/nonlinear/JacobianMap.h b/gtsam/nonlinear/internal/JacobianMap.h similarity index 97% rename from gtsam/nonlinear/JacobianMap.h rename to gtsam/nonlinear/internal/JacobianMap.h index 43e22fc16..f4d2e9565 100644 --- a/gtsam/nonlinear/JacobianMap.h +++ b/gtsam/nonlinear/internal/JacobianMap.h @@ -23,6 +23,7 @@ #include namespace gtsam { +namespace internal { // A JacobianMap is the primary mechanism by which derivatives are returned. // Expressions are designed to write their derivatives into an already allocated @@ -48,5 +49,6 @@ public: } }; +} // namespace internal } // namespace gtsam diff --git a/gtsam/nonlinear/tests/testCallRecord.cpp b/gtsam/nonlinear/tests/testCallRecord.cpp index 09099ba1b..208f0b284 100644 --- a/gtsam/nonlinear/tests/testCallRecord.cpp +++ b/gtsam/nonlinear/tests/testCallRecord.cpp @@ -80,13 +80,13 @@ struct Record: public internal::CallRecordImplementor { } void print(const std::string& indent) const { } - void startReverseAD4(JacobianMap& jacobians) const { + void startReverseAD4(internal::JacobianMap& jacobians) const { } mutable CallConfig cc; private: template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + void reverseAD4(const SomeMatrix & dFdT, internal::JacobianMap& jacobians) const { cc.compTimeRows = SomeMatrix::RowsAtCompileTime; cc.compTimeCols = SomeMatrix::ColsAtCompileTime; cc.runTimeRows = dFdT.rows(); @@ -97,7 +97,7 @@ struct Record: public internal::CallRecordImplementor { friend struct internal::CallRecordImplementor; }; -JacobianMap & NJM= *static_cast(NULL); +internal::JacobianMap & NJM= *static_cast(NULL); /* ************************************************************************* */ typedef Eigen::Matrix DynRowMat; From 8fecac46c037bb647691f788bbae1610b1208ebd Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 23:00:50 -0700 Subject: [PATCH 22/35] more descriptive names, get rid of value confusion --- gtsam/nonlinear/Expression-inl.h | 12 +++++++----- gtsam/nonlinear/Expression.h | 7 ++++--- gtsam/nonlinear/ExpressionFactor.h | 8 ++++---- 3 files changed, 15 insertions(+), 12 deletions(-) diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 06a25ac23..6c6c155c7 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -135,7 +135,7 @@ T Expression::value(const Values& values, if (H) { // Call private version that returns derivatives in H KeysAndDims pair = keysAndDims(); - return value(values, pair.first, pair.second, *H); + return valueAndDerivatives(values, pair.first, pair.second, *H); } else // no derivatives needed, just return value return root_->value(values); @@ -154,8 +154,9 @@ size_t Expression::traceSize() const { // Private methods: template -T Expression::value(const Values& values, const FastVector& keys, - const FastVector& dims, std::vector& H) const { +T Expression::valueAndDerivatives(const Values& values, + const FastVector& keys, const FastVector& dims, + std::vector& H) const { // H should be pre-allocated assert(H.size()==keys.size()); @@ -167,7 +168,7 @@ T Expression::value(const Values& values, const FastVector& keys, internal::JacobianMap jacobianMap(keys, Ab); // Call unsafe version - T result = value(values, jacobianMap); + T result = valueAndJacobianMap(values, jacobianMap); // Copy blocks into the vector of jacobians passed in for (DenseIndex i = 0; i < static_cast(keys.size()); i++) @@ -184,7 +185,8 @@ T Expression::traceExecution(const Values& values, } template -T Expression::value(const Values& values, internal::JacobianMap& jacobians) const { +T Expression::valueAndJacobianMap(const Values& values, + internal::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 diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index faa884562..3985d8a58 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -170,7 +170,7 @@ private: KeysAndDims keysAndDims() const; /// private version that takes keys and dimensions, returns derivatives - T value(const Values& values, const FastVector& keys, + T valueAndDerivatives(const Values& values, const FastVector& keys, const FastVector& dims, std::vector& H) const; /// trace execution, very unsafe @@ -178,10 +178,11 @@ private: void* traceStorage) const; /// brief Return value and derivatives, reverse AD version - T value(const Values& values, internal::JacobianMap& jacobians) const; + T valueAndJacobianMap(const Values& values, + internal::JacobianMap& jacobians) const; // be very selective on who can access these private methods: - friend class ExpressionFactor; + friend class ExpressionFactor ; friend class internal::ExpressionNode; // and add tests diff --git a/gtsam/nonlinear/ExpressionFactor.h b/gtsam/nonlinear/ExpressionFactor.h index afb487ff4..63e2b0ae8 100644 --- a/gtsam/nonlinear/ExpressionFactor.h +++ b/gtsam/nonlinear/ExpressionFactor.h @@ -65,8 +65,8 @@ public: */ virtual Vector unwhitenedError(const Values& x, boost::optional&> H = boost::none) const { - if (H) { - const T value = expression_.value(x, keys_, dims_, *H); + if (H) { + const T value = expression_.valueAndDerivatives(x, keys_, dims_, *H); return traits::Local(measurement_, value); } else { const T value = expression_.value(x); @@ -95,7 +95,7 @@ public: Ab.matrix().setZero(); // Get value and Jacobians, writing directly into JacobianFactor - T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here ! + T value = expression_.valueAndJacobianMap(x, jacobianMap); // <<< Reverse AD happens here ! // Evaluate error and set RHS vector b Ab(size()).col(0) = -traits::Local(measurement_, value); @@ -109,5 +109,5 @@ public: }; // ExpressionFactor -} // \ namespace gtsam +}// \ namespace gtsam From b8024b10b54686e66fbcf5c6bfc25396eb09e575 Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 11 May 2015 23:37:50 -0700 Subject: [PATCH 23/35] 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); From e1b3a119574d6aa081097d8c5afddaaff4f95558 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 12 May 2015 01:10:03 -0700 Subject: [PATCH 24/35] BinaryExpression now without MPL --- gtsam/nonlinear/internal/ExpressionNode.h | 110 ++++++++++++------ gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 4 +- gtsam/nonlinear/tests/testExpression.cpp | 34 ++++-- .../nonlinear/tests/testExpressionFactor.cpp | 11 +- 4 files changed, 108 insertions(+), 51 deletions(-) diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index d3c736e9d..e72445716 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -454,6 +454,9 @@ struct FunctionalNode { }; //----------------------------------------------------------------------------- +// Eigen format for printing Jacobians +static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); + /// Unary Function Expression template class UnaryExpression: public ExpressionNode { @@ -466,24 +469,24 @@ public: /// Constructor with a unary function f, and input argument e1 UnaryExpression(Function f, const Expression& e1) : - function_(f) { - this->expression1_ = e1.root(); + expression1_(e1.root()), function_(f) { ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); } /// Return value virtual T value(const Values& values) const { - return function_(this->expression1_->value(values), boost::none); + using boost::none; + return function_(expression1_->value(values), none); } /// Return keys that play in this expression virtual std::set keys() const { - return this->expression1_->keys(); + return expression1_->keys(); } /// Return dimensions for each argument virtual void dims(std::map& map) const { - this->expression1_->dims(map); + expression1_->dims(map); } // Inner Record Class @@ -496,8 +499,7 @@ public: /// 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 << dTdA1.format(matlab) << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; trace1.print(indent); std::cout << indent << "}" << std::endl; } @@ -516,7 +518,6 @@ 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 { trace1.reverseAD1(dFdT * dTdA1, jacobians); @@ -553,50 +554,93 @@ public: /// Binary Expression template -class BinaryExpression: public FunctionalNode >::type { - typedef typename FunctionalNode >::type Base; - -public: - typedef typename Base::Record Record; - -private: +class BinaryExpression: public ExpressionNode { typedef typename Expression::template BinaryFunction::type Function; + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; Function function_; - /// Constructor with a ternary function f, and three input arguments +public: + + /// Constructor with a binary function f, and two input arguments BinaryExpression(Function f, const Expression& e1, const Expression& e2) : - function_(f) { - this->template reset(e1.root()); - this->template reset(e2.root()); + expression1_(e1.root()), expression2_(e2.root()), function_(f) { ExpressionNode::traceSize_ = // upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); } - friend class Expression ; friend class ::ExpressionFactorBinaryTest; -public: - /// Return value virtual T value(const Values& values) const { using boost::none; - return function_(this->template expression()->value(values), - this->template expression()->value(values), - none, none); + return function_(expression1_->value(values), expression2_->value(values), + none, none); } - /// Construct an execution trace for reverse AD + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys = expression1_->keys(); + std::set myKeys = expression2_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + expression2_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + A2 value2; + ExecutionTrace trace2; + typename Jacobian::type dTdA2; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "BinaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; + trace2.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process, see comments in Base + void startReverseAD4(JacobianMap& jacobians) const { + trace1.reverseAD1(dTdA1, jacobians); + trace2.reverseAD1(dTdA2, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + trace2.reverseAD1(dFdT * dTdA2, jacobians); + } + }; + + /// Construct an execution trace for reverse AD, see UnaryExpression for explanation virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + ptr += expression1_->traceSize() + expression2_->traceSize(); trace.setFunction(record); - - return function_(record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian()); + return function_(record->value1, record->value2, record->dTdA1, + record->dTdA2); } }; diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index f3858c818..ac56b77ca 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -265,9 +265,9 @@ TEST(AdaptAutoDiff, Snavely) { typedef AdaptAutoDiff Adaptor; Expression expression(Adaptor(), P, X); #ifdef GTSAM_USE_QUATERNIONS - EXPECT_LONGS_EQUAL(400,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(384,expression.traceSize()); // Todo, should be zero #else - EXPECT_LONGS_EQUAL(432,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(416,expression.traceSize()); // Todo, should be zero #endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); diff --git a/gtsam/nonlinear/tests/testExpression.cpp b/gtsam/nonlinear/tests/testExpression.cpp index 5d443e073..3e86bcb8c 100644 --- a/gtsam/nonlinear/tests/testExpression.cpp +++ b/gtsam/nonlinear/tests/testExpression.cpp @@ -68,19 +68,19 @@ TEST(Expression, Leaves) { Point3 somePoint(1, 2, 3); values.insert(Symbol('p', 10), somePoint); std::vector > points = createUnknowns(10, 'p', 1); - EXPECT(assert_equal(somePoint,points.back().value(values))); + EXPECT(assert_equal(somePoint, points.back().value(values))); } /* ************************************************************************* */ // Unary(Leaf) namespace unary { -Point2 f1(const Point3& p, OptionalJacobian<2,3> H) { +Point2 f1(const Point3& p, OptionalJacobian<2, 3> H) { return Point2(); } double f2(const Point3& p, OptionalJacobian<1, 3> H) { return 0.0; } -Vector f3(const Point3& p, OptionalJacobian H) { +Vector f3(const Point3& p, OptionalJacobian H) { return p.vector(); } Expression p(1); @@ -127,7 +127,7 @@ TEST(Expression, NullaryMethod) { EXPECT(actual == sqrt(50)); Matrix expected(1, 3); expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0); - EXPECT(assert_equal(expected,H[0])); + EXPECT(assert_equal(expected, H[0])); } /* ************************************************************************* */ // Binary(Leaf,Leaf) @@ -158,7 +158,7 @@ TEST(Expression, BinaryKeys) { TEST(Expression, BinaryDimensions) { map actual, expected = map_list_of(1, 6)(2, 3); binary::p_cam.dims(actual); - EXPECT(actual==expected); + EXPECT(actual == expected); } /* ************************************************************************* */ // dimensions @@ -189,17 +189,26 @@ TEST(Expression, TreeKeys) { TEST(Expression, TreeDimensions) { map actual, expected = map_list_of(1, 6)(2, 3)(3, 5); tree::uv_hat.dims(actual); - EXPECT(actual==expected); + EXPECT(actual == expected); } /* ************************************************************************* */ // TraceSize TEST(Expression, TreeTraceSize) { - typedef internal::UnaryExpression Unary; typedef internal::BinaryExpression Binary1; - typedef internal::BinaryExpression Binary2; - size_t expectedTraceSize = sizeof(Unary::Record) + sizeof(Binary1::Record) - + sizeof(Binary2::Record); - EXPECT_LONGS_EQUAL(expectedTraceSize, tree::uv_hat.traceSize()); + EXPECT_LONGS_EQUAL(internal::upAligned(sizeof(Binary1::Record)), + tree::p_cam.traceSize()); + + typedef internal::UnaryExpression Unary; + EXPECT_LONGS_EQUAL( + internal::upAligned(sizeof(Unary::Record)) + tree::p_cam.traceSize(), + tree::projection.traceSize()); + + EXPECT_LONGS_EQUAL(0, tree::K.traceSize()); + + typedef internal::BinaryExpression Binary2; + EXPECT_LONGS_EQUAL( + internal::upAligned(sizeof(Binary2::Record)) + tree::K.traceSize() + + tree::projection.traceSize(), tree::uv_hat.traceSize()); } /* ************************************************************************* */ @@ -243,7 +252,8 @@ TEST(Expression, compose3) { /* ************************************************************************* */ // Test with ternary function Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, - OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, OptionalJacobian<3, 3> H3) { + OptionalJacobian<3, 3> H1, OptionalJacobian<3, 3> H2, + OptionalJacobian<3, 3> H3) { // return dummy derivatives (not correct, but that's ok for testing here) if (H1) *H1 = eye(3); diff --git a/gtsam/nonlinear/tests/testExpressionFactor.cpp b/gtsam/nonlinear/tests/testExpressionFactor.cpp index 3573378ed..3aee9e50a 100644 --- a/gtsam/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam/nonlinear/tests/testExpressionFactor.cpp @@ -188,7 +188,11 @@ TEST(ExpressionFactor, Binary) { EXPECT_LONGS_EQUAL(16, sizeof(internal::ExecutionTrace)); EXPECT_LONGS_EQUAL(2*5*8, sizeof(internal::Jacobian::type)); EXPECT_LONGS_EQUAL(2*2*8, sizeof(internal::Jacobian::type)); - size_t expectedRecordSize = 16 + 16 + 40 + 2 * 16 + 80 + 32; + size_t expectedRecordSize = sizeof(Cal3_S2) + + sizeof(internal::ExecutionTrace) + + +sizeof(internal::Jacobian::type) + sizeof(Point2) + + sizeof(internal::ExecutionTrace) + + sizeof(internal::Jacobian::type); EXPECT_LONGS_EQUAL(expectedRecordSize + 8, sizeof(Binary::Record)); // Check size @@ -212,9 +216,8 @@ TEST(ExpressionFactor, Binary) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT( - assert_equal(expected25, (Matrix) (*r)-> jacobian(), 1e-9)); - EXPECT( assert_equal(expected22, (Matrix) (*r)->jacobian(), 1e-9)); + EXPECT(assert_equal(expected25, (Matrix ) (*r)->dTdA1, 1e-9)); + EXPECT(assert_equal(expected22, (Matrix ) (*r)->dTdA2, 1e-9)); } /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) From 4a8dbd689af3c82c81b1f40152b4d7e0dc92010e Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 12 May 2015 01:25:34 -0700 Subject: [PATCH 25/35] TernaryExpression now without MPL --- gtsam/nonlinear/internal/ExpressionNode.h | 120 ++++++++++++++++------ 1 file changed, 86 insertions(+), 34 deletions(-) diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index e72445716..65b521d6c 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -648,56 +648,108 @@ public: /// Ternary Expression template -class TernaryExpression: public FunctionalNode >::type { - - typedef typename FunctionalNode >::type Base; - typedef typename Base::Record Record; - -private: +class TernaryExpression: public ExpressionNode { typedef typename Expression::template TernaryFunction::type Function; + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; + boost::shared_ptr > expression3_; Function function_; - /// Constructor with a ternary function f, and three input arguments +public: + + /// Constructor with a ternary function f, and two input arguments TernaryExpression(Function f, const Expression& e1, const Expression& e2, const Expression& e3) : + expression1_(e1.root()), expression2_(e2.root()), expression3_(e3.root()), // function_(f) { - this->template reset(e1.root()); - this->template reset(e2.root()); - this->template reset(e3.root()); - ExpressionNode::traceSize_ = // - upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize() - + e3.traceSize(); + ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + // + e1.traceSize() + e2.traceSize() + e3.traceSize(); } - friend class Expression ; - -public: - /// Return value virtual T value(const Values& values) const { using boost::none; - return function_(this->template expression()->value(values), - this->template expression()->value(values), - this->template expression()->value(values), - none, none, none); + return function_(expression1_->value(values), expression2_->value(values), + expression3_->value(values), none, none, none); } - /// Construct an execution trace for reverse AD + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys = expression1_->keys(); + std::set myKeys = expression2_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + myKeys = expression3_->keys(); + keys.insert(myKeys.begin(), myKeys.end()); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + expression1_->dims(map); + expression2_->dims(map); + expression3_->dims(map); + } + + // Inner Record Class + struct Record: public CallRecordImplementor::dimension> { + + A1 value1; + ExecutionTrace trace1; + typename Jacobian::type dTdA1; + + A2 value2; + ExecutionTrace trace2; + typename Jacobian::type dTdA2; + + A3 value3; + ExecutionTrace trace3; + typename Jacobian::type dTdA3; + + /// Print to std::cout + void print(const std::string& indent) const { + std::cout << indent << "TernaryExpression::Record {" << std::endl; + std::cout << indent << dTdA1.format(kMatlabFormat) << std::endl; + trace1.print(indent); + std::cout << indent << dTdA2.format(kMatlabFormat) << std::endl; + trace2.print(indent); + std::cout << indent << dTdA3.format(kMatlabFormat) << std::endl; + trace3.print(indent); + std::cout << indent << "}" << std::endl; + } + + /// Start the reverse AD process, see comments in Base + void startReverseAD4(JacobianMap& jacobians) const { + trace1.reverseAD1(dTdA1, jacobians); + trace2.reverseAD1(dTdA2, jacobians); + trace3.reverseAD1(dTdA3, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + template + void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { + trace1.reverseAD1(dFdT * dTdA1, jacobians); + trace2.reverseAD1(dFdT * dTdA2, jacobians); + trace3.reverseAD1(dFdT * dTdA3, jacobians); + } + }; + + /// Construct an execution trace for reverse AD, see UnaryExpression for explanation virtual T traceExecution(const Values& values, ExecutionTrace& trace, - ExecutionTraceStorage* traceStorage) const { - - Record* record = Base::trace(values, traceStorage); + ExecutionTraceStorage* ptr) const { + assert(reinterpret_cast(ptr) % TraceAlignment == 0); + Record* record = new (ptr) Record(); + ptr += upAligned(sizeof(Record)); + record->value1 = expression1_->traceExecution(values, record->trace1, ptr); + record->value2 = expression2_->traceExecution(values, record->trace2, ptr); + record->value3 = expression3_->traceExecution(values, record->trace3, ptr); + ptr += expression1_->traceSize() + expression2_->traceSize() + + expression3_->traceSize(); trace.setFunction(record); - - return function_( - record->template value(), record->template value(), - record->template value(), record->template jacobian(), - record->template jacobian(), record->template jacobian()); + return function_(record->value1, record->value2, record->value3, + record->dTdA1, record->dTdA2, record->dTdA3); } - }; -} - // namespace internal -}// namespace gtsam +} // namespace internal +} // namespace gtsam From 4f846ff75f43d168972d65bc964161f39d7c3f12 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 12 May 2015 01:33:33 -0700 Subject: [PATCH 26/35] No more boost::mpl needed for Expressions --- .cproject | 8 - gtsam/nonlinear/internal/CallRecord.h | 1 - gtsam/nonlinear/internal/ExpressionNode.h | 264 +----------------- .../nonlinear/tests/testExpressionMeta.cpp | 248 ---------------- 4 files changed, 1 insertion(+), 520 deletions(-) delete mode 100644 gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp diff --git a/.cproject b/.cproject index 5f9d5d349..7c8190e6a 100644 --- a/.cproject +++ b/.cproject @@ -2038,14 +2038,6 @@ true true - - make - -j5 - testExpressionMeta.run - true - true - true - make -j5 diff --git a/gtsam/nonlinear/internal/CallRecord.h b/gtsam/nonlinear/internal/CallRecord.h index 50d08cb8e..90aa8a8be 100644 --- a/gtsam/nonlinear/internal/CallRecord.h +++ b/gtsam/nonlinear/internal/CallRecord.h @@ -21,7 +21,6 @@ #pragma once #include -#include namespace gtsam { namespace internal { diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 65b521d6c..6e47adef0 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -23,10 +23,6 @@ #include #include -// template meta-programming headers -#include -namespace MPL = boost::mpl::placeholders; - #include // operator typeid #include #include @@ -191,272 +187,16 @@ public: }; //----------------------------------------------------------------------------- -// Below we use the "Class Composition" technique described in the book -// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost -// and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. -// to recursively generate a class, that will be the base for function nodes. -// -// The class generated, for three arguments A1, A2, and A3 will be -// -// struct Base1 : Argument, FunctionalBase { -// ... storage related to A1 ... -// ... methods that work on A1 ... -// }; -// -// struct Base2 : Argument, Base1 { -// ... storage related to A2 ... -// ... methods that work on A2 and (recursively) on A1 ... -// }; -// -// struct Base3 : Argument, Base2 { -// ... storage related to A3 ... -// ... methods that work on A3 and (recursively) on A2 and A1 ... -// }; -// -// struct FunctionalNode : Base3 { -// Provides convenience access to storage in hierarchy by using -// static_cast &>(*this) -// } -// -// All this magic happens when we generate the Base3 base class of FunctionalNode -// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode -// -// Similarly, the inner Record struct will be -// -// struct Record1 : JacobianTrace, CallRecord::value> { -// ... storage related to A1 ... -// ... methods that work on A1 ... -// }; -// -// struct Record2 : JacobianTrace, Record1 { -// ... storage related to A2 ... -// ... methods that work on A2 and (recursively) on A1 ... -// }; -// -// struct Record3 : JacobianTrace, Record2 { -// ... storage related to A3 ... -// ... methods that work on A3 and (recursively) on A2 and A1 ... -// }; -// -// struct Record : Record3 { -// Provides convenience access to storage in hierarchy by using -// static_cast &>(*this) -// } -// - -//----------------------------------------------------------------------------- - /// meta-function to generate fixed-size JacobianTA type template struct Jacobian { typedef Eigen::Matrix::dimension, traits::dimension> type; }; -/** - * Base case for recursive FunctionalNode class - */ -template -struct FunctionalBase: ExpressionNode { - static size_t const N = 0; // number of arguments - - struct Record { - void print(const std::string& indent) const { - } - void startReverseAD4(JacobianMap& jacobians) const { - } - template - void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const { - } - }; - /// Construct an execution trace for reverse AD - void trace(const Values& values, Record* record, - ExecutionTraceStorage*& traceStorage) const { - // base case: does not do anything - } -}; - -/** - * Building block for recursive FunctionalNode class - * The integer argument N is to guarantee a unique type signature, - * so we are guaranteed to be able to extract their values by static cast. - */ -template -struct Argument { - /// Expression that will generate value/derivatives for argument - boost::shared_ptr > expression; -}; - -/** - * Building block for Recursive Record Class - * Records the evaluation of a single argument in a functional expression - */ -template -struct JacobianTrace { - A value; - ExecutionTrace trace; - typename Jacobian::type dTdA; -}; - -// Recursive Definition of Functional ExpressionNode -// The reason we inherit from Argument is because we can then -// case to this unique signature to retrieve the expression at any level -template -struct GenerateFunctionalNode: Argument, Base { - - static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy - typedef Argument This; ///< The storage we have direct access to - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys = Base::keys(); - std::set myKeys = This::expression->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::expression->dims(map); - } - - // Recursive Record Class for Functional Expressions - // 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: JacobianTrace, Base::Record { - - typedef JacobianTrace This; - - /// Print to std::cout - void print(const std::string& indent) const { - Base::Record::print(indent); - static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << This::dTdA.format(matlab) << std::endl; - This::trace.print(indent); - } - - /// Start the reverse AD process - void startReverseAD4(JacobianMap& jacobians) const { - Base::Record::startReverseAD4(jacobians); - // This is the crucial point where the size of the AD pipeline is selected. - // One pipeline is started for each argument, but the number of rows in each - // pipeline is the same, namely the dimension of the output argument T. - // For example, if the entire expression is rooted by a binary function - // yielding a 2D result, then the matrix dTdA will have 2 rows. - // 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); - } - - /// 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 { - Base::Record::reverseAD4(dFdT, jacobians); - This::trace.reverseAD1(dFdT * This::dTdA, jacobians); - } - }; - - /// Construct an execution trace for reverse AD - 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 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, traceStorage); - // traceStorage is never modified by traceExecution, but if traceExecution has - // written in the buffer, the next caller expects we advance the pointer - traceStorage += This::expression->traceSize(); - } -}; - -/** - * Recursive GenerateFunctionalNode class Generator - */ -template -struct FunctionalNode { - - /// The following typedef generates the recursively defined Base class - typedef typename boost::mpl::fold, - GenerateFunctionalNode >::type Base; - - /** - * The type generated by this meta-function derives from Base - * and adds access functions as well as the crucial [trace] function - */ - struct type: public Base { - - // Argument types and derived, note these are base 0 ! - // These are currently not used - useful for Phoenix in future -#ifdef EXPRESSIONS_PHOENIX - typedef TYPES Arguments; - typedef typename boost::mpl::transform >::type Jacobians; - typedef typename boost::mpl::transform >::type Optionals; -#endif - - /// Reset expression shared pointer - template - void reset(const boost::shared_ptr >& ptr) { - static_cast &>(*this).expression = ptr; - } - - /// Access Expression shared pointer - template - boost::shared_ptr > expression() const { - return static_cast const &>(*this).expression; - } - - /// Provide convenience access to Record storage and implement - /// the virtual function based interface of CallRecord using the CallRecordImplementor - struct Record: public CallRecordImplementor::dimension>, - public Base::Record { - using Base::Record::print; - using Base::Record::startReverseAD4; - using Base::Record::reverseAD4; - - virtual ~Record() { - } - - /// Access Value - template - const A& value() const { - return static_cast const &>(*this).value; - } - - /// Access Jacobian - template - typename Jacobian::type& jacobian() { - return static_cast&>(*this).dTdA; - } - }; - - /// 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 - Base::trace(values, record, traceStorage); - - // Return the record for this function evaluation - return record; - } - }; -}; -//----------------------------------------------------------------------------- - // Eigen format for printing Jacobians static const Eigen::IOFormat kMatlabFormat(0, 1, " ", "; ", "", "", "[", "]"); +//----------------------------------------------------------------------------- /// Unary Function Expression template class UnaryExpression: public ExpressionNode { @@ -552,7 +292,6 @@ public: //----------------------------------------------------------------------------- /// Binary Expression - template class BinaryExpression: public ExpressionNode { @@ -646,7 +385,6 @@ public: //----------------------------------------------------------------------------- /// Ternary Expression - template class TernaryExpression: public ExpressionNode { diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp deleted file mode 100644 index 211d07329..000000000 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ /dev/null @@ -1,248 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file testExpressionMeta.cpp - * @date October 14, 2014 - * @author Frank Dellaert - * @brief Test meta-programming constructs for Expressions - */ - -#include -#include -#include -#include - -#include -#include - -using namespace std; -using namespace gtsam; -using namespace gtsam::internal; - -/* ************************************************************************* */ -namespace mpl = boost::mpl; - -#include -#include -template struct Incomplete; - -// Check generation of FunctionalNode -typedef mpl::vector MyTypes; -typedef FunctionalNode::type Generated; -//Incomplete incomplete; - -// Try generating vectors of ExecutionTrace -typedef mpl::vector, ExecutionTrace > ExpectedTraces; - -typedef mpl::transform >::type MyTraces; -BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces >)); - -template -struct MakeTrace { - typedef ExecutionTrace type; -}; -typedef mpl::transform >::type MyTraces1; -BOOST_MPL_ASSERT((mpl::equal< ExpectedTraces, MyTraces1 >)); - -// Try generating vectors of Expression types -typedef mpl::vector, Expression > ExpectedExpressions; -typedef mpl::transform >::type Expressions; -BOOST_MPL_ASSERT((mpl::equal< ExpectedExpressions, Expressions >)); - -// Try generating vectors of Jacobian types -typedef mpl::vector ExpectedJacobians; -typedef mpl::transform >::type Jacobians; -BOOST_MPL_ASSERT((mpl::equal< ExpectedJacobians, Jacobians >)); - -// Try accessing a Jacobian -typedef mpl::at_c::type Jacobian23; // base zero ! -BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>)); - -/* ************************************************************************* */ -// Boost Fusion includes allow us to create/access values from MPL vectors -#include -#include - -// Create a value and access it -TEST(ExpressionFactor, JacobiansValue) { - using boost::fusion::at_c; - Matrix23 expected; - Jacobians jacobians; - - at_c<1>(jacobians) << 1, 2, 3, 4, 5, 6; - - Matrix23 actual = at_c<1>(jacobians); - CHECK(actual.cols() == expected.cols()); - CHECK(actual.rows() == expected.rows()); -} - -/* ************************************************************************* */ -// Test out polymorphic transform -#include -#include -#include - -struct triple { - template struct result; // says we will provide result - - template - struct result { - typedef double type; // result for int argument - }; - - template - struct result { - typedef double type; // result for int argument - }; - - template - struct result { - typedef double type; // result for double argument - }; - - template - struct result { - typedef double type; // result for double argument - }; - - // actual function - template - typename result::type operator()(const T& x) const { - return (double) x; - } -}; - -// Test out polymorphic transform -TEST(ExpressionFactor, Triple) { - typedef boost::fusion::vector IntDouble; - IntDouble H = boost::fusion::make_vector(1, 2.0); - - // Only works if I use Double2 - typedef boost::fusion::result_of::transform::type Result; - typedef boost::fusion::vector Double2; - Double2 D = boost::fusion::transform(H, triple()); - - using boost::fusion::at_c; - DOUBLES_EQUAL(1.0, at_c<0>(D), 1e-9); - DOUBLES_EQUAL(2.0, at_c<1>(D), 1e-9); -} - -/* ************************************************************************* */ -#include -#include -#include -#include - -// Test out invoke -TEST(ExpressionFactor, Invoke) { - EXPECT_LONGS_EQUAL(2, invoke(plus(),boost::fusion::make_vector(1,1))); - - // Creating a Pose3 (is there another way?) - boost::fusion::vector pair; - Pose3 pose = boost::fusion::invoke(boost::value_factory(), pair); -} - -/* ************************************************************************* */ -// debug const issue (how to make read/write arguments for invoke) -struct test { - typedef void result_type; - void operator()(int& a, int& b) const { - a = 6; - b = 7; - } -}; - -TEST(ExpressionFactor, ConstIssue) { - int a, b; - boost::fusion::invoke(test(), - boost::fusion::make_vector(boost::ref(a), boost::ref(b))); - LONGS_EQUAL(6, a); - LONGS_EQUAL(7, b); -} - -/* ************************************************************************* */ -// Test out invoke on a given GTSAM function -// then construct prototype for it's derivatives -TEST(ExpressionFactor, InvokeDerivatives) { - // This is the method in Pose3: - // Point3 transform_to(const Point3& p) const; - // Point3 transform_to(const Point3& p, - // boost::optional Dpose, boost::optional Dpoint) const; - - // Let's assign it it to a boost function object - // cast is needed because Pose3::transform_to is overloaded -// typedef boost::function F; -// F f = static_cast(&Pose3::transform_to); -// -// // Create arguments -// Pose3 pose; -// Point3 point; -// typedef boost::fusion::vector Arguments; -// Arguments args = boost::fusion::make_vector(pose, point); -// -// // Create fused function (takes fusion vector) and call it -// boost::fusion::fused g(f); -// Point3 actual = g(args); -// CHECK(assert_equal(point,actual)); -// -// // We can *immediately* do this using invoke -// Point3 actual2 = boost::fusion::invoke(f, args); -// CHECK(assert_equal(point,actual2)); - - // Now, let's create the optional Jacobian arguments - typedef Point3 T; - typedef boost::mpl::vector TYPES; - typedef boost::mpl::transform >::type Optionals; - - // Unfortunately this is moot: we need a pointer to a function with the - // optional derivatives; I don't see a way of calling a function that we - // did not get access to by the caller passing us a pointer. - // Let's test below whether we can have a proxy object -} - -struct proxy { - typedef Point3 result_type; - Point3 operator()(const Pose3& pose, const Point3& point) const { - return pose.transform_to(point); - } - Point3 operator()(const Pose3& pose, const Point3& point, - OptionalJacobian<3,6> Dpose, - OptionalJacobian<3,3> Dpoint) const { - return pose.transform_to(point, Dpose, Dpoint); - } -}; - -TEST(ExpressionFactor, InvokeDerivatives2) { - // without derivatives - Pose3 pose; - Point3 point; - Point3 actual = boost::fusion::invoke(proxy(), - boost::fusion::make_vector(pose, point)); - CHECK(assert_equal(point,actual)); - - // with derivatives, does not work, const issue again - Matrix36 Dpose; - Matrix3 Dpoint; - Point3 actual2 = boost::fusion::invoke(proxy(), - boost::fusion::make_vector(pose, point, boost::ref(Dpose), - boost::ref(Dpoint))); - CHECK(assert_equal(point,actual2)); -} - -/* ************************************************************************* */ -int main() { - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ - From 32ecaf7e89205371571f66f2b8beb95fc2e9a07c Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 12 May 2015 12:42:06 -0700 Subject: [PATCH 27/35] Forgotten header --- gtsam/nonlinear/Expression.h | 1 + 1 file changed, 1 insertion(+) diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index 3985d8a58..afbc5e93a 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -24,6 +24,7 @@ #include #include +#include #include // Forward declare tests From e43591e4d5ad5fdaec8a1ea7c3db4180289d3f43 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 12 May 2015 12:42:18 -0700 Subject: [PATCH 28/35] Fixed Eigen include issue --- gtsam/3rdparty/ceres/eigen.h | 2 +- gtsam/3rdparty/ceres/fixed_array.h | 2 +- gtsam/3rdparty/ceres/jet.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/3rdparty/ceres/eigen.h b/gtsam/3rdparty/ceres/eigen.h index 18a602cf4..a25fde97f 100644 --- a/gtsam/3rdparty/ceres/eigen.h +++ b/gtsam/3rdparty/ceres/eigen.h @@ -31,7 +31,7 @@ #ifndef CERES_INTERNAL_EIGEN_H_ #define CERES_INTERNAL_EIGEN_H_ -#include +#include namespace ceres { diff --git a/gtsam/3rdparty/ceres/fixed_array.h b/gtsam/3rdparty/ceres/fixed_array.h index db1591636..455fce383 100644 --- a/gtsam/3rdparty/ceres/fixed_array.h +++ b/gtsam/3rdparty/ceres/fixed_array.h @@ -33,7 +33,7 @@ #define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_ #include -#include +#include #include #include diff --git a/gtsam/3rdparty/ceres/jet.h b/gtsam/3rdparty/ceres/jet.h index 12d4e8bc9..4a7683f50 100644 --- a/gtsam/3rdparty/ceres/jet.h +++ b/gtsam/3rdparty/ceres/jet.h @@ -162,7 +162,7 @@ #include #include -#include +#include #include namespace ceres { From 4ba329c23bf31bdd592af6758e10094a9c377687 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 12 May 2015 13:46:24 -0700 Subject: [PATCH 29/35] Fixed many warnings on Ubuntu --- gtsam/base/tests/testTreeTraversal.cpp | 10 +++++----- gtsam/discrete/DecisionTree-inl.h | 11 +++++------ gtsam/linear/NoiseModel.cpp | 2 +- gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp | 9 +++++---- .../slam/tests/testInvDepthFactorVariant1.cpp | 4 +--- wrap/utilities.cpp | 3 ++- 6 files changed, 19 insertions(+), 20 deletions(-) diff --git a/gtsam/base/tests/testTreeTraversal.cpp b/gtsam/base/tests/testTreeTraversal.cpp index c9083f781..8fe5e53ef 100644 --- a/gtsam/base/tests/testTreeTraversal.cpp +++ b/gtsam/base/tests/testTreeTraversal.cpp @@ -45,11 +45,11 @@ struct TestForest { }; TestForest makeTestForest() { - // 0 1 - // / \ - // 2 3 - // | - // 4 + // 0 1 + // / | + // 2 3 + // | + // 4 TestForest forest; forest.roots_.push_back(boost::make_shared(0)); forest.roots_.push_back(boost::make_shared(1)); diff --git a/gtsam/discrete/DecisionTree-inl.h b/gtsam/discrete/DecisionTree-inl.h index cd56780e5..c1648888e 100644 --- a/gtsam/discrete/DecisionTree-inl.h +++ b/gtsam/discrete/DecisionTree-inl.h @@ -467,7 +467,7 @@ namespace gtsam { // find highest label among branches boost::optional highestLabel; - boost::optional nrChoices; + size_t nrChoices = 0; for (Iterator it = begin; it != end; it++) { if (it->root_->isLeaf()) continue; @@ -475,22 +475,21 @@ namespace gtsam { boost::dynamic_pointer_cast(it->root_); if (!highestLabel || c->label() > *highestLabel) { highestLabel.reset(c->label()); - nrChoices.reset(c->nrChoices()); + nrChoices = c->nrChoices(); } } // if label is already in correct order, just put together a choice on label - if (!highestLabel || !nrChoices || label > *highestLabel) { + if (!nrChoices || !highestLabel || label > *highestLabel) { boost::shared_ptr choiceOnLabel(new Choice(label, end - begin)); for (Iterator it = begin; it != end; it++) choiceOnLabel->push_back(it->root_); return Choice::Unique(choiceOnLabel); } else { // Set up a new choice on the highest label - boost::shared_ptr choiceOnHighestLabel( - new Choice(*highestLabel, *nrChoices)); + boost::shared_ptr choiceOnHighestLabel(new Choice(*highestLabel, nrChoices)); // now, for all possible values of highestLabel - for (size_t index = 0; index < *nrChoices; index++) { + for (size_t index = 0; index < nrChoices; index++) { // make a new set of functions for composing by iterating over the given // functions, and selecting the appropriate branch. std::vector functions; diff --git a/gtsam/linear/NoiseModel.cpp b/gtsam/linear/NoiseModel.cpp index 2031a4b73..db69c9006 100644 --- a/gtsam/linear/NoiseModel.cpp +++ b/gtsam/linear/NoiseModel.cpp @@ -293,7 +293,7 @@ namespace internal { // switch precisions and invsigmas to finite value // TODO: why?? And, why not just ask s==0.0 below ? static void fix(const Vector& sigmas, Vector& precisions, Vector& invsigmas) { - for (size_t i = 0; i < sigmas.size(); ++i) + for (Vector::Index i = 0; i < sigmas.size(); ++i) if (!std::isfinite(1. / sigmas[i])) { precisions[i] = 0.0; invsigmas[i] = 0.0; diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp index 2f8fcfcee..5fb51a243 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp @@ -25,9 +25,10 @@ #include #include +#include +#include #include #include -#include using namespace std; @@ -178,7 +179,7 @@ GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem( SharedDiagonal model = noiseModel::Isotropic::Sigma(dim, sigma); damped += boost::make_shared(key_vector.first, A, b, model); - } catch (std::exception e) { + } catch (const std::exception& e) { // Don't attempt any damping if no key found in diagonal continue; } @@ -247,7 +248,7 @@ void LevenbergMarquardtOptimizer::iterate() { double modelFidelity = 0.0; bool step_is_successful = false; bool stopSearchingLambda = false; - double newError; + double newError = numeric_limits::infinity(); Values newValues; VectorValues delta; @@ -255,7 +256,7 @@ void LevenbergMarquardtOptimizer::iterate() { try { delta = solve(dampedSystem, state_.values, params_); systemSolvedSuccessfully = true; - } catch (IndeterminantLinearSystemException) { + } catch (const IndeterminantLinearSystemException& e) { systemSolvedSuccessfully = false; } diff --git a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp index 20e96e6bf..dd7e6a1d2 100644 --- a/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp +++ b/gtsam_unstable/slam/tests/testInvDepthFactorVariant1.cpp @@ -73,9 +73,7 @@ TEST( InvDepthFactorVariant1, optimize) { // Optimize the graph to recover the actual landmark position LevenbergMarquardtParams params; Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); - Vector6 actual = result.at(landmarkKey); - - +// Vector6 actual = result.at(landmarkKey); // values.at(poseKey1).print("Pose1 Before:\n"); // result.at(poseKey1).print("Pose1 After:\n"); // pose1.print("Pose1 Truth:\n"); diff --git a/wrap/utilities.cpp b/wrap/utilities.cpp index 51dc6f4c3..57a5bce29 100644 --- a/wrap/utilities.cpp +++ b/wrap/utilities.cpp @@ -91,7 +91,8 @@ bool files_equal(const string& expected, const string& actual, bool skipheader) cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n"; stringstream command; command << "diff --ignore-all-space " << expected << " " << actual << endl; - system(command.str().c_str()); + if (system(command.str().c_str())<0) + throw "command '" + command.str() + "' failed"; cerr << ">>>\n"; } return equal; From 3b83c18d67659dbbc99f3f0e3b0548c351e175bd Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 12 May 2015 13:46:43 -0700 Subject: [PATCH 30/35] Fixed traceSize failures --- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 3 ++- gtsam/nonlinear/tests/testExpressionFactor.cpp | 4 ++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index ac56b77ca..3f477098a 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -267,7 +267,8 @@ TEST(AdaptAutoDiff, Snavely) { #ifdef GTSAM_USE_QUATERNIONS EXPECT_LONGS_EQUAL(384,expression.traceSize()); // Todo, should be zero #else - EXPECT_LONGS_EQUAL(416,expression.traceSize()); // Todo, should be zero + EXPECT_LONGS_EQUAL(sizeof(internal::BinaryExpression::Record), + expression.traceSize()); // Todo, should be zero #endif set expected = list_of(1)(2); EXPECT(expected == expression.keys()); diff --git a/gtsam/nonlinear/tests/testExpressionFactor.cpp b/gtsam/nonlinear/tests/testExpressionFactor.cpp index 3aee9e50a..a36d15cee 100644 --- a/gtsam/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam/nonlinear/tests/testExpressionFactor.cpp @@ -261,8 +261,8 @@ TEST(ExpressionFactor, Shallow) { EXPECT_LONGS_EQUAL(352, sizeof(Binary::Record)); LONGS_EQUAL(96+352, expectedTraceSize); #else - EXPECT_LONGS_EQUAL(400, sizeof(Binary::Record)); - LONGS_EQUAL(112+400, expectedTraceSize); + EXPECT_LONGS_EQUAL(384, sizeof(Binary::Record)); + LONGS_EQUAL(96+384, expectedTraceSize); #endif size_t size = expression.traceSize(); CHECK(size); From 68d26ad2af7d9559eddf29d923d1fba32ab5e622 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 12 May 2015 13:54:01 -0700 Subject: [PATCH 31/35] Fixed typo --- gtsam/base/OptionalJacobian.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/OptionalJacobian.h b/gtsam/base/OptionalJacobian.h index 9ab8bc598..7055a287a 100644 --- a/gtsam/base/OptionalJacobian.h +++ b/gtsam/base/OptionalJacobian.h @@ -168,7 +168,7 @@ public: Jacobian* operator->(){ return pointer_; } }; -// forward declate +// forward declare template struct traits; /** From 41a1aab0e22f86bc592346b4d3d48dc25389e5e7 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 12 May 2015 14:23:42 -0700 Subject: [PATCH 32/35] Virtual destructor --- gtsam/nonlinear/Expression.h | 8 ++++- gtsam/nonlinear/internal/ExpressionNode.h | 41 ++++++++++++++++++----- 2 files changed, 39 insertions(+), 10 deletions(-) diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index afbc5e93a..d24a568f5 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -60,7 +60,9 @@ private: public: // 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 + // The meta-functions below are useful to specify the type of those functions. + // Example, a function taking a camera and a 3D point and yielding a 2D point: + // Expression::BinaryFunction::type template struct UnaryFunction { typedef boost::function< @@ -135,6 +137,10 @@ public: typename MakeOptionalJacobian::type) const, const Expression& expression2, const Expression& expression3); + /// Destructor + virtual ~Expression() { + } + /// Return keys that play in this expression std::set keys() const; diff --git a/gtsam/nonlinear/internal/ExpressionNode.h b/gtsam/nonlinear/internal/ExpressionNode.h index 6e47adef0..e7aa34db0 100644 --- a/gtsam/nonlinear/internal/ExpressionNode.h +++ b/gtsam/nonlinear/internal/ExpressionNode.h @@ -129,6 +129,10 @@ class ConstantExpression: public ExpressionNode { public: + /// Destructor + virtual ~ConstantExpression() { + } + /// Return value virtual T value(const Values& values) const { return constant_; @@ -154,12 +158,15 @@ class LeafExpression: public ExpressionNode { LeafExpression(Key key) : key_(key) { } - // todo: do we need a virtual destructor here too? friend class Expression ; public: + /// Destructor + virtual ~LeafExpression() { + } + /// Return keys that play in this expression virtual std::set keys() const { std::set keys; @@ -205,18 +212,23 @@ class UnaryExpression: public ExpressionNode { boost::shared_ptr > expression1_; Function function_; -public: - /// Constructor with a unary function f, and input argument e1 UnaryExpression(Function f, const Expression& e1) : expression1_(e1.root()), function_(f) { ExpressionNode::traceSize_ = upAligned(sizeof(Record)) + e1.traceSize(); } + friend class Expression ; + +public: + + /// Destructor + virtual ~UnaryExpression() { + } + /// Return value virtual T value(const Values& values) const { - using boost::none; - return function_(expression1_->value(values), none); + return function_(expression1_->value(values), boost::none); } /// Return keys that play in this expression @@ -300,8 +312,6 @@ class BinaryExpression: public ExpressionNode { boost::shared_ptr > expression2_; Function function_; -public: - /// Constructor with a binary function f, and two input arguments BinaryExpression(Function f, const Expression& e1, const Expression& e2) : @@ -310,8 +320,15 @@ public: upAligned(sizeof(Record)) + e1.traceSize() + e2.traceSize(); } + friend class Expression ; friend class ::ExpressionFactorBinaryTest; +public: + + /// Destructor + virtual ~BinaryExpression() { + } + /// Return value virtual T value(const Values& values) const { using boost::none; @@ -394,8 +411,6 @@ class TernaryExpression: public ExpressionNode { boost::shared_ptr > expression3_; Function function_; -public: - /// Constructor with a ternary function f, and two input arguments TernaryExpression(Function f, const Expression& e1, const Expression& e2, const Expression& e3) : @@ -405,6 +420,14 @@ public: e1.traceSize() + e2.traceSize() + e3.traceSize(); } + friend class Expression ; + +public: + + /// Destructor + virtual ~TernaryExpression() { + } + /// Return value virtual T value(const Values& values) const { using boost::none; From 72d2d77e210c1856c125c60aee7a0ecfdf3895bf Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 12 May 2015 14:23:51 -0700 Subject: [PATCH 33/35] Fixed warning --- wrap/tests/testWrap.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/wrap/tests/testWrap.cpp b/wrap/tests/testWrap.cpp index 1a9c82143..c03c40444 100644 --- a/wrap/tests/testWrap.cpp +++ b/wrap/tests/testWrap.cpp @@ -41,10 +41,6 @@ static string topdir = "TOPSRCDIR_NOT_CONFIGURED"; // If TOPSRCDIR is not define typedef vector strvec; -// NOTE: as this path is only used to generate makefiles, it is hardcoded here for testing -// In practice, this path will be an absolute system path, which makes testing it more annoying -static const std::string headerPath = "/not_really_a_real_path/borg/gtsam/wrap"; - /* ************************************************************************* */ TEST( wrap, ArgumentList ) { ArgumentList args; From 057aef90d918fc35fa6f9ca664c38b5549c13c91 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 12 May 2015 15:05:34 -0700 Subject: [PATCH 34/35] Fixed some more warnings on Ubuntu --- examples/SolverComparer.cpp | 50 +++++++------------ .../SmartStereoProjectionFactorExample.cpp | 14 +++--- 2 files changed, 26 insertions(+), 38 deletions(-) diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 923b0b9de..19b03d7b3 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -31,28 +31,29 @@ * */ -#include -#include -#include -#include -#include #include #include -#include -#include -#include +#include +#include +#include #include #include #include +#include +#include +#include +#include +#include -#include -#include -#include #include -#include +#include #include #include #include +#include + +#include +#include #ifdef GTSAM_USE_TBB #include @@ -72,23 +73,6 @@ typedef NoiseModelFactor1 NM1; typedef NoiseModelFactor2 NM2; typedef BearingRangeFactor BR; -//GTSAM_VALUE_EXPORT(Value); -//GTSAM_VALUE_EXPORT(Pose); -//GTSAM_VALUE_EXPORT(Rot2); -//GTSAM_VALUE_EXPORT(Point2); -//GTSAM_VALUE_EXPORT(NonlinearFactor); -//GTSAM_VALUE_EXPORT(NoiseModelFactor); -//GTSAM_VALUE_EXPORT(NM1); -//GTSAM_VALUE_EXPORT(NM2); -//GTSAM_VALUE_EXPORT(BetweenFactor); -//GTSAM_VALUE_EXPORT(PriorFactor); -//GTSAM_VALUE_EXPORT(BR); -//GTSAM_VALUE_EXPORT(noiseModel::Base); -//GTSAM_VALUE_EXPORT(noiseModel::Isotropic); -//GTSAM_VALUE_EXPORT(noiseModel::Gaussian); -//GTSAM_VALUE_EXPORT(noiseModel::Diagonal); -//GTSAM_VALUE_EXPORT(noiseModel::Unit); - double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) { // Compute degrees of freedom (observations - variables) // In ocaml, +1 was added to the observations to account for the prior, but @@ -269,12 +253,12 @@ void runIncremental() boost::dynamic_pointer_cast >(datasetMeasurements[nextMeasurement])) { Key key1 = measurement->key1(), key2 = measurement->key2(); - if((key1 >= firstStep && key1 < key2) || (key2 >= firstStep && key2 < key1)) { + if(((int)key1 >= firstStep && key1 < key2) || (key2 >= firstStep && key2 < key1)) { // We found an odometry starting at firstStep firstPose = std::min(key1, key2); break; } - if((key2 >= firstStep && key1 < key2) || (key1 >= firstStep && key2 < key1)) { + if(((int)key2 >= firstStep && key1 < key2) || (key1 >= firstStep && key2 < key1)) { // We found an odometry joining firstStep with a previous pose havePreviousPose = true; firstPose = std::max(key1, key2); @@ -303,7 +287,9 @@ void runIncremental() cout << "Playing forward time steps..." << endl; - for(size_t step = firstPose; nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || step <= lastStep); ++step) + for (size_t step = firstPose; + nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || (int)step <= lastStep); + ++step) { Values newVariables; NonlinearFactorGraph newFactors; diff --git a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp index cfed9ae0c..ac2c31077 100644 --- a/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp @@ -88,11 +88,13 @@ int main(int argc, char** argv){ } Values initial_pose_values = initial_estimate.filter(); - if(output_poses){ - init_pose3Out.open(init_poseOutput.c_str(),ios::out); - for(int i = 1; i<=initial_pose_values.size(); i++){ - init_pose3Out << i << " " << initial_pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, - " ", " ")) << endl; + if (output_poses) { + init_pose3Out.open(init_poseOutput.c_str(), ios::out); + for (size_t i = 1; i <= initial_pose_values.size(); i++) { + init_pose3Out + << i << " " + << initial_pose_values.at(Symbol('x', i)).matrix().format( + Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } } @@ -141,7 +143,7 @@ int main(int argc, char** argv){ if(output_poses){ pose3Out.open(poseOutput.c_str(),ios::out); - for(int i = 1; i<=pose_values.size(); i++){ + for(size_t i = 1; i<=pose_values.size(); i++){ pose3Out << i << " " << pose_values.at(Symbol('x',i)).matrix().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, " ", " ")) << endl; } From a585e8ac09c9bfe751c46f3a951b6357448ee796 Mon Sep 17 00:00:00 2001 From: Frank Date: Tue, 12 May 2015 15:07:49 -0700 Subject: [PATCH 35/35] And one more warning... --- examples/SolverComparer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/SolverComparer.cpp b/examples/SolverComparer.cpp index 19b03d7b3..0393affe1 100644 --- a/examples/SolverComparer.cpp +++ b/examples/SolverComparer.cpp @@ -253,12 +253,12 @@ void runIncremental() boost::dynamic_pointer_cast >(datasetMeasurements[nextMeasurement])) { Key key1 = measurement->key1(), key2 = measurement->key2(); - if(((int)key1 >= firstStep && key1 < key2) || (key2 >= firstStep && key2 < key1)) { + if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) { // We found an odometry starting at firstStep firstPose = std::min(key1, key2); break; } - if(((int)key2 >= firstStep && key1 < key2) || (key1 >= firstStep && key2 < key1)) { + if(((int)key2 >= firstStep && key1 < key2) || ((int)key1 >= firstStep && key2 < key1)) { // We found an odometry joining firstStep with a previous pose havePreviousPose = true; firstPose = std::max(key1, key2);