diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 8b98b3b94..4ffb9afcd 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -30,6 +30,9 @@ #include #include #include +#include +#include +namespace MPL = boost::mpl::placeholders; namespace gtsam { @@ -149,13 +152,50 @@ struct Select<2, A> { } }; -//template -//struct TypedTrace { -// virtual void startReverseAD(JacobianMap& jacobians) const = 0; -// virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const = 0; -//// template -//// void reverseAD(const JacobianFT& dFdT, JacobianMap& jacobians) const { -//}; +/** + * Recursive Trace Class for Functional Expressions + * Abrahams, David; Gurtovoy, Aleksey (2004-12-10). + * C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost + * and Beyond (Kindle Location 1244). Pearson Education. + */ +template +struct Trace: More { + // define dimensions + static const size_t m = T::dimension; + static const size_t n = A::dimension; + + // define fixed size Jacobian matrix types + typedef Eigen::Matrix JacobianTA; + typedef Eigen::Matrix Jacobian2T; + + // declare trace that produces value A, and corresponding Jacobian + typename JacobianTrace::Pointer trace; + JacobianTA dTdA; + + /// Start the reverse AD process + virtual void startReverseAD(JacobianMap& jacobians) const { + More::startReverseAD(jacobians); + Select::reverseAD(trace, dTdA, jacobians); + } + /// Given df/dT, multiply in dT/dA and continue reverse AD process + virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { + More::reverseAD(dFdT, jacobians); + trace.reverseAD(dFdT * dTdA, jacobians); + } + /// Version specialized to 2-dimensional output + virtual void reverseAD2(const Jacobian2T& dFdT, + JacobianMap& jacobians) const { + More::reverseAD2(dFdT, jacobians); + trace.reverseAD2(dFdT * dTdA, jacobians); + } +}; + +/// Recursive Trace Class Generator +template +struct GenerateTrace { + typedef typename boost::mpl::fold, + Trace >::type type; +}; //----------------------------------------------------------------------------- /** @@ -388,45 +428,6 @@ public: }; -//----------------------------------------------------------------------------- - -// Abrahams, David; Gurtovoy, Aleksey (2004-12-10). -// C++ Template Metaprogramming: Concepts, Tools, and Techniques from Boost -// and Beyond (Kindle Location 1244). Pearson Education. - -/// Recursive Trace Class -template -struct MakeTrace: public JacobianTrace { - - typedef typename boost::mpl::front::type A; - - // define dimensions - static const size_t m = T::dimension; - static const size_t n = A::dimension; - - // define fixed size Jacobian matrix types - typedef Eigen::Matrix JacobianTA; - typedef Eigen::Matrix Jacobian2T; - - // declare trace that produces value A, and corresponding Jacobian - typename JacobianTrace::Pointer trace; - JacobianTA dTdA; - - /// Start the reverse AD process - virtual void startReverseAD(JacobianMap& jacobians) const { - Select::reverseAD(trace, dTdA, jacobians); - } - /// Given df/dT, multiply in dT/dA and continue reverse AD process - virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { - trace.reverseAD(dFdT * dTdA, jacobians); - } - /// Version specialized to 2-dimensional output - virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const { - trace.reverseAD2(dFdT * dTdA, jacobians); - } -}; - //----------------------------------------------------------------------------- /// Unary Function Expression template @@ -472,7 +473,8 @@ public: } /// Trace structure for reverse AD - typedef MakeTrace > Trace; + typedef boost::mpl::vector Arguments; + typedef typename GenerateTrace::type Trace; /// Construct an execution trace for reverse AD virtual T traceExecution(const Values& values, diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index e5e5e41e6..ccbc303f2 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -322,49 +322,6 @@ TEST(ExpressionFactor, composeTernary) { namespace mpl = boost::mpl; -/// Recursive Trace Class -template -struct Trace: More { - // define dimensions - static const size_t m = T::dimension; - static const size_t n = A::dimension; - - // define fixed size Jacobian matrix types - typedef Eigen::Matrix JacobianTA; - typedef Eigen::Matrix Jacobian2T; - - // declare trace that produces value A, and corresponding Jacobian - typename JacobianTrace::Pointer trace; - JacobianTA dTdA; - - /// Start the reverse AD process - virtual void startReverseAD(JacobianMap& jacobians) const { - More::startReverseAD(jacobians); - Select::reverseAD(trace, dTdA, jacobians); - } - /// Given df/dT, multiply in dT/dA and continue reverse AD process - virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { - More::reverseAD(dFdT, jacobians); - trace.reverseAD(dFdT * dTdA, jacobians); - } - /// Version specialized to 2-dimensional output - virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const { - More::reverseAD2(dFdT, jacobians); - trace.reverseAD2(dFdT * dTdA, jacobians); - } -}; - -#include -#include -namespace MPL = mpl::placeholders; - -/// Recursive Trace Class Generator -template -struct GenerateTrace { - typedef typename mpl::fold, Trace >::type type; -}; - #include template struct Incomplete;