diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 0f73efac9..231193261 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -53,8 +53,8 @@ public: virtual std::set keys() const = 0; /// Return value and optional derivatives - virtual T value(const Values& values, - boost::optional = boost::none) const = 0; + virtual T value(const Values& values, boost::optional = + boost::none) const = 0; }; /// Constant Expression @@ -205,6 +205,7 @@ public: typedef boost::function< T(const E1&, const E2&, boost::optional, boost::optional)> function; + private: boost::shared_ptr > expression1_; @@ -277,5 +278,65 @@ public: }; +//----------------------------------------------------------------------------- +/// Binary Expression + +template +class MethodExpression: public ExpressionNode { + +public: + + typedef std::map JacobianMap; + + typedef boost::function< + T(const E1*, const E2&, boost::optional, + boost::optional)> method; + +private: + + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; + method f_; + + /// Constructor with a binary function f, and two input arguments + MethodExpression(const Expression& e1, method f, const Expression& e2) : + expression1_(e1.root()), expression2_(e2.root()), f_(f) { + } + + friend class Expression ; + +public: + + /// Destructor + virtual ~MethodExpression() { + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys1 = expression1_->keys(); + std::set keys2 = expression2_->keys(); + keys1.insert(keys2.begin(), keys2.end()); + return keys1; + } + + /// Return value and optional derivatives + virtual T value(const Values& values, + boost::optional jacobians = boost::none) const { + T val; + if (jacobians) { + JacobianMap terms1, terms2; + Matrix H1, H2; + val = f_(expression1_->value(values, terms1), + expression2_->value(values, terms2), H1, H2); + BinaryExpression::combine(H1, H2, terms1, terms2, *jacobians); + } else { + val = f_(expression1_->value(values), expression2_->value(values), + boost::none, boost::none); + } + return val; + } + +}; + } diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 9dcc8d722..826b2caf9 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -57,6 +57,15 @@ public: root_.reset(new BinaryExpression(f, expression1, expression2)); } + /// Construct a binary expression, where a method is passed + template + Expression(const Expression& expression1, + typename MethodExpression::method f, + const Expression& expression2) { + // TODO Assert that root of expressions 1 and 2 are not null. + root_.reset(new MethodExpression(f, expression1, expression2)); + } + /// Return keys that play in this expression std::set keys() const { return root_->keys(); diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 2d60400e9..faad56777 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -58,6 +58,9 @@ TEST(BAD, test) { Expression K(3); // Create expression tree +// MethodExpression::method m = &Pose3::transform_to; +// MethodExpression methodExpression(x, &Pose3::transform_to, p); +// Expression p_cam(x, &Pose3::transform_to, p); Expression p_cam(transformTo, x, p); Expression projection(project, p_cam); Expression uv_hat(uncalibrate, K, projection);