12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Split off ExpressionNode hierarchy

release/4.3a0
dellaert 2014-09-30 12:20:02 +02:00
parent e2f6f01941
commit 5a1ea6071b
2 changed files with 261 additions and 232 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

255
gtsam_unstable/base/Expression-inl.h Normal file
View File

@ -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 <gtsam/inference/Key.h>
#include <boost/foreach.hpp>
namespace gtsam {
template<typename T>
class Expression;
/**
* Expression node. The superclass for objects that do the heavy lifting
* An Expression<T> has a pointer to an ExpressionNode<T> 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 T>
class ExpressionNode {
protected:
ExpressionNode() {
}
public:
virtual ~ExpressionNode() {
}
/// Return keys that play in this expression as a set
virtual std::set<Key> keys() const = 0;
/// Return value and optional derivatives
virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> = boost::none) const = 0;
};
/// Constant Expression
template<class T>
class ConstantExpression: public ExpressionNode<T> {
T value_;
/// Constructor with a value, yielding a constant
ConstantExpression(const T& value) :
value_(value) {
}
friend class Expression<T> ;
public:
virtual ~ConstantExpression() {
}
/// Return keys that play in this expression, i.e., the empty set
virtual std::set<Key> keys() const {
std::set<Key> keys;
return keys;
}
/// Return value and optional derivatives
virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
return value_;
}
};
//-----------------------------------------------------------------------------
/// Leaf Expression
template<class T>
class LeafExpression: public ExpressionNode<T> {
Key key_;
/// Constructor with a single key
LeafExpression(Key key) :
key_(key) {
}
friend class Expression<T> ;
public:
virtual ~LeafExpression() {
}
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys;
keys.insert(key_);
return keys;
}
/// Return value and optional derivatives
virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
const T& value = values.at<T>(key_);
if (jacobians) {
std::map<Key, Matrix>::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 T, class E>
class UnaryExpression: public ExpressionNode<T> {
public:
typedef boost::function<T(const E&, boost::optional<Matrix&>)> function;
private:
boost::shared_ptr<ExpressionNode<E> > expression_;
function f_;
/// Constructor with a unary function f, and input argument e
UnaryExpression(function f, const Expression<E>& e) :
expression_(e.root()), f_(f) {
}
friend class Expression<T> ;
public:
virtual ~UnaryExpression() {
}
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
return expression_->keys();
}
/// Return value and optional derivatives
virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
T value;
if (jacobians) {
Eigen::MatrixXd H;
value = f_(expression_->value(values, jacobians), H);
std::map<Key, Matrix>::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 T, class E1, class E2>
class BinaryExpression: public ExpressionNode<T> {
public:
typedef boost::function<
T(const E1&, const E2&, boost::optional<Matrix&>,
boost::optional<Matrix&>)> function;
private:
boost::shared_ptr<ExpressionNode<E1> > expression1_;
boost::shared_ptr<ExpressionNode<E2> > expression2_;
function f_;
/// Constructor with a binary function f, and two input arguments
BinaryExpression(function f, //
const Expression<E1>& e1, const Expression<E2>& e2) :
expression1_(e1.root()), expression2_(e2.root()), f_(f) {
}
friend class Expression<T> ;
public:
virtual ~BinaryExpression() {
}
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys1 = expression1_->keys();
std::set<Key> keys2 = expression2_->keys();
keys1.insert(keys2.begin(), keys2.end());
return keys1;
}
/// Return value and optional derivatives
virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
T val;
if (jacobians) {
std::map<Key, Matrix> terms1;
std::map<Key, Matrix> 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<Key, Matrix> Pair;
BOOST_FOREACH(const Pair& term, terms1) {
std::map<Key, Matrix>::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<Key, Matrix>::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;
}
};
}

238
gtsam_unstable/base/Expression.h
View File

@ -17,243 +17,14 @@
* @brief Expressions for Block Automatic Differentiation * @brief Expressions for Block Automatic Differentiation
*/ */
#include "Expression-inl.h"
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/foreach.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>
namespace gtsam { namespace gtsam {
///-----------------------------------------------------------------------------
/// Expression node. The superclass for objects that do the heavy lifting
/// An Expression<T> has a pointer to an ExpressionNode<T> 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 T>
class ExpressionNode {
protected:
ExpressionNode() {
}
public:
virtual ~ExpressionNode() {
}
/// Return keys that play in this expression as a set
virtual std::set<Key> keys() const = 0;
/// Return value and optional derivatives
virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> = boost::none) const = 0;
};
template<typename T>
class Expression;
/// Constant Expression
template<class T>
class ConstantExpression: public ExpressionNode<T> {
T value_;
/// Constructor with a value, yielding a constant
ConstantExpression(const T& value) :
value_(value) {
}
friend class Expression<T> ;
public:
virtual ~ConstantExpression() {
}
/// Return keys that play in this expression, i.e., the empty set
virtual std::set<Key> keys() const {
std::set<Key> keys;
return keys;
}
/// Return value and optional derivatives
virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
return value_;
}
};
//-----------------------------------------------------------------------------
/// Leaf Expression
template<class T>
class LeafExpression: public ExpressionNode<T> {
Key key_;
/// Constructor with a single key
LeafExpression(Key key) :
key_(key) {
}
friend class Expression<T> ;
public:
virtual ~LeafExpression() {
}
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys;
keys.insert(key_);
return keys;
}
/// Return value and optional derivatives
virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
const T& value = values.at<T>(key_);
if (jacobians) {
std::map<Key, Matrix>::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 T, class E>
class UnaryExpression: public ExpressionNode<T> {
public:
typedef boost::function<T(const E&, boost::optional<Matrix&>)> function;
private:
boost::shared_ptr<ExpressionNode<E> > expression_;
function f_;
/// Constructor with a unary function f, and input argument e
UnaryExpression(function f, const Expression<E>& e) :
expression_(e.root()), f_(f) {
}
friend class Expression<T> ;
public:
virtual ~UnaryExpression() {
}
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
return expression_->keys();
}
/// Return value and optional derivatives
virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
T value;
if (jacobians) {
Eigen::MatrixXd H;
value = f_(expression_->value(values, jacobians), H);
std::map<Key, Matrix>::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 T, class E1, class E2>
class BinaryExpression: public ExpressionNode<T> {
public:
typedef boost::function<
T(const E1&, const E2&, boost::optional<Matrix&>,
boost::optional<Matrix&>)> function;
private:
boost::shared_ptr<ExpressionNode<E1> > expression1_;
boost::shared_ptr<ExpressionNode<E2> > expression2_;
function f_;
/// Constructor with a binary function f, and two input arguments
BinaryExpression(function f, //
const Expression<E1>& e1, const Expression<E2>& e2) :
expression1_(e1.root()), expression2_(e2.root()), f_(f) {
}
friend class Expression<T> ;
public:
virtual ~BinaryExpression() {
}
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys1 = expression1_->keys();
std::set<Key> keys2 = expression2_->keys();
keys1.insert(keys2.begin(), keys2.end());
return keys1;
}
/// Return value and optional derivatives
virtual T value(const Values& values,
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const {
T val;
if (jacobians) {
std::map<Key, Matrix> terms1;
std::map<Key, Matrix> 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<Key, Matrix> Pair;
BOOST_FOREACH(const Pair& term, terms1) {
std::map<Key, Matrix>::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<Key, Matrix>::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 * Expression class that supports automatic differentiation
*/ */
@ -323,8 +94,9 @@ Expression<T> operator*(const Expression<T>& expression1,
expression1, expression2); expression1, expression2);
} }
//----------------------------------------------------------------------------- /**
/// AD Factor * BAD Factor that supports arbitrary expressions via AD
*/
template<class T> template<class T>
class BADFactor: NonlinearFactor { class BADFactor: NonlinearFactor {
@ -381,5 +153,7 @@ public:
} }
}; };
// BADFactor
} }