diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 30ab3ca4c..fccb517c3 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -54,6 +54,114 @@ void move(JacobianMap& jacobians, std::vector& H) { it->second.swap(H[j++]); } +//----------------------------------------------------------------------------- +/** + * Value and Jacobians + */ +template +class Augmented { + +private: + + T value_; + JacobianMap jacobians_; + + typedef std::pair Pair; + + /// Insert terms into jacobians_, adding if already exists + void add(const JacobianMap& terms) { + BOOST_FOREACH(const Pair& term, terms) { + JacobianMap::iterator it = jacobians_.find(term.first); + if (it != jacobians_.end()) + it->second += term.second; + else + jacobians_[term.first] = term.second; + } + } + + /// Insert terms into jacobians_, premultiplying by H, adding if already exists + void add(const Matrix& H, const JacobianMap& terms) { + BOOST_FOREACH(const Pair& term, terms) { + JacobianMap::iterator it = jacobians_.find(term.first); + if (it != jacobians_.end()) + it->second += H * term.second; + else + jacobians_[term.first] = H * term.second; + } + } + +public: + + /// Construct value that does not depend on anything + Augmented(const T& t) : + value_(t) { + } + + /// Construct value dependent on a single key + Augmented(const T& t, Key key) : + value_(t) { + size_t n = t.dim(); + jacobians_[key] = Eigen::MatrixXd::Identity(n, n); + } + + /// Construct value, pre-multiply jacobians by dTdA + Augmented(const T& t, const Matrix& dTdA, const JacobianMap& jacobians) : + value_(t) { + add(dTdA, jacobians); + } + + /// Construct value, pre-multiply jacobians + Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1, + const Matrix& dTdA2, const JacobianMap& jacobians2) : + value_(t) { + add(dTdA1, jacobians1); + add(dTdA2, jacobians2); + } + + /// Construct value, pre-multiply jacobians + Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1, + const Matrix& dTdA2, const JacobianMap& jacobians2, const Matrix& dTdA3, + const JacobianMap& jacobians3) : + value_(t) { + add(dTdA1, jacobians1); + add(dTdA2, jacobians2); + add(dTdA3, jacobians3); + } + + /// Return value + const T& value() const { + return value_; + } + + /// Return jacobians + const JacobianMap& jacobians() const { + return jacobians_; + } + + /// Return jacobians + JacobianMap& jacobians() { + return jacobians_; + } + + /// Not dependent on any key + bool constant() const { + return jacobians_.empty(); + } + + /// debugging + void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) { + BOOST_FOREACH(const Pair& term, jacobians_) + std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows() + << "x" << term.second.cols() << ") "; + std::cout << std::endl; + } + + /// Move terms to array, destroys content + void move(std::vector& H) { + move(jacobians_, H); + } + +}; //----------------------------------------------------------------------------- /** @@ -188,195 +296,6 @@ struct Select<2, A> { } }; -//----------------------------------------------------------------------------- -/** - * Record the evaluation of a single argument in a functional expression - * Building block for Recursive Record Class - */ -template -struct Argument { - typedef Eigen::Matrix JacobianTA; - ExecutionTrace trace; - JacobianTA dTdA; -}; - -/** - * 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 Record: Argument, More { - - typedef T return_type; - static size_t const N = More::N + 1; - typedef Argument This; - - /// Print to std::cout - virtual void print(const std::string& indent) const { - More::print(indent); - static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); - std::cout << This::dTdA.format(matlab) << std::endl; - This::trace.print(indent); - } - - /// Start the reverse AD process - virtual void startReverseAD(JacobianMap& jacobians) const { - More::startReverseAD(jacobians); - Select::reverseAD(This::trace, This::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); - This::trace.reverseAD(dFdT * This::dTdA, jacobians); - } - - /// Version specialized to 2-dimensional output - typedef Eigen::Matrix Jacobian2T; - virtual void reverseAD2(const Jacobian2T& dFdT, - JacobianMap& jacobians) const { - More::reverseAD2(dFdT, jacobians); - This::trace.reverseAD2(dFdT * This::dTdA, jacobians); - } -}; - -/// Recursive Record class Generator -template -struct GenerateRecord { - typedef typename boost::mpl::fold, - Record >::type type; -}; - -/// Access Argument -template -Argument& argument(Record& record) { - return static_cast&>(record); -} - -/// Access Trace -template -ExecutionTrace& getTrace(Record* record) { - return argument(*record).trace; -} - -/// Access Jacobian -template -Eigen::Matrix& jacobian( - Record* record) { - return argument(*record).dTdA; -} - -//----------------------------------------------------------------------------- -/** - * Value and Jacobians - */ -template -class Augmented { - -private: - - T value_; - JacobianMap jacobians_; - - typedef std::pair Pair; - - /// Insert terms into jacobians_, adding if already exists - void add(const JacobianMap& terms) { - BOOST_FOREACH(const Pair& term, terms) { - JacobianMap::iterator it = jacobians_.find(term.first); - if (it != jacobians_.end()) - it->second += term.second; - else - jacobians_[term.first] = term.second; - } - } - - /// Insert terms into jacobians_, premultiplying by H, adding if already exists - void add(const Matrix& H, const JacobianMap& terms) { - BOOST_FOREACH(const Pair& term, terms) { - JacobianMap::iterator it = jacobians_.find(term.first); - if (it != jacobians_.end()) - it->second += H * term.second; - else - jacobians_[term.first] = H * term.second; - } - } - -public: - - /// Construct value that does not depend on anything - Augmented(const T& t) : - value_(t) { - } - - /// Construct value dependent on a single key - Augmented(const T& t, Key key) : - value_(t) { - size_t n = t.dim(); - jacobians_[key] = Eigen::MatrixXd::Identity(n, n); - } - - /// Construct value, pre-multiply jacobians by dTdA - Augmented(const T& t, const Matrix& dTdA, const JacobianMap& jacobians) : - value_(t) { - add(dTdA, jacobians); - } - - /// Construct value, pre-multiply jacobians - Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1, - const Matrix& dTdA2, const JacobianMap& jacobians2) : - value_(t) { - add(dTdA1, jacobians1); - add(dTdA2, jacobians2); - } - - /// Construct value, pre-multiply jacobians - Augmented(const T& t, const Matrix& dTdA1, const JacobianMap& jacobians1, - const Matrix& dTdA2, const JacobianMap& jacobians2, const Matrix& dTdA3, - const JacobianMap& jacobians3) : - value_(t) { - add(dTdA1, jacobians1); - add(dTdA2, jacobians2); - add(dTdA3, jacobians3); - } - - /// Return value - const T& value() const { - return value_; - } - - /// Return jacobians - const JacobianMap& jacobians() const { - return jacobians_; - } - - /// Return jacobians - JacobianMap& jacobians() { - return jacobians_; - } - - /// Not dependent on any key - bool constant() const { - return jacobians_.empty(); - } - - /// debugging - void print(const KeyFormatter& keyFormatter = DefaultKeyFormatter) { - BOOST_FOREACH(const Pair& term, jacobians_) - std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows() - << "x" << term.second.cols() << ") "; - std::cout << std::endl; - } - - /// Move terms to array, destroys content - void move(std::vector& H) { - move(jacobians_, H); - } - -}; - //----------------------------------------------------------------------------- /** * Expression node. The superclass for objects that do the heavy lifting @@ -388,6 +307,10 @@ public: template class ExpressionNode { +public: + + static size_t const N = 0; // number of arguments + protected: size_t traceSize_; @@ -505,6 +428,123 @@ public: }; +//----------------------------------------------------------------------------- +/** + * Building block for Recursive Record Class + * Records the evaluation of a single argument in a functional expression + * 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 JacobianTrace { + typedef Eigen::Matrix JacobianTA; + ExecutionTrace trace; + JacobianTA dTdA; +}; + +/** + * 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 Record: JacobianTrace, Base { + + typedef T return_type; + static size_t const N = Base::N + 1; + typedef JacobianTrace This; + + /// Print to std::cout + virtual void print(const std::string& indent) const { + Base::print(indent); + static const Eigen::IOFormat matlab(0, 1, " ", "; ", "", "", "[", "]"); + std::cout << This::dTdA.format(matlab) << std::endl; + This::trace.print(indent); + } + + /// Start the reverse AD process + virtual void startReverseAD(JacobianMap& jacobians) const { + Base::startReverseAD(jacobians); + Select::reverseAD(This::trace, This::dTdA, jacobians); + } + + /// Given df/dT, multiply in dT/dA and continue reverse AD process + virtual void reverseAD(const Matrix& dFdT, JacobianMap& jacobians) const { + Base::reverseAD(dFdT, jacobians); + This::trace.reverseAD(dFdT * This::dTdA, jacobians); + } + + /// Version specialized to 2-dimensional output + typedef Eigen::Matrix Jacobian2T; + virtual void reverseAD2(const Jacobian2T& dFdT, + JacobianMap& jacobians) const { + Base::reverseAD2(dFdT, jacobians); + This::trace.reverseAD2(dFdT * This::dTdA, jacobians); + } +}; + +/// Recursive Record class Generator +template +struct GenerateRecord { + typedef typename boost::mpl::fold, + Record >::type type; +}; + +/// Access JacobianTrace +template +JacobianTrace& jacobianTrace( + Record& record) { + return static_cast&>(record); +} + +/// Access Trace +template +ExecutionTrace& getTrace(Record* record) { + return jacobianTrace(*record).trace; +} + +/// Access Jacobian +template +Eigen::Matrix& jacobian( + Record* record) { + return jacobianTrace(*record).dTdA; +} + +//----------------------------------------------------------------------------- +/** + * Building block for Recursive FunctionalNode Class + */ +template +struct Argument { + boost::shared_ptr > expressionA_; +}; + +/** + * Recursive Definition of Functional ExpressionNode + */ +template +struct FunctionalNode: Argument, Base { + + typedef T return_type; + static size_t const N = Base::N + 1; + typedef Argument This; + +}; + +/// Recursive GenerateFunctionalNode class Generator +template +struct GenerateFunctionalNode { + typedef typename boost::mpl::fold, + Record >::type type; +}; + +/// Access Argument +template +Argument& argument(Record& record) { + return static_cast&>(record); +} + //----------------------------------------------------------------------------- /// Unary Function Expression template diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index 9b8c8bac3..82b03c0e4 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -175,10 +175,8 @@ TEST(ExpressionFactor, binary) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT( - assert_equal(expected25, (Matrix) static_cast*> (*r)->dTdA, 1e-9)); - EXPECT( - assert_equal(expected22, (Matrix) static_cast*> (*r)->dTdA, 1e-9)); + EXPECT(assert_equal(expected25, (Matrix) jacobian(*r), 1e-9)); + EXPECT(assert_equal(expected22, (Matrix) jacobian(*r), 1e-9)); } /* ************************************************************************* */ // Unary(Binary(Leaf,Leaf)) @@ -224,8 +222,7 @@ TEST(ExpressionFactor, shallow) { // Check matrices boost::optional r = trace.record(); CHECK(r); - EXPECT( - assert_equal(expected23, (Matrix)static_cast*>(*r)->dTdA, 1e-9)); + EXPECT(assert_equal(expected23, (Matrix)jacobian(*r), 1e-9)); // Linearization ExpressionFactor f2(model, measured, expression);