diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index a16ad8a79..3d619b5b4 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -24,8 +24,8 @@ #include #include #include -#include // for placement new -struct TestBinaryExpression; +#include +#include // template meta-programming headers #include @@ -37,9 +37,10 @@ struct TestBinaryExpression; #include #include #include - namespace MPL = boost::mpl::placeholders; +#include // for placement new + class ExpressionFactorBinaryTest; // Forward declare for testing @@ -225,12 +226,20 @@ public: return keys; } - /// Return dimensions for each argument - virtual std::map dimensions() const { + /// Return dimensions for each argument, as a map + virtual std::map dims() const { std::map map; return map; } + /// Return dimensions as vector, ordered as keys + std::vector dimensions() const { + std::map map = dims(); + std::vector dims(map.size()); + boost::copy(map | boost::adaptors::map_values, dims.begin()); + return dims; + } + // Return size needed for memory buffer in traceExecution size_t traceSize() const { return traceSize_; @@ -298,7 +307,7 @@ public: } /// Return dimensions for each argument - virtual std::map dimensions() const { + virtual std::map dims() const { std::map map; map[key_] = T::dimension; return map; @@ -416,9 +425,9 @@ struct GenerateFunctionalNode: Argument, Base { } /// Return dimensions for each argument - virtual std::map dimensions() const { - std::map map = Base::dimensions(); - std::map myMap = This::expression->dimensions(); + virtual std::map dims() const { + std::map map = Base::dims(); + std::map myMap = This::expression->dims(); map.insert(myMap.begin(), myMap.end()); return map; } diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 8ef72f914..7556ea629 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -22,8 +22,6 @@ #include "Expression-inl.h" #include #include -#include -#include namespace gtsam { @@ -36,43 +34,51 @@ class Expression { private: // Paul's trick shared pointer, polymorphic root of entire expression tree - boost::shared_ptr > root_; + const boost::shared_ptr > root_; + + // Fixed dimensions: an Expression is assumed unmutable + const std::vector dimensions_; public: // Construct a constant expression Expression(const T& value) : - root_(new ConstantExpression(value)) { + root_(new ConstantExpression(value)), // + dimensions_(root_->dimensions()) { } // Construct a leaf expression, with Key Expression(const Key& key) : - root_(new LeafExpression(key)) { + root_(new LeafExpression(key)), // + dimensions_(root_->dimensions()) { } // Construct a leaf expression, with Symbol Expression(const Symbol& symbol) : - root_(new LeafExpression(symbol)) { + root_(new LeafExpression(symbol)), // + dimensions_(root_->dimensions()) { } // Construct a leaf expression, creating Symbol Expression(unsigned char c, size_t j) : - root_(new LeafExpression(Symbol(c, j))) { + root_(new LeafExpression(Symbol(c, j))), // + dimensions_(root_->dimensions()) { } /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(typename Optional::type) const) { - root_.reset( - new UnaryExpression(boost::bind(method, _1, _2), expression)); + T (A::*method)(typename Optional::type) const) : + root_(new UnaryExpression(boost::bind(method, _1, _2), expression)), // + dimensions_(root_->dimensions()) { } /// Construct a unary function expression template Expression(typename UnaryExpression::Function function, - const Expression& expression) { - root_.reset(new UnaryExpression(function, expression)); + const Expression& expression) : + root_(new UnaryExpression(function, expression)), // + dimensions_(root_->dimensions()) { } /// Construct a unary method expression @@ -80,28 +86,31 @@ public: Expression(const Expression& expression1, T (A1::*method)(const A2&, typename Optional::type, typename Optional::type) const, - const Expression& expression2) { - root_.reset( - new BinaryExpression(boost::bind(method, _1, _2, _3, _4), - expression1, expression2)); + const Expression& expression2) : + root_( + new BinaryExpression(boost::bind(method, _1, _2, _3, _4), + expression1, expression2)), // + dimensions_(root_->dimensions()) { } /// Construct a binary function expression template Expression(typename BinaryExpression::Function function, - const Expression& expression1, const Expression& expression2) { - root_.reset( - new BinaryExpression(function, expression1, expression2)); + const Expression& expression1, const Expression& expression2) : + root_( + new BinaryExpression(function, expression1, expression2)), // + dimensions_(root_->dimensions()) { } /// Construct a ternary function expression template Expression(typename TernaryExpression::Function function, const Expression& expression1, const Expression& expression2, - const Expression& expression3) { - root_.reset( - new TernaryExpression(function, expression1, expression2, - expression3)); + const Expression& expression3) : + root_( + new TernaryExpression(function, expression1, + expression2, expression3)), // + dimensions_(root_->dimensions()) { } /// Return keys that play in this expression @@ -110,11 +119,8 @@ public: } /// Return dimensions for each argument, must be same order as keys - std::vector dimensions() const { - std::map map = root_->dimensions(); - std::vector dims(map.size()); - boost::copy(map | boost::adaptors::map_values, dims.begin()); - return dims; + const std::vector& dimensions() const { + return dimensions_; } // Return size needed for memory buffer in traceExecution