diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 63b159e05..9f5a3ab90 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -19,17 +19,16 @@ #pragma once +#include #include -#include #include #include -#include #include #include #include -// template meta-programming headers +// template meta-programming headers, TODO not all needed? #include #include #include @@ -40,8 +39,10 @@ #include #include namespace MPL = boost::mpl::placeholders; +// +//#include // for placement new +#include -#include // for placement new class ExpressionFactorBinaryTest; // Forward declare for testing @@ -71,121 +72,6 @@ public: } }; -//----------------------------------------------------------------------------- -/** - * MaxVirtualStaticRows defines how many separate virtual reverseAD with specific - * static rows (1..MaxVirtualStaticRows) methods will be part of the CallRecord interface. - */ -const int MaxVirtualStaticRows = 4; - -namespace internal { -/** - * ConvertToDynamicIf converts to a dense matrix with dynamic rows iff ConvertToDynamicRows (colums stay as they are) otherwise - * it just passes dense Eigen matrices through. - */ -template -struct ConvertToDynamicRowsIf { - template - static Eigen::Matrix convert(const Eigen::MatrixBase & x){ - return x; - } -}; -template <> -struct ConvertToDynamicRowsIf { - template - static const Eigen::Matrix & convert(const Eigen::Matrix & x){ - return x; - } -}; - -/** - * Recursive definition of an interface having several purely - * virtual _reverseAD(const Eigen::Matrix &, JacobianMap&) - * with Rows in 1..MaxSupportedStaticRows - */ -template -struct ReverseADInterface : public ReverseADInterface < MaxSupportedStaticRows - 1, Cols> { - protected: - using ReverseADInterface < MaxSupportedStaticRows - 1, Cols>::_reverseAD; - virtual void _reverseAD(const Eigen::Matrix & dFdT, JacobianMap& jacobians) const = 0; -}; - -template -struct ReverseADInterface<0, Cols> { - protected: - void _reverseAD(){} //dummy to allow the using directive in the template without failing for MaxSupportedStaticRows == 1. -}; -} - -/** - * The CallRecord class stores the Jacobians of applying a function - * with respect to each of its arguments. It also stores an executation trace - * (defined below) for each of its arguments. - * - * It is implemented in the function-style ExpressionNode's nested Record class below. - */ -template -struct CallRecord : private internal::ReverseADInterface { - inline void print(const std::string& indent) const { - _print(indent); - } - inline void startReverseAD(JacobianMap& jacobians) const { - _startReverseAD(jacobians); - } - template - inline void reverseAD(const Eigen::Matrix & dFdT, JacobianMap& jacobians) const{ - _reverseAD(internal::ConvertToDynamicRowsIf<(Rows > MaxVirtualStaticRows)>::convert(dFdT), jacobians); - } - virtual ~CallRecord() { - } - private: - using internal::ReverseADInterface::_reverseAD; - virtual void _print(const std::string& indent) const = 0; - virtual void _startReverseAD(JacobianMap& jacobians) const = 0; - virtual void _reverseAD(const Eigen::Matrix & dFdT, JacobianMap& jacobians) const = 0; - virtual void _reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const = 0; -}; - -namespace internal { -/** - * ReverseADImplementor is a utility class used by CallRecordImplementor to implementing the recursive CallRecord interface. - */ -template -struct ReverseADImplementor : ReverseADImplementor { - protected: - virtual void _reverseAD(const Eigen::Matrix & dFdT, JacobianMap& jacobians) const { - static_cast(this)->reverseAD(dFdT, jacobians); - } -}; -template -struct ReverseADImplementor : CallRecord { -}; - -/** - * The CallRecordImplementor implements the CallRecord interface for a Derived class by - * delegating to its corresponding (templated) non-virtual methods. - */ -template -struct CallRecordImplementor : public ReverseADImplementor { - private: - const Derived & derived() const { - return static_cast(*this); - } - virtual void _print(const std::string& indent) const { - derived().print(indent); - } - virtual void _startReverseAD(JacobianMap& jacobians) const { - derived().startReverseAD(jacobians); - } - virtual void _reverseAD(const Eigen::Matrix & dFdT, JacobianMap& jacobians) const { - derived().reverseAD(dFdT, jacobians); - } - virtual void _reverseAD(const Matrix & dFdT, JacobianMap& jacobians) const { - derived().reverseAD(dFdT, jacobians); - } -}; -} - //----------------------------------------------------------------------------- /// Handle Leaf Case: reverseAD ends here, by writing a matrix into Jacobians template