From 5a1ea6071bb2011a6206db22e7964d996fab5839 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 30 Sep 2014 12:20:02 +0200 Subject: [PATCH] Split off ExpressionNode hierarchy --- gtsam_unstable/base/Expression-inl.h | 255 +++++++++++++++++++++++++++ gtsam_unstable/base/Expression.h | 238 +------------------------ 2 files changed, 261 insertions(+), 232 deletions(-) create mode 100644 gtsam_unstable/base/Expression-inl.h diff --git a/gtsam_unstable/base/Expression-inl.h b/gtsam_unstable/base/Expression-inl.h new file mode 100644 index 000000000..de6dbeb6f --- /dev/null +++ b/gtsam_unstable/base/Expression-inl.h @@ -0,0 +1,255 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Expression-inl.h + * @date September 18, 2014 + * @author Frank Dellaert + * @author Paul Furgale + * @brief Internals for Expression.h, not for general consumption + */ + +#include +#include + +namespace gtsam { + +template +class Expression; + +/** + * Expression node. The superclass for objects that do the heavy lifting + * An Expression has a pointer to an ExpressionNode underneath + * allowing Expressions to have polymorphic behaviour even though they + * are passed by value. This is the same way boost::function works. + * http://loki-lib.sourceforge.net/html/a00652.html + */ +template +class ExpressionNode { +protected: + ExpressionNode() { + } +public: + virtual ~ExpressionNode() { + } + + /// Return keys that play in this expression as a set + virtual std::set keys() const = 0; + + /// Return value and optional derivatives + virtual T value(const Values& values, + boost::optional&> = boost::none) const = 0; +}; + +/// Constant Expression +template +class ConstantExpression: public ExpressionNode { + + T value_; + + /// Constructor with a value, yielding a constant + ConstantExpression(const T& value) : + value_(value) { + } + + friend class Expression ; + +public: + + virtual ~ConstantExpression() { + } + + /// Return keys that play in this expression, i.e., the empty set + virtual std::set keys() const { + std::set keys; + return keys; + } + + /// Return value and optional derivatives + virtual T value(const Values& values, + boost::optional&> jacobians = boost::none) const { + return value_; + } +}; + +//----------------------------------------------------------------------------- +/// Leaf Expression +template +class LeafExpression: public ExpressionNode { + + Key key_; + + /// Constructor with a single key + LeafExpression(Key key) : + key_(key) { + } + + friend class Expression ; + +public: + + virtual ~LeafExpression() { + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys; + keys.insert(key_); + return keys; + } + + /// Return value and optional derivatives + virtual T value(const Values& values, + boost::optional&> jacobians = boost::none) const { + const T& value = values.at(key_); + if (jacobians) { + std::map::iterator it = jacobians->find(key_); + if (it != jacobians->end()) { + it->second += Eigen::MatrixXd::Identity(value.dim(), value.dim()); + } else { + (*jacobians)[key_] = Eigen::MatrixXd::Identity(value.dim(), + value.dim()); + } + } + return value; + } + +}; + +//----------------------------------------------------------------------------- +/// Unary Expression +template +class UnaryExpression: public ExpressionNode { + +public: + + typedef boost::function)> function; + +private: + + boost::shared_ptr > expression_; + function f_; + + /// Constructor with a unary function f, and input argument e + UnaryExpression(function f, const Expression& e) : + expression_(e.root()), f_(f) { + } + + friend class Expression ; + +public: + + virtual ~UnaryExpression() { + } + + /// Return keys that play in this expression + virtual std::set keys() const { + return expression_->keys(); + } + + /// Return value and optional derivatives + virtual T value(const Values& values, + 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(); + for (; it != jacobians->end(); ++it) { + it->second = H * it->second; + } + } else { + value = f_(expression_->value(values), boost::none); + } + return value; + } + +}; + +//----------------------------------------------------------------------------- +/// Binary Expression + +template +class BinaryExpression: public ExpressionNode { + +public: + + typedef boost::function< + T(const E1&, const E2&, boost::optional, + boost::optional)> function; +private: + + boost::shared_ptr > expression1_; + boost::shared_ptr > expression2_; + function f_; + + /// Constructor with a binary function f, and two input arguments + BinaryExpression(function f, // + const Expression& e1, const Expression& e2) : + expression1_(e1.root()), expression2_(e2.root()), f_(f) { + } + + friend class Expression ; + +public: + + virtual ~BinaryExpression() { + } + + /// Return keys that play in this expression + virtual std::set keys() const { + std::set keys1 = expression1_->keys(); + std::set keys2 = expression2_->keys(); + keys1.insert(keys2.begin(), keys2.end()); + return keys1; + } + + /// Return value and optional derivatives + virtual T value(const Values& values, + boost::optional&> jacobians = boost::none) const { + T val; + if (jacobians) { + std::map terms1; + std::map 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; + } + } + } else { + val = f_(expression1_->value(values), expression2_->value(values), + boost::none, boost::none); + } + return val; + } + +}; + +} + diff --git a/gtsam_unstable/base/Expression.h b/gtsam_unstable/base/Expression.h index 0a62fbe37..24280db98 100644 --- a/gtsam_unstable/base/Expression.h +++ b/gtsam_unstable/base/Expression.h @@ -17,243 +17,14 @@ * @brief Expressions for Block Automatic Differentiation */ +#include "Expression-inl.h" #include #include - #include -#include #include namespace gtsam { -///----------------------------------------------------------------------------- -/// Expression node. The superclass for objects that do the heavy lifting -/// An Expression has a pointer to an ExpressionNode underneath -/// allowing Expressions to have polymorphic behaviour even though they -/// are passed by value. This is the same way boost::function works. -/// http://loki-lib.sourceforge.net/html/a00652.html -template -class ExpressionNode { -protected: - ExpressionNode() { - } -public: - virtual ~ExpressionNode() { - } - - /// Return keys that play in this expression as a set - virtual std::set keys() const = 0; - - /// Return value and optional derivatives - virtual T value(const Values& values, - boost::optional&> = boost::none) const = 0; -}; - -template -class Expression; - -/// Constant Expression -template -class ConstantExpression: public ExpressionNode { - - T value_; - - /// Constructor with a value, yielding a constant - ConstantExpression(const T& value) : - value_(value) { - } - - friend class Expression ; - -public: - - virtual ~ConstantExpression() { - } - - /// Return keys that play in this expression, i.e., the empty set - virtual std::set keys() const { - std::set keys; - return keys; - } - - /// Return value and optional derivatives - virtual T value(const Values& values, - boost::optional&> jacobians = boost::none) const { - return value_; - } -}; - -//----------------------------------------------------------------------------- -/// Leaf Expression -template -class LeafExpression: public ExpressionNode { - - Key key_; - - /// Constructor with a single key - LeafExpression(Key key) : - key_(key) { - } - - friend class Expression ; - -public: - - virtual ~LeafExpression() { - } - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys; - keys.insert(key_); - return keys; - } - - /// Return value and optional derivatives - virtual T value(const Values& values, - boost::optional&> jacobians = boost::none) const { - const T& value = values.at(key_); - if (jacobians) { - std::map::iterator it = jacobians->find(key_); - if (it != jacobians->end()) { - it->second += Eigen::MatrixXd::Identity(value.dim(), value.dim()); - } else { - (*jacobians)[key_] = Eigen::MatrixXd::Identity(value.dim(), - value.dim()); - } - } - return value; - } - -}; - -//----------------------------------------------------------------------------- -/// Unary Expression -template -class UnaryExpression: public ExpressionNode { - -public: - - typedef boost::function)> function; - -private: - - boost::shared_ptr > expression_; - function f_; - - /// Constructor with a unary function f, and input argument e - UnaryExpression(function f, const Expression& e) : - expression_(e.root()), f_(f) { - } - - friend class Expression ; - -public: - - virtual ~UnaryExpression() { - } - - /// Return keys that play in this expression - virtual std::set keys() const { - return expression_->keys(); - } - - /// Return value and optional derivatives - virtual T value(const Values& values, - 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(); - for (; it != jacobians->end(); ++it) { - it->second = H * it->second; - } - } else { - value = f_(expression_->value(values), boost::none); - } - return value; - } - -}; - -//----------------------------------------------------------------------------- -/// Binary Expression - -template -class BinaryExpression: public ExpressionNode { - -public: - - typedef boost::function< - T(const E1&, const E2&, boost::optional, - boost::optional)> function; -private: - - boost::shared_ptr > expression1_; - boost::shared_ptr > expression2_; - function f_; - - /// Constructor with a binary function f, and two input arguments - BinaryExpression(function f, // - const Expression& e1, const Expression& e2) : - expression1_(e1.root()), expression2_(e2.root()), f_(f) { - } - - friend class Expression ; - -public: - - virtual ~BinaryExpression() { - } - - /// Return keys that play in this expression - virtual std::set keys() const { - std::set keys1 = expression1_->keys(); - std::set keys2 = expression2_->keys(); - keys1.insert(keys2.begin(), keys2.end()); - return keys1; - } - - /// Return value and optional derivatives - virtual T value(const Values& values, - boost::optional&> jacobians = boost::none) const { - T val; - if (jacobians) { - std::map terms1; - std::map 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; - } - } - } else { - val = f_(expression1_->value(values), expression2_->value(values), - boost::none, boost::none); - } - return val; - } - -}; - /** * Expression class that supports automatic differentiation */ @@ -323,8 +94,9 @@ Expression operator*(const Expression& expression1, expression1, expression2); } -//----------------------------------------------------------------------------- -/// AD Factor +/** + * BAD Factor that supports arbitrary expressions via AD + */ template class BADFactor: NonlinearFactor { @@ -381,5 +153,7 @@ public: } }; +// BADFactor + }