diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 66366ae14..0f73efac9 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -17,6 +17,8 @@ * @brief Internals for Expression.h, not for general consumption */ +#pragma once + #include #include @@ -34,10 +36,16 @@ class Expression; */ template class ExpressionNode { + protected: ExpressionNode() { } + public: + + typedef std::map JacobianMap; + + /// Destructor virtual ~ExpressionNode() { } @@ -46,7 +54,7 @@ public: /// Return value and optional derivatives virtual T value(const Values& values, - boost::optional&> = boost::none) const = 0; + boost::optional = boost::none) const = 0; }; /// Constant Expression @@ -64,6 +72,9 @@ class ConstantExpression: public ExpressionNode { public: + typedef std::map JacobianMap; + + /// Destructor virtual ~ConstantExpression() { } @@ -75,7 +86,7 @@ public: /// Return value and optional derivatives virtual T value(const Values& values, - boost::optional&> jacobians = boost::none) const { + boost::optional jacobians = boost::none) const { return value_; } }; @@ -96,6 +107,9 @@ class LeafExpression: public ExpressionNode { public: + typedef std::map JacobianMap; + + /// Destructor virtual ~LeafExpression() { } @@ -108,10 +122,10 @@ public: /// Return value and optional derivatives virtual T value(const Values& values, - boost::optional&> jacobians = boost::none) const { + boost::optional jacobians = boost::none) const { const T& value = values.at(key_); if (jacobians) { - std::map::iterator it = jacobians->find(key_); + JacobianMap::iterator it = jacobians->find(key_); if (it != jacobians->end()) { it->second += Eigen::MatrixXd::Identity(value.dim(), value.dim()); } else { @@ -147,6 +161,9 @@ private: public: + typedef std::map JacobianMap; + + /// Destructor virtual ~UnaryExpression() { } @@ -157,13 +174,13 @@ public: /// Return value and optional derivatives virtual T value(const Values& values, - boost::optional&> jacobians = boost::none) const { + boost::optional jacobians = boost::none) const { T value; if (jacobians) { Eigen::MatrixXd H; value = f_(expression_->value(values, jacobians), H); - std::map::iterator it = jacobians->begin(); + JacobianMap::iterator it = jacobians->begin(); for (; it != jacobians->end(); ++it) { it->second = H * it->second; } @@ -183,6 +200,8 @@ class BinaryExpression: public ExpressionNode { public: + typedef std::map JacobianMap; + typedef boost::function< T(const E1&, const E2&, boost::optional, boost::optional)> function; @@ -200,8 +219,34 @@ private: friend class Expression ; + /// Combine Jacobians + static void combine(const Matrix& H1, const Matrix& H2, + const JacobianMap& terms1, const JacobianMap& terms2, + JacobianMap& jacobians) { + // TODO: both Jacobians and terms are sorted. There must be a simple + // but fast algorithm that does this. + typedef std::pair Pair; + BOOST_FOREACH(const Pair& term, terms1) { + JacobianMap::iterator it = jacobians.find(term.first); + if (it != jacobians.end()) { + it->second += H1 * term.second; + } else { + jacobians[term.first] = H1 * term.second; + } + } + BOOST_FOREACH(const Pair& term, terms2) { + JacobianMap::iterator it = jacobians.find(term.first); + if (it != jacobians.end()) { + it->second += H2 * term.second; + } else { + jacobians[term.first] = H2 * term.second; + } + } + } + public: + /// Destructor virtual ~BinaryExpression() { } @@ -215,33 +260,14 @@ public: /// Return value and optional derivatives virtual T value(const Values& values, - boost::optional&> jacobians = boost::none) const { + boost::optional jacobians = boost::none) const { T val; if (jacobians) { - std::map terms1; - std::map terms2; + JacobianMap terms1, terms2; Matrix H1, H2; val = f_(expression1_->value(values, terms1), expression2_->value(values, terms2), H1, H2); - // TODO: both Jacobians and terms are sorted. There must be a simple - // but fast algorithm that does this. - typedef std::pair Pair; - BOOST_FOREACH(const Pair& term, terms1) { - std::map::iterator it = jacobians->find(term.first); - if (it != jacobians->end()) { - it->second += H1 * term.second; - } else { - (*jacobians)[term.first] = H1 * term.second; - } - } - BOOST_FOREACH(const Pair& term, terms2) { - std::map::iterator it = jacobians->find(term.first); - if (it != jacobians->end()) { - it->second += H2 * term.second; - } else { - (*jacobians)[term.first] = H2 * term.second; - } - } + combine(H1, H2, terms1, terms2, *jacobians); } else { val = f_(expression1_->value(values), expression2_->value(values), boost::none, boost::none); diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 562554703..9dcc8d722 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -17,8 +17,9 @@ * @brief Expressions for Block Automatic Differentiation */ +#pragma once + #include "Expression-inl.h" -#include #include namespace gtsam {