diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 2393493d0..86a2bfa96 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -277,9 +277,9 @@ public: }; /// Primary template calls the generic Matrix reverseAD pipeline -template +template struct Select { - typedef Eigen::Matrix Jacobian; + typedef Eigen::Matrix Jacobian; static void reverseAD(const ExecutionTrace& trace, const Jacobian& dTdA, JacobianMap& jacobians) { trace.reverseAD(dTdA, jacobians); @@ -426,6 +426,13 @@ public: }; //----------------------------------------------------------------------------- +/// meta-function to generate fixed-size JacobianTA type +template +struct Jacobian { + typedef Eigen::Matrix type; + typedef boost::optional optional; +}; + /** * Building block for Recursive Record Class * Records the evaluation of a single argument in a functional expression @@ -434,9 +441,8 @@ public: */ template struct JacobianTrace { - typedef Eigen::Matrix JacobianTA; ExecutionTrace trace; - JacobianTA dTdA; + typename Jacobian::type dTdA; }; /** @@ -491,7 +497,7 @@ struct Record: public boost::mpl::fold, /// Access Jacobian template - Eigen::Matrix& jacobian() { + typename Jacobian::type& jacobian() { return static_cast&>(*this).dTdA; } @@ -535,19 +541,10 @@ struct Record: public boost::mpl::fold, */ template struct Argument { - /// Fixed size Jacobian type for the argument A - typedef Eigen::Matrix JacobianTA; - /// Expression that will generate value/derivatives for argument boost::shared_ptr > expression; }; -/// meta-function to access JacobianTA type -template -struct Jacobian { - typedef typename Argument::JacobianTA type; -}; - /** * Recursive Definition of Functional ExpressionNode */ @@ -599,8 +596,7 @@ class UnaryExpression: public FunctionalNode > { public: - typedef typename Jacobian::type JacobianTA1; - typedef boost::function)> Function; + typedef boost::function::optional)> Function; private: @@ -625,11 +621,10 @@ public: /// Return value and derivatives virtual Augmented forward(const Values& values) const { using boost::none; - Augmented argument = this->template expression()->forward(values); - JacobianTA1 dTdA; - T t = function_(argument.value(), - argument.constant() ? none : boost::optional(dTdA)); - return Augmented(t, dTdA, argument.jacobians()); + Augmented a1 = this->template expression()->forward(values); + typename Jacobian::type dTdA1; + T t = function_(a1.value(), a1.constant() ? none : typename Jacobian::optional(dTdA1)); + return Augmented(t, dTdA1, a1.jacobians()); } /// CallRecord structure for reverse AD @@ -658,11 +653,9 @@ class BinaryExpression: public FunctionalNode > { public: - typedef typename Jacobian::type JacobianTA1; - typedef typename Jacobian::type JacobianTA2; typedef boost::function< - T(const A1&, const A2&, boost::optional, - boost::optional)> Function; + T(const A1&, const A2&, typename Jacobian::optional, + typename Jacobian::optional)> Function; private: @@ -696,11 +689,11 @@ public: using boost::none; Augmented a1 = this->template expression()->forward(values); Augmented a2 = this->template expression()->forward(values); - JacobianTA1 dTdA1; - JacobianTA2 dTdA2; + typename Jacobian::type dTdA1; + typename Jacobian::type dTdA2; T t = function_(a1.value(), a2.value(), - a1.constant() ? none : boost::optional(dTdA1), - a2.constant() ? none : boost::optional(dTdA2)); + a1.constant() ? none : typename Jacobian::optional(dTdA1), + a2.constant() ? none : typename Jacobian::optional(dTdA2)); return Augmented(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians()); } @@ -734,12 +727,10 @@ class TernaryExpression: public FunctionalNode public: - typedef typename Jacobian::type JacobianTA1; - typedef typename Jacobian::type JacobianTA2; - typedef typename Jacobian::type JacobianTA3; typedef boost::function< - T(const A1&, const A2&, const A3&, boost::optional, - boost::optional, boost::optional)> Function; + T(const A1&, const A2&, const A3&, typename Jacobian::optional, + typename Jacobian::optional, + typename Jacobian::optional)> Function; private: @@ -775,13 +766,13 @@ public: Augmented a1 = this->template expression()->forward(values); Augmented a2 = this->template expression()->forward(values); Augmented a3 = this->template expression()->forward(values); - JacobianTA1 dTdA1; - JacobianTA2 dTdA2; - JacobianTA3 dTdA3; + typename Jacobian::type dTdA1; + typename Jacobian::type dTdA2; + typename Jacobian::type dTdA3; T t = function_(a1.value(), a2.value(), a3.value(), - a1.constant() ? none : boost::optional(dTdA1), - a2.constant() ? none : boost::optional(dTdA2), - a3.constant() ? none : boost::optional(dTdA3)); + a1.constant() ? none : typename Jacobian::optional(dTdA1), + a2.constant() ? none : typename Jacobian::optional(dTdA2), + a3.constant() ? none : typename Jacobian::optional(dTdA3)); return Augmented(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians(), dTdA3, a3.jacobians()); } diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 913a2b037..502826f61 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -76,9 +76,8 @@ public: /// Construct a unary method expression template Expression(const Expression& expression1, - T (A1::*method)(const A2&, - boost::optional::JacobianTA1&>, - boost::optional::JacobianTA2&>) const, + T (A1::*method)(const A2&, typename Jacobian::optional, + typename Jacobian::optional) const, const Expression& expression2) { root_.reset( new BinaryExpression(boost::bind(method, _1, _2, _3, _4), diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 16eb2fd7b..c92853d33 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -150,8 +150,8 @@ TEST(ExpressionFactor, binary) { EXPECT_LONGS_EQUAL(8, sizeof(double)); EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); EXPECT_LONGS_EQUAL(16, sizeof(ExecutionTrace)); - EXPECT_LONGS_EQUAL(2*5*8, sizeof(Binary::JacobianTA1)); - EXPECT_LONGS_EQUAL(2*2*8, sizeof(Binary::JacobianTA2)); + EXPECT_LONGS_EQUAL(2*5*8, sizeof(Jacobian::type)); + EXPECT_LONGS_EQUAL(2*2*8, sizeof(Jacobian::type)); size_t expectedRecordSize = 16 + 2 * 16 + 80 + 32; EXPECT_LONGS_EQUAL(expectedRecordSize, sizeof(Binary::Record));