diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index fb05444c6..15a88a051 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -221,19 +221,26 @@ public: }; //----------------------------------------------------------------------------- -/// Unary Expression +/// Unary Function Expression template class UnaryExpression: public ExpressionNode { -protected: +public: + typedef boost::function)> Function; + +private: + + Function function_; boost::shared_ptr > expressionA_; - /// Constructor with one input argument expression - UnaryExpression(const Expression& e) : - expressionA_(e.root()) { + /// Constructor with a unary function f, and input argument e + UnaryExpression(Function f, const Expression& e) : + function_(f), expressionA_(e.root()) { } + friend class Expression ; + public: /// Destructor @@ -245,78 +252,6 @@ public: return expressionA_->keys(); } -}; - -//----------------------------------------------------------------------------- -/// Nullary Method Expression -template -class NullaryMethodExpression: public UnaryExpression { - -public: - - typedef T (A::*Method)(boost::optional) const; - -private: - - Method method_; - - /// Constructor with a unary function f, and input argument e - NullaryMethodExpression(const Expression& e, Method f) : - UnaryExpression(e), method_(f) { - } - - friend class Expression ; - -public: - - /// Destructor - virtual ~NullaryMethodExpression() { - } - - /// Return value - virtual T value(const Values& values) const { - using boost::none; - return (this->expressionA_->value(values).*(method_))(none); - } - - /// Return value and derivatives - virtual Augmented augmented(const Values& values) const { - using boost::none; - Augmented argument = this->expressionA_->augmented(values); - Matrix H; - T t = (argument.value().*(method_))( - argument.constant() ? none : boost::optional(H)); - return Augmented(t, H, argument.jacobians()); - } - -}; - -//----------------------------------------------------------------------------- -/// Unary Function Expression -template -class UnaryFunctionExpression: public UnaryExpression { - -public: - - typedef boost::function)> Function; - -private: - - Function function_; - - /// Constructor with a unary function f, and input argument e - UnaryFunctionExpression(Function f, const Expression& e) : - UnaryExpression(e), function_(f) { - } - - friend class Expression ; - -public: - - /// Destructor - virtual ~UnaryFunctionExpression() { - } - /// Return value virtual T value(const Values& values) const { return function_(this->expressionA_->value(values), boost::none); @@ -340,16 +275,26 @@ public: template class BinaryExpression: public ExpressionNode { -protected: +public: + typedef boost::function< + T(const A1&, const A2&, boost::optional, + boost::optional)> Function; + +private: + + Function function_; boost::shared_ptr > expressionA1_; boost::shared_ptr > expressionA2_; /// Constructor with a binary function f, and two input arguments - BinaryExpression(const Expression& e1, const Expression& e2) : - expressionA1_(e1.root()), expressionA2_(e2.root()) { + BinaryExpression(Function f, // + const Expression& e1, const Expression& e2) : + function_(f), expressionA1_(e1.root()), expressionA2_(e2.root()) { } + friend class Expression ; + public: /// Destructor @@ -364,88 +309,6 @@ public: return keys1; } -}; - -//----------------------------------------------------------------------------- -/// Binary Expression - -template -class UnaryMethodExpression: public BinaryExpression { - -public: - - typedef T (A1::*Method)(const A2&, boost::optional, - boost::optional) const; - -private: - - Method method_; - - /// Constructor with a binary function f, and two input arguments - UnaryMethodExpression(const Expression& e1, Method f, - const Expression& e2) : - BinaryExpression(e1, e2), method_(f) { - } - - friend class Expression ; - -public: - - /// Destructor - virtual ~UnaryMethodExpression() { - } - - /// Return value - virtual T value(const Values& values) const { - using boost::none; - return (this->expressionA1_->value(values).*(method_))( - this->expressionA2_->value(values), none, none); - } - - /// Return value and derivatives - virtual Augmented augmented(const Values& values) const { - using boost::none; - Augmented argument1 = this->expressionA1_->augmented(values); - Augmented argument2 = this->expressionA2_->augmented(values); - Matrix H1, H2; - T t = (argument1.value().*(method_))(argument2.value(), - argument1.constant() ? none : boost::optional(H1), - argument2.constant() ? none : boost::optional(H2)); - return Augmented(t, H1, argument1.jacobians(), H2, argument2.jacobians()); - } - -}; - -//----------------------------------------------------------------------------- -/// Binary function Expression - -template -class BinaryFunctionExpression: public BinaryExpression { - -public: - - typedef boost::function< - T(const A1&, const A2&, boost::optional, - boost::optional)> Function; - -private: - - Function function_; - - /// Constructor with a binary function f, and two input arguments - BinaryFunctionExpression(Function f, // - const Expression& e1, const Expression& e2) : - BinaryExpression(e1, e2), function_(f) { - } - - friend class Expression ; - -public: - - /// Destructor - virtual ~BinaryFunctionExpression() { - } - /// Return value virtual T value(const Values& values) const { using boost::none; diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 1cf167e0d..3686195e0 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -30,6 +30,12 @@ namespace gtsam { */ template class Expression { + +private: + + // Paul's trick shared pointer, polymorphic root of entire expression tree + boost::shared_ptr > root_; + public: // Construct a constant expression @@ -53,36 +59,36 @@ public: } /// Construct a nullary method expression - template - Expression(const Expression& expression, - typename NullaryMethodExpression::Method f) { - // TODO Assert that root of expression is not null. - root_.reset(new NullaryMethodExpression(expression, f)); + template + Expression(const Expression& expression, + T (A::*method)(boost::optional) const) { + root_.reset( + new UnaryExpression(boost::bind(method, _1, _2), expression)); } /// Construct a unary function expression - template - Expression(typename UnaryFunctionExpression::Function f, - const Expression& expression) { - // TODO Assert that root of expression is not null. - root_.reset(new UnaryFunctionExpression(f, expression)); + template + Expression(typename UnaryExpression::Function function, + const Expression& expression) { + root_.reset(new UnaryExpression(function, expression)); } /// Construct a unary method expression - template - Expression(const Expression& expression1, - typename UnaryMethodExpression::Method f, - const Expression& expression2) { - // TODO Assert that root of expressions 1 and 2 are not null. - root_.reset(new UnaryMethodExpression(expression1, f, expression2)); + template + Expression(const Expression& expression1, + T (A1::*method)(const A2&, boost::optional, + boost::optional) const, const Expression& expression2) { + root_.reset( + new BinaryExpression(boost::bind(method, _1, _2, _3, _4), + expression1, expression2)); } /// Construct a binary function expression - template - Expression(typename BinaryFunctionExpression::Function f, - const Expression& expression1, const Expression& expression2) { - // TODO Assert that root of expressions 1 and 2 are not null. - root_.reset(new BinaryFunctionExpression(f, expression1, expression2)); + template + Expression(typename BinaryExpression::Function function, + const Expression& expression1, const Expression& expression2) { + root_.reset( + new BinaryExpression(function, expression1, expression2)); } /// Return keys that play in this expression @@ -103,8 +109,7 @@ public: const boost::shared_ptr >& root() const { return root_; } -private: - boost::shared_ptr > root_; + }; // http://stackoverflow.com/questions/16260445/boost-bind-to-operator