/* ---------------------------------------------------------------------------- * 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.h * @date September 18, 2014 * @author Frank Dellaert * @author Paul Furgale * @brief Expressions for Block Automatic Differentiation */ #include #include #include #include #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 */ template class Expression { public: // Construct a constant expression Expression(const T& value) : root_(new ConstantExpression(value)) { } // Construct a leaf expression Expression(const Key& key) : root_(new LeafExpression(key)) { } /// Construct a unary expression template Expression(typename UnaryExpression::function f, const Expression& expression) { // TODO Assert that root of expression is not null. root_.reset(new UnaryExpression(f, expression)); } /// Construct a binary expression template Expression(typename BinaryExpression::function f, const Expression& expression1, const Expression& expression2) { // TODO Assert that root of expressions 1 and 2 are not null. root_.reset(new BinaryExpression(f, expression1, expression2)); } /// Return keys that play in this expression std::set keys() const { return root_->keys(); } /// Return value and optional derivatives T value(const Values& values, boost::optional&> jacobians = boost::none) const { return root_->value(values, jacobians); } const boost::shared_ptr >& root() const { return root_; } private: boost::shared_ptr > root_; }; // http://stackoverflow.com/questions/16260445/boost-bind-to-operator template struct apply_compose { typedef T result_type; T operator()(const T& x, const T& y, boost::optional H1, boost::optional H2) const { return x.compose(y, H1, H2); } }; /// Construct a product expression, assumes T::compose(T) -> T template Expression operator*(const Expression& expression1, const Expression& expression2) { return Expression(boost::bind(apply_compose(), _1, _2, _3, _4), expression1, expression2); } // http://stackoverflow.com/questions/16260445/boost-bind-to-operator template struct apply_product { typedef E2 result_type; E2 operator()(E1 const& x, E2 const& y) const { return x * y; } }; /// Construct a product expression, assumes E1 * E2 -> E1 template Expression operator*(const Expression& expression1, const Expression& expression2) { using namespace boost; return Expression(boost::bind(apply_product(), _1, _2), expression1, expression2); } //----------------------------------------------------------------------------- /// AD Factor template class BADFactor: NonlinearFactor { const T measurement_; const Expression expression_; /// get value from expression and calculate error with respect to measurement Vector unwhitenedError(const Values& values) const { const T& value = expression_.value(values); return value.localCoordinates(measurement_); } public: /// Constructor BADFactor(const T& measurement, const Expression& expression) : measurement_(measurement), expression_(expression) { } /// Constructor BADFactor(const T& measurement, const ExpressionNode& expression) : measurement_(measurement), expression_(expression) { } /** * Calculate the error of the factor. * This is the log-likelihood, e.g. \f$ 0.5(h(x)-z)^2/\sigma^2 \f$ in case of Gaussian. * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. */ virtual double error(const Values& values) const { if (this->active(values)) { const Vector e = unwhitenedError(values); return 0.5 * e.squaredNorm(); } else { return 0.0; } } /// get the dimension of the factor (number of rows on linearization) size_t dim() const { return 0; } /// linearize to a GaussianFactor boost::shared_ptr linearize(const Values& values) const { // We will construct an n-ary factor below, where terms is a container whose // value type is std::pair, specifying the // collection of keys and matrices making up the factor. std::map terms; expression_.value(values, terms); Vector b = unwhitenedError(values); SharedDiagonal model = SharedDiagonal(); return boost::shared_ptr( new JacobianFactor(terms, b, model)); } }; }