diff --git a/gtsam/base/ChartValue.h b/gtsam/base/ChartValue.h index 52ae1650a..1ba51ff6a 100644 --- a/gtsam/base/ChartValue.h +++ b/gtsam/base/ChartValue.h @@ -139,6 +139,11 @@ private: struct PoolTag { }; }; +namespace traits { +template +struct dimension > : public dimension {}; +} + template const Chart& Value::getChart() const { // define Value::cast here since now ChartValue has been declared diff --git a/gtsam/base/Manifold.h b/gtsam/base/Manifold.h index c5b0268aa..0cac8cc6d 100644 --- a/gtsam/base/Manifold.h +++ b/gtsam/base/Manifold.h @@ -161,8 +161,11 @@ struct DefaultChart { return origin.dim(); } }; + namespace traits { +// populate default traits template struct is_chart > : public std::true_type {}; +template struct dimension > : public dimension {}; } template diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index d5c5f1279..5919d1464 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -293,8 +293,9 @@ public: //----------------------------------------------------------------------------- /// Leaf Expression -template +template > class LeafExpression: public ExpressionNode { + typedef ChartValue value_type; // perhaps this can be something else like a std::pair ?? /// The key into values Key key_; @@ -303,6 +304,53 @@ class LeafExpression: public ExpressionNode { LeafExpression(Key key) : key_(key) { } + // todo: do we need a virtual destructor here too? + + friend class Expression ; + +public: + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys; + keys.insert(key_); + return keys; + } + + /// Return dimensions for each argument + virtual void dims(std::map& map) const { + // get dimension from the chart; only works for fixed dimension charts + map[key_] = traits::dimension::value; + } + + /// Return value + virtual const value_type& value(const Values& values) const { + return dynamic_cast(values.at(key_)); + } + + /// Construct an execution trace for reverse AD + virtual const value_type& traceExecution(const Values& values, ExecutionTrace& trace, + char* raw) const { + trace.setLeaf(key_); + return dynamic_cast(values.at(key_)); + } + +}; + +//----------------------------------------------------------------------------- +/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value +template +class LeafExpression >: public ExpressionNode { + typedef T value_type; + + /// The key into values + Key key_; + + /// Constructor with a single key + LeafExpression(Key key) : + key_(key) { + } + // todo: do we need a virtual destructor here too? friend class Expression ; @@ -374,7 +422,7 @@ struct Jacobian { /// meta-function to generate JacobianTA optional reference template -struct Optional { +struct OptionalJacobian { typedef Eigen::Matrix::value, traits::dimension::value> Jacobian; typedef boost::optional type; @@ -504,7 +552,7 @@ struct FunctionalNode { // 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; + typedef typename boost::mpl::transform >::type Optionals; /// Reset expression shared pointer template @@ -559,7 +607,7 @@ class UnaryExpression: public FunctionalNode >::type { public: - typedef boost::function::type)> Function; + typedef boost::function::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -604,8 +652,8 @@ class BinaryExpression: public FunctionalNode >::t public: typedef boost::function< - T(const A1&, const A2&, typename Optional::type, - typename Optional::type)> Function; + T(const A1&, const A2&, typename OptionalJacobian::type, + typename OptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; @@ -658,8 +706,8 @@ class TernaryExpression: public FunctionalNode public: typedef boost::function< - T(const A1&, const A2&, const A3&, typename Optional::type, - typename Optional::type, typename Optional::type)> Function; + T(const A1&, const A2&, const A3&, typename OptionalJacobian::type, + typename OptionalJacobian::type, typename OptionalJacobian::type)> Function; typedef typename FunctionalNode >::type Base; typedef typename Base::Record Record; diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 6ac7d9ce8..6e97dd583 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)(typename Optional::type) const) : + T (A::*method)(typename OptionalJacobian::type) const) : root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { } @@ -75,8 +75,8 @@ public: /// Construct a unary method expression template Expression(const Expression& expression1, - T (A1::*method)(const A2&, typename Optional::type, - typename Optional::type) const, + T (A1::*method)(const A2&, typename OptionalJacobian::type, + typename OptionalJacobian::type) const, const Expression& expression2) : root_( new BinaryExpression(boost::bind(method, _1, _2, _3, _4), diff --git a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp index ecc343384..84a1ca720 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionMeta.cpp @@ -201,7 +201,7 @@ Pose3 pose; // Now, let's create the optional Jacobian arguments typedef Point3 T; typedef boost::mpl::vector TYPES; - typedef boost::mpl::transform >::type Optionals; + typedef boost::mpl::transform >::type Optionals; // Unfortunately this is moot: we need a pointer to a function with the // optional derivatives; I don't see a way of calling a function that we