diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 6e6b98c8f..93240f80d 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -225,22 +225,15 @@ public: template class UnaryExpression: public ExpressionNode { -public: - - typedef boost::function)> function; - -private: +protected: boost::shared_ptr > expressionA_; - function f_; - /// Constructor with a unary function f, and input argument e - UnaryExpression(function f, const Expression& e) : - expressionA_(e.root()), f_(f) { + /// Constructor with one input argument expression + UnaryExpression(const Expression& e) : + expressionA_(e.root()) { } - friend class Expression ; - public: /// Destructor @@ -252,17 +245,46 @@ 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 { - return f_(expressionA_->value(values), boost::none); + 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 = expressionA_->augmented(values); + Augmented argument = this->expressionA_->augmented(values); Matrix H; - T t = f_(argument.value(), + T t = (argument.value().*(method_))( argument.constant() ? none : boost::optional(H)); return Augmented(t, H, argument.jacobians()); } @@ -270,7 +292,50 @@ public: }; //----------------------------------------------------------------------------- -/// Binary Expression +/// 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); + } + + /// Return value and derivatives + virtual Augmented augmented(const Values& values) const { + using boost::none; + Augmented argument = this->expressionA_->augmented(values); + Matrix H; + T t = function_(argument.value(), + argument.constant() ? none : boost::optional(H)); + return Augmented(t, H, argument.jacobians()); + } + +}; + +//----------------------------------------------------------------------------- +/// Binary function Expression template class BinaryExpression: public ExpressionNode { @@ -285,12 +350,12 @@ private: boost::shared_ptr > expressionA1_; boost::shared_ptr > expressionA2_; - function f_; + function function_; /// Constructor with a binary function f, and two input arguments BinaryExpression(function f, // const Expression& e1, const Expression& e2) : - expressionA1_(e1.root()), expressionA2_(e2.root()), f_(f) { + expressionA1_(e1.root()), expressionA2_(e2.root()), function_(f) { } friend class Expression ; @@ -312,8 +377,8 @@ public: /// Return value virtual T value(const Values& values) const { using boost::none; - return f_(expressionA1_->value(values), expressionA2_->value(values), none, - none); + return function_(expressionA1_->value(values), expressionA2_->value(values), + none, none); } /// Return value and derivatives @@ -322,7 +387,7 @@ public: Augmented argument1 = expressionA1_->augmented(values); Augmented argument2 = expressionA2_->augmented(values); Matrix H1, H2; - T t = f_(argument1.value(), argument2.value(), + T t = function_(argument1.value(), argument2.value(), argument1.constant() ? none : boost::optional(H1), argument2.constant() ? none : boost::optional(H2)); return Augmented(t, H1, argument1.jacobians(), H2, argument2.jacobians()); @@ -345,11 +410,11 @@ private: boost::shared_ptr > expressionA1_; boost::shared_ptr > expressionA2_; - method f_; + method method_; /// Constructor with a binary function f, and two input arguments MethodExpression(const Expression& e1, method f, const Expression& e2) : - expressionA1_(e1.root()), expressionA2_(e2.root()), f_(f) { + expressionA1_(e1.root()), expressionA2_(e2.root()), method_(f) { } friend class Expression ; @@ -371,7 +436,7 @@ public: /// Return value virtual T value(const Values& values) const { using boost::none; - return (expressionA1_->value(values).*(f_))(expressionA2_->value(values), + return (expressionA1_->value(values).*(method_))(expressionA2_->value(values), none, none); } @@ -381,7 +446,7 @@ public: Augmented argument1 = expressionA1_->augmented(values); Augmented argument2 = expressionA2_->augmented(values); Matrix H1, H2; - T t = (argument1.value().*(f_))(argument2.value(), + 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()); diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index b3cf8cb3c..82c0e2119 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -52,12 +52,20 @@ public: root_(new LeafExpression(Symbol(c, j))) { } - /// Construct a unary expression + /// Construct a nullary method expression template - Expression(typename UnaryExpression::function f, + Expression(const Expression& expression, + typename NullaryMethodExpression::method f) { + // TODO Assert that root of expression is not null. + root_.reset(new NullaryMethodExpression(expression, f)); + } + + /// 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 UnaryExpression(f, expression)); + root_.reset(new UnaryFunctionExpression(f, expression)); } /// Construct a binary expression diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 4515e7111..057359155 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -62,6 +62,20 @@ TEST(Expression, leaf) { /* ************************************************************************* */ +TEST(Expression, nullaryMethod) { + Expression p(67); + Expression norm(p, &Point3::norm); + Values values; + values.insert(67,Point3(3,4,5)); + Augmented a = norm.augmented(values); + EXPECT(a.value() == sqrt(50)); + JacobianMap expected; + expected[67] = (Matrix(1,3) << 3/sqrt(50),4/sqrt(50),5/sqrt(50)); + EXPECT(assert_equal(expected.at(67),a.jacobians().at(67))); +} + +/* ************************************************************************* */ + TEST(Expression, test) { // Test Constant expression