diff --git a/gtsam/nonlinear/Expression-inl.h b/gtsam/nonlinear/Expression-inl.h index 48445bba5..1074400c9 100644 --- a/gtsam/nonlinear/Expression-inl.h +++ b/gtsam/nonlinear/Expression-inl.h @@ -562,8 +562,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); } @@ -669,10 +668,12 @@ struct FunctionalNode { template class UnaryExpression: public FunctionalNode >::type { + typedef typename MakeOptionalJacobian::type OJ1; + public: - typedef boost::function< - T(const A1&, typename MakeOptionalJacobian::type)> Function; + typedef T (A1::*Method)(OJ1) const; + typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -712,13 +713,16 @@ public: /// Binary Expression template -class BinaryExpression: public FunctionalNode >::type { +class BinaryExpression: + public FunctionalNode >::type { + + typedef typename MakeOptionalJacobian::type OJ1; + typedef typename MakeOptionalJacobian::type OJ2; public: - typedef boost::function< - T(const A1&, const A2&, typename MakeOptionalJacobian::type, - typename MakeOptionalJacobian::type)> Function; + typedef T (A1::*Method)(const A2&, OJ1, OJ2) const; + typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -766,15 +770,17 @@ 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< - T(const A1&, const A2&, const A3&, - typename MakeOptionalJacobian::type, - typename MakeOptionalJacobian::type, - typename MakeOptionalJacobian::type)> Function; + typedef T (A1::*Method)(const A2&, const A3&, OJ1, OJ2, OJ3) const; + typedef boost::function Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; diff --git a/gtsam/nonlinear/Expression.h b/gtsam/nonlinear/Expression.h index e63fbc686..cf87ac2ce 100644 --- a/gtsam/nonlinear/Expression.h +++ b/gtsam/nonlinear/Expression.h @@ -75,7 +75,7 @@ public: /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(typename MakeOptionalJacobian::type) const) : + typename UnaryExpression::Method method) : root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { } @@ -89,8 +89,7 @@ public: /// Construct a unary method expression template Expression(const Expression& expression1, - T (A1::*method)(const A2&, typename MakeOptionalJacobian::type, - typename MakeOptionalJacobian::type) const, + typename BinaryExpression::Method method, const Expression& expression2) : root_( new BinaryExpression(boost::bind(method, _1, _2, _3, _4), @@ -104,6 +103,17 @@ public: root_(new BinaryExpression(function, expression1, expression2)) { } + /// Construct a binary method expression + template + Expression(const Expression& expression1, + typename TernaryExpression::Method method, + 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 Expression(typename TernaryExpression::Function function,