From aefad1e548def0cc3596a7dff773901e20606e34 Mon Sep 17 00:00:00 2001 From: dellaert Date: Fri, 3 Oct 2014 10:25:02 +0200 Subject: [PATCH] MAJOR refactor: I now use separate functions for value (only) and "augmented", for combined value-derivatives. The latter returns a new templated class, Augmented. --- gtsam_unstable/nonlinear/BADFactor.h | 12 +- gtsam_unstable/nonlinear/Expression-inl.h | 313 ++++++++++-------- gtsam_unstable/nonlinear/Expression.h | 10 +- .../nonlinear/tests/testExpression.cpp | 24 ++ 4 files changed, 214 insertions(+), 145 deletions(-) diff --git a/gtsam_unstable/nonlinear/BADFactor.h b/gtsam_unstable/nonlinear/BADFactor.h index 374c87472..a2240c0a9 100644 --- a/gtsam_unstable/nonlinear/BADFactor.h +++ b/gtsam_unstable/nonlinear/BADFactor.h @@ -52,12 +52,14 @@ public: assert(H->size()==size()); typedef std::map MapType; MapType terms; - const T& value = expression_.value(x, terms); - // move terms to H, which is pre-allocated to correct size + Augmented augmented = expression_.augmented(x); + // copy terms to H, which is pre-allocated to correct size + // TODO apply move semantics size_t j = 0; - for (MapType::iterator it = terms.begin(); it != terms.end(); ++it) - it->second.swap((*H)[j++]); - return measurement_.localCoordinates(value); + MapType::const_iterator it = augmented.jacobians().begin(); + for (; it != augmented.jacobians().end(); ++it) + (*H)[j++] = it->second; + return measurement_.localCoordinates(augmented.value()); } else { const T& value = expression_.value(x); return measurement_.localCoordinates(value); diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index d7634d62a..6e6b98c8f 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -27,9 +27,87 @@ namespace gtsam { template class Expression; -template -class MethodExpression; +typedef std::map JacobianMap; + +//----------------------------------------------------------------------------- +/** + * Value and Jacobians + */ +template +class Augmented { + +private: + + T value_; + JacobianMap jacobians_; + + typedef std::pair Pair; + + /// 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 H + Augmented(const T& t, const Matrix& H, const JacobianMap& jacobians) : + value_(t) { + add(H, jacobians); + } + + /// Construct value, pre-multiply jacobians by H + Augmented(const T& t, const Matrix& H1, const JacobianMap& jacobians1, + const Matrix& H2, const JacobianMap& jacobians2) : + value_(t) { + add(H1, jacobians1); + add(H2, jacobians2); + } + + /// Return value + const T& value() const { + return value_; + } + + /// Return jacobians + const JacobianMap& jacobians() const { + 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; + } +}; + +//----------------------------------------------------------------------------- /** * Expression node. The superclass for objects that do the heavy lifting * An Expression has a pointer to an ExpressionNode underneath @@ -46,8 +124,6 @@ protected: public: - typedef std::map JacobianMap; - /// Destructor virtual ~ExpressionNode() { } @@ -55,55 +131,31 @@ public: /// 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; + /// Return value + virtual T value(const Values& values) const = 0; -protected: + /// Return value and derivatives + virtual Augmented augmented(const Values& values) const = 0; - typedef std::pair Pair; - - /// Insert terms into Jacobians, premultiplying by H, adding if already exists - static void add(const Matrix& H, const JacobianMap& terms, - JacobianMap& jacobians) { - 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; - } - } - } - - /// debugging - static void print(const JacobianMap& terms, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) { - BOOST_FOREACH(const Pair& term, terms) { - std::cout << "(" << keyFormatter(term.first) << ", " << term.second.rows() - << "x" << term.second.cols() << ") "; - } - std::cout << std::endl; - } }; +//----------------------------------------------------------------------------- /// Constant Expression template class ConstantExpression: public ExpressionNode { - T value_; + /// The constant value + T constant_; /// Constructor with a value, yielding a constant ConstantExpression(const T& value) : - value_(value) { + constant_(value) { } friend class Expression ; public: - typedef std::map JacobianMap; - /// Destructor virtual ~ConstantExpression() { } @@ -114,11 +166,17 @@ public: return keys; } - /// Return value and optional derivatives - virtual T value(const Values& values, - boost::optional jacobians = boost::none) const { - return value_; + /// Return value + virtual T value(const Values& values) const { + return constant_; } + + /// Return value and derivatives + virtual Augmented augmented(const Values& values) const { + T t = value(values); + return Augmented(t); + } + }; //----------------------------------------------------------------------------- @@ -126,6 +184,7 @@ public: template class LeafExpression: public ExpressionNode { + /// The key into values Key key_; /// Constructor with a single key @@ -137,8 +196,6 @@ class LeafExpression: public ExpressionNode { public: - typedef std::map JacobianMap; - /// Destructor virtual ~LeafExpression() { } @@ -150,74 +207,64 @@ public: 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_); - size_t n = value.dim(); - if (jacobians) { - JacobianMap::iterator it = jacobians->find(key_); - if (it != jacobians->end()) { - it->second += Eigen::MatrixXd::Identity(n, n); - } else { - (*jacobians)[key_] = Eigen::MatrixXd::Identity(n, n); - } - } - return value; + /// Return value + virtual T value(const Values& values) const { + return values.at(key_); + } + + /// Return value and derivatives + virtual Augmented augmented(const Values& values) const { + T t = value(values); + return Augmented(t, key_); } }; //----------------------------------------------------------------------------- /// Unary Expression -template +template class UnaryExpression: public ExpressionNode { public: - typedef boost::function)> function; + typedef boost::function)> function; private: - boost::shared_ptr > expression_; + boost::shared_ptr > expressionA_; function f_; /// Constructor with a unary function f, and input argument e - UnaryExpression(function f, const Expression& e) : - expression_(e.root()), f_(f) { + UnaryExpression(function f, const Expression& e) : + expressionA_(e.root()), f_(f) { } friend class Expression ; public: - typedef std::map JacobianMap; - /// Destructor virtual ~UnaryExpression() { } /// Return keys that play in this expression virtual std::set keys() const { - return expression_->keys(); + return expressionA_->keys(); } - /// Return value and optional derivatives - virtual T value(const Values& values, - boost::optional jacobians = boost::none) const { + /// Return value + virtual T value(const Values& values) const { + return f_(expressionA_->value(values), boost::none); + } - T value; - if (jacobians) { - Eigen::MatrixXd H; - value = f_(expression_->value(values, jacobians), H); - JacobianMap::iterator it = jacobians->begin(); - for (; it != jacobians->end(); ++it) { - it->second = H * it->second; - } - } else { - value = f_(expression_->value(values), boost::none); - } - return value; + /// Return value and derivatives + virtual Augmented augmented(const Values& values) const { + using boost::none; + Augmented argument = expressionA_->augmented(values); + Matrix H; + T t = f_(argument.value(), + argument.constant() ? none : boost::optional(H)); + return Augmented(t, H, argument.jacobians()); } }; @@ -225,27 +272,25 @@ public: //----------------------------------------------------------------------------- /// Binary Expression -template +template class BinaryExpression: public ExpressionNode { public: - typedef std::map JacobianMap; - typedef boost::function< - T(const E1&, const E2&, boost::optional, + T(const A1&, const A2&, boost::optional, boost::optional)> function; private: - boost::shared_ptr > expression1_; - boost::shared_ptr > expression2_; + boost::shared_ptr > expressionA1_; + boost::shared_ptr > expressionA2_; 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) { + const Expression& e1, const Expression& e2) : + expressionA1_(e1.root()), expressionA2_(e2.root()), f_(f) { } friend class Expression ; @@ -258,31 +303,29 @@ public: /// Return keys that play in this expression virtual std::set keys() const { - std::set keys1 = expression1_->keys(); - std::set keys2 = expression2_->keys(); + std::set keys1 = expressionA1_->keys(); + std::set keys2 = expressionA2_->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) { - JacobianMap terms1, terms2; - E1 arg1 = expression1_->value(values, terms1); - E2 arg2 = expression2_->value(values, terms2); - Matrix H1, H2; - val = f_(arg1, arg2, - terms1.empty() ? boost::none : boost::optional(H1), - terms2.empty() ? boost::none : boost::optional(H2)); - ExpressionNode::add(H1, terms1, *jacobians); - ExpressionNode::add(H2, terms2, *jacobians); - } else { - val = f_(expression1_->value(values), expression2_->value(values), - boost::none, boost::none); - } - return val; + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return f_(expressionA1_->value(values), expressionA2_->value(values), none, + none); + } + + /// Return value and derivatives + virtual Augmented augmented(const Values& values) const { + using boost::none; + Augmented argument1 = expressionA1_->augmented(values); + Augmented argument2 = expressionA2_->augmented(values); + Matrix H1, H2; + T t = f_(argument1.value(), argument2.value(), + argument1.constant() ? none : boost::optional(H1), + argument2.constant() ? none : boost::optional(H2)); + return Augmented(t, H1, argument1.jacobians(), H2, argument2.jacobians()); } }; @@ -290,25 +333,23 @@ public: //----------------------------------------------------------------------------- /// Binary Expression -template +template class MethodExpression: public ExpressionNode { public: - typedef std::map JacobianMap; - - typedef T (E1::*method)(const E2&, boost::optional, + typedef T (A1::*method)(const A2&, boost::optional, boost::optional) const; private: - boost::shared_ptr > expression1_; - boost::shared_ptr > expression2_; + boost::shared_ptr > expressionA1_; + boost::shared_ptr > expressionA2_; method f_; /// Constructor with a binary function f, and two input arguments - MethodExpression(const Expression& e1, method f, const Expression& e2) : - expression1_(e1.root()), expression2_(e2.root()), f_(f) { + MethodExpression(const Expression& e1, method f, const Expression& e2) : + expressionA1_(e1.root()), expressionA2_(e2.root()), f_(f) { } friend class Expression ; @@ -321,31 +362,29 @@ public: /// Return keys that play in this expression virtual std::set keys() const { - std::set keys1 = expression1_->keys(); - std::set keys2 = expression2_->keys(); + std::set keys1 = expressionA1_->keys(); + std::set keys2 = expressionA2_->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) { - JacobianMap terms1, terms2; - E1 arg1 = expression1_->value(values, terms1); - E2 arg2 = expression2_->value(values, terms2); - Matrix H1, H2; - val = (arg1.*(f_))(arg2, - terms1.empty() ? boost::none : boost::optional(H1), - terms2.empty() ? boost::none : boost::optional(H2)); - ExpressionNode::add(H1, terms1, *jacobians); - ExpressionNode::add(H2, terms2, *jacobians); - } else { - val = (expression1_->value(values).*(f_))(expression2_->value(values), - boost::none, boost::none); - } - return val; + /// Return value + virtual T value(const Values& values) const { + using boost::none; + return (expressionA1_->value(values).*(f_))(expressionA2_->value(values), + none, none); + } + + /// Return value and derivatives + virtual Augmented augmented(const Values& values) const { + using boost::none; + Augmented argument1 = expressionA1_->augmented(values); + Augmented argument2 = expressionA2_->augmented(values); + Matrix H1, H2; + T t = (argument1.value().*(f_))(argument2.value(), + argument1.constant() ? none : boost::optional(H1), + argument2.constant() ? none : boost::optional(H2)); + return Augmented(t, H1, argument1.jacobians(), H2, argument2.jacobians()); } }; diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index eb270ae1b..b3cf8cb3c 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -83,9 +83,13 @@ public: } /// Return value and optional derivatives - T value(const Values& values, - boost::optional&> jacobians = boost::none) const { - return root_->value(values, jacobians); + T value(const Values& values) const { + return root_->value(values); + } + + /// Return value and derivatives + Augmented augmented(const Values& values) const { + return root_->augmented(values); } const boost::shared_ptr >& root() const { diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 31f5fb295..4515e7111 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -38,6 +38,30 @@ Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional Dcal, /* ************************************************************************* */ +TEST(Expression, constant) { + Expression R(Rot3::identity()); + Values values; + Augmented a = R.augmented(values); + EXPECT(assert_equal(Rot3::identity(), a.value())); + JacobianMap expected; + EXPECT(a.jacobians() == expected); +} + +/* ************************************************************************* */ + +TEST(Expression, leaf) { + Expression R(100); + Values values; + values.insert(100,Rot3::identity()); + Augmented a = R.augmented(values); + EXPECT(assert_equal(Rot3::identity(), a.value())); + JacobianMap expected; + expected[100] = eye(3); + EXPECT(a.jacobians() == expected); +} + +/* ************************************************************************* */ + TEST(Expression, test) { // Test Constant expression