diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 75407fb54..2393493d0 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -441,9 +441,6 @@ struct JacobianTrace { /** * Recursive Record Class for Functional Expressions - * Abrahams, David; Gurtovoy, Aleksey (2004-12-10). - * C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost - * and Beyond. Pearson Education. */ template struct GenerateRecord: JacobianTrace, Base { @@ -501,23 +498,64 @@ struct Record: public boost::mpl::fold, }; //----------------------------------------------------------------------------- +// Below we use the "Class Composition" technique described in the book +// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost +// and Beyond. Abrahams, David; Gurtovoy, Aleksey. Pearson Education. +// to recursively generate a class, that will be the base for function nodes. +// The class generated, for two arguments A1, A2, and A3 will be +// +// struct Base1 : Argument, ExpressionNode { +// ... storage related to A1 ... +// ... methods that work on A1 ... +// }; +// +// struct Base2 : Argument, Base1 { +// ... storage related to A2 ... +// ... methods that work on A2 and (recursively) on A2 ... +// }; +// +// struct Base2 : Argument, Base2 { +// ... storage related to A3 ... +// ... methods that work on A3 and (recursively) on A2 and A3 ... +// }; +// +// struct FunctionalNode : Base3 { +// Provides convenience access to storage in hierarchy by using +// static_cast &>(*this) +// } +// +// All this magic happens when we generate the Base3 base class of FunctionalNode +// by invoking boost::mpl::fold over the meta-function GenerateFunctionalNode +//----------------------------------------------------------------------------- + /** * Building block for Recursive FunctionalNode Class + * The integer argument N is to guarantee a unique type signature, + * so we are guaranteed to be able to extract their values by static cast. */ 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 */ template struct GenerateFunctionalNode: Argument, Base { - typedef T return_type; - static size_t const N = Base::N + 1; - typedef Argument This; + static size_t const N = Base::N + 1; ///< Number of arguments in hierarchy + typedef Argument This; ///< The storage we have direct access to /// Return keys that play in this expression virtual std::set keys() const { @@ -529,18 +567,20 @@ struct GenerateFunctionalNode: Argument, Base { }; -/// Recursive GenerateFunctionalNode class Generator +/** + * Recursive GenerateFunctionalNode class Generator + */ template struct FunctionalNode: public boost::mpl::fold, GenerateFunctionalNode >::type { - /// Access Expression + /// Reset expression shared pointer template void reset(const boost::shared_ptr >& ptr) { static_cast &>(*this).expression = ptr; } - /// Access Expression, const version + /// Access Expression shared pointer template boost::shared_ptr > expression() const { return static_cast const &>(*this).expression; @@ -554,10 +594,13 @@ struct FunctionalNode: public boost::mpl::fold, template class UnaryExpression: public FunctionalNode > { + /// The automatically generated Base class + typedef FunctionalNode > Base; + public: - typedef Eigen::Matrix JacobianTA; - typedef boost::function)> Function; + typedef typename Jacobian::type JacobianTA1; + typedef boost::function)> Function; private: @@ -583,9 +626,9 @@ public: virtual Augmented forward(const Values& values) const { using boost::none; Augmented argument = this->template expression()->forward(values); - JacobianTA dTdA; + JacobianTA1 dTdA; T t = function_(argument.value(), - argument.constant() ? none : boost::optional(dTdA)); + argument.constant() ? none : boost::optional(dTdA)); return Augmented(t, dTdA, argument.jacobians()); } @@ -615,8 +658,8 @@ class BinaryExpression: public FunctionalNode > { public: - typedef Eigen::Matrix JacobianTA1; - typedef Eigen::Matrix JacobianTA2; + typedef typename Jacobian::type JacobianTA1; + typedef typename Jacobian::type JacobianTA2; typedef boost::function< T(const A1&, const A2&, boost::optional, boost::optional)> Function; @@ -691,9 +734,9 @@ class TernaryExpression: public FunctionalNode public: - typedef Eigen::Matrix JacobianTA1; - typedef Eigen::Matrix JacobianTA2; - typedef Eigen::Matrix JacobianTA3; + 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;