diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index ba41ab485..53b38bba4 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -35,6 +35,12 @@ struct TestBinaryExpression; #include #include #include +#include +#include + +// Boost Fusion includes allow us to create/access values from MPL vectors +#include +#include namespace MPL = boost::mpl::placeholders; @@ -462,7 +468,13 @@ public: template struct Jacobian { typedef Eigen::Matrix type; - typedef boost::optional optional; +}; + +/// meta-function to generate JacobianTA optional reference +template +struct Optional { + typedef Eigen::Matrix Jacobian; + typedef boost::optional type; }; /** @@ -573,11 +585,17 @@ struct GenerateFunctionalNode: Argument, Base { */ template struct FunctionalNode { + typedef typename boost::mpl::fold, GenerateFunctionalNode >::type Base; struct type: public Base { + // Argument types and derived, note these are base 0 ! + typedef TYPES Arguments; + typedef typename boost::mpl::transform >::type Jacobians; + typedef typename boost::mpl::transform >::type Optionals; + /// Reset expression shared pointer template void reset(const boost::shared_ptr >& ptr) { @@ -631,7 +649,7 @@ class UnaryExpression: public FunctionalNode >::type { public: - typedef boost::function::optional)> Function; + typedef boost::function::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -661,7 +679,7 @@ public: Augmented a1 = this->template expression()->forward(values); typename Jacobian::type dTdA1; T t = function_(a1.value(), - a1.constant() ? none : typename Jacobian::optional(dTdA1)); + a1.constant() ? none : typename Optional::type(dTdA1)); return Augmented(t, dTdA1, a1.jacobians()); } @@ -686,8 +704,8 @@ class BinaryExpression: public FunctionalNode >::t public: typedef boost::function< - T(const A1&, const A2&, typename Jacobian::optional, - typename Jacobian::optional)> Function; + T(const A1&, const A2&, typename Optional::type, + typename Optional::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -726,8 +744,8 @@ public: typename Jacobian::type dTdA1; typename Jacobian::type dTdA2; T t = function_(a1.value(), a2.value(), - a1.constant() ? none : typename Jacobian::optional(dTdA1), - a2.constant() ? none : typename Jacobian::optional(dTdA2)); + a1.constant() ? none : typename Optional::type(dTdA1), + a2.constant() ? none : typename Optional::type(dTdA2)); return Augmented(t, dTdA1, a1.jacobians(), dTdA2, a2.jacobians()); } @@ -753,9 +771,8 @@ class TernaryExpression: public FunctionalNode public: typedef boost::function< - T(const A1&, const A2&, const A3&, typename Jacobian::optional, - typename Jacobian::optional, - typename Jacobian::optional)> Function; + T(const A1&, const A2&, const A3&, typename Optional::type, + typename Optional::type, typename Optional::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -793,15 +810,17 @@ public: Augmented a1 = this->template expression()->forward(values); Augmented a2 = this->template expression()->forward(values); Augmented a3 = this->template expression()->forward(values); - typename Jacobian::type dTdA1; - typename Jacobian::type dTdA2; - typename Jacobian::type dTdA3; - T t = function_(a1.value(), a2.value(), a3.value(), - 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()); + + typedef typename Base::Jacobians Jacobians; + + using boost::fusion::at_c; + Jacobians H; + typename boost::mpl::at_c::type H1; + typename boost::mpl::at_c::type H2; + typename boost::mpl::at_c::type H3; + T t = function_(a1.value(), a2.value(), a3.value(),H1,H2,H3); + return Augmented(t, H1, a1.jacobians(), H2, a2.jacobians(), + H3, a3.jacobians()); } /// Construct an execution trace for reverse AD @@ -819,5 +838,5 @@ public: }; //----------------------------------------------------------------------------- -} + } diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 6b28667a7..23621f2bb 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -61,7 +61,7 @@ public: /// Construct a nullary method expression template Expression(const Expression& expression, - T (A::*method)(boost::optional) const) { + T (A::*method)(typename Optional::type) const) { root_.reset( new UnaryExpression(boost::bind(method, _1, _2), expression)); } @@ -76,8 +76,8 @@ public: /// Construct a unary method expression template Expression(const Expression& expression1, - T (A1::*method)(const A2&, typename Jacobian::optional, - typename Jacobian::optional) const, + T (A1::*method)(const A2&, typename Optional::type, + typename Optional::type) 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 45936fc8e..62ad55294 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -465,16 +465,14 @@ BOOST_MPL_ASSERT((mpl::equal< ExpectedJacobians, Jacobians >)); typedef mpl::at_c::type Jacobian23; // base zero ! BOOST_MPL_ASSERT((boost::is_same< Matrix23, Jacobian23>)); -#include -#include -#include - // Create a value and access it TEST(ExpressionFactor, JacobiansValue) { Matrix23 expected; - ExpectedJacobians jacobians; + Jacobians jacobians; using boost::fusion::at_c; + at_c<1>(jacobians) << 1,2,3,4,5,6; + Matrix23 actual = at_c<1>(jacobians); CHECK(actual.cols() == expected.cols()); CHECK(actual.rows() == expected.rows());