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

MAJOR refactor: I now use separate functions for value (only) and "augmented", for combined value-derivatives. The latter returns a new templated class, Augmented<T>.

release/4.3a0
dellaert 2014-10-03 10:25:02 +02:00
parent da4cfe6fdc
commit aefad1e548
4 changed files with 214 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam_unstable/nonlinear/BADFactor.h
View File

@ -52,12 +52,14 @@ public:
assert(H->size()==size()); assert(H->size()==size());
typedef std::map<Key, Matrix> MapType; typedef std::map<Key, Matrix> MapType;
MapType terms; MapType terms;
const T& value = expression_.value(x, terms); Augmented<T> augmented = expression_.augmented(x);
// move terms to H, which is pre-allocated to correct size // copy terms to H, which is pre-allocated to correct size
// TODO apply move semantics
size_t j = 0; size_t j = 0;
for (MapType::iterator it = terms.begin(); it != terms.end(); ++it) MapType::const_iterator it = augmented.jacobians().begin();
it->second.swap((*H)[j++]); for (; it != augmented.jacobians().end(); ++it)
return measurement_.localCoordinates(value); (*H)[j++] = it->second;
return measurement_.localCoordinates(augmented.value());
} else { } else {
const T& value = expression_.value(x); const T& value = expression_.value(x);
return measurement_.localCoordinates(value); return measurement_.localCoordinates(value);

307
gtsam_unstable/nonlinear/Expression-inl.h
View File

@ -27,9 +27,87 @@ namespace gtsam {
template<typename T> template<typename T>
class Expression; class Expression;
template<typename T, typename E1, typename E2>
class MethodExpression;
typedef std::map<Key, Matrix> JacobianMap;
//-----------------------------------------------------------------------------
/**
* Value and Jacobians
*/
template<class T>
class Augmented {
private:
T value_;
JacobianMap jacobians_;
typedef std::pair<Key, Matrix> 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 * Expression node. The superclass for objects that do the heavy lifting
* An Expression<T> has a pointer to an ExpressionNode<T> underneath * An Expression<T> has a pointer to an ExpressionNode<T> underneath
@ -46,8 +124,6 @@ protected:
public: public:
typedef std::map<Key, Matrix> JacobianMap;
/// Destructor /// Destructor
virtual ~ExpressionNode() { virtual ~ExpressionNode() {
} }
@ -55,55 +131,31 @@ public:
/// Return keys that play in this expression as a set /// Return keys that play in this expression as a set
virtual std::set<Key> keys() const = 0; virtual std::set<Key> keys() const = 0;
/// Return value and optional derivatives /// Return value
virtual T value(const Values& values, boost::optional<JacobianMap&> = virtual T value(const Values& values) const = 0;
boost::none) const = 0;
protected: /// Return value and derivatives
virtual Augmented<T> augmented(const Values& values) const = 0;
typedef std::pair<Key, Matrix> 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 /// Constant Expression
template<class T> template<class T>
class ConstantExpression: public ExpressionNode<T> { class ConstantExpression: public ExpressionNode<T> {
T value_; /// The constant value
T constant_;
/// Constructor with a value, yielding a constant /// Constructor with a value, yielding a constant
ConstantExpression(const T& value) : ConstantExpression(const T& value) :
value_(value) { constant_(value) {
} }
friend class Expression<T> ; friend class Expression<T> ;
public: public:
typedef std::map<Key, Matrix> JacobianMap;
/// Destructor /// Destructor
virtual ~ConstantExpression() { virtual ~ConstantExpression() {
} }
@ -114,11 +166,17 @@ public:
return keys; return keys;
} }
/// Return value and optional derivatives /// Return value
virtual T value(const Values& values, virtual T value(const Values& values) const {
boost::optional<JacobianMap&> jacobians = boost::none) const { return constant_;
return value_;
} }
/// Return value and derivatives
virtual Augmented<T> augmented(const Values& values) const {
T t = value(values);
return Augmented<T>(t);
}
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
@ -126,6 +184,7 @@ public:
template<class T> template<class T>
class LeafExpression: public ExpressionNode<T> { class LeafExpression: public ExpressionNode<T> {
/// The key into values
Key key_; Key key_;
/// Constructor with a single key /// Constructor with a single key
@ -137,8 +196,6 @@ class LeafExpression: public ExpressionNode<T> {
public: public:
typedef std::map<Key, Matrix> JacobianMap;
/// Destructor /// Destructor
virtual ~LeafExpression() { virtual ~LeafExpression() {
} }
@ -150,74 +207,64 @@ public:
return keys; return keys;
} }
/// Return value and optional derivatives /// Return value
virtual T value(const Values& values, virtual T value(const Values& values) const {
boost::optional<JacobianMap&> jacobians = boost::none) const { return values.at<T>(key_);
const T& value = values.at<T>(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 and derivatives
virtual Augmented<T> augmented(const Values& values) const {
T t = value(values);
return Augmented<T>(t, key_);
} }
}; };
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/// Unary Expression /// Unary Expression
template<class T, class E> template<class T, class A>
class UnaryExpression: public ExpressionNode<T> { class UnaryExpression: public ExpressionNode<T> {
public: public:
typedef boost::function<T(const E&, boost::optional<Matrix&>)> function; typedef boost::function<T(const A&, boost::optional<Matrix&>)> function;
private: private:
boost::shared_ptr<ExpressionNode<E> > expression_; boost::shared_ptr<ExpressionNode<A> > expressionA_;
function f_; function f_;
/// Constructor with a unary function f, and input argument e /// Constructor with a unary function f, and input argument e
UnaryExpression(function f, const Expression<E>& e) : UnaryExpression(function f, const Expression<A>& e) :
expression_(e.root()), f_(f) { expressionA_(e.root()), f_(f) {
} }
friend class Expression<T> ; friend class Expression<T> ;
public: public:
typedef std::map<Key, Matrix> JacobianMap;
/// Destructor /// Destructor
virtual ~UnaryExpression() { virtual ~UnaryExpression() {
} }
/// Return keys that play in this expression /// Return keys that play in this expression
virtual std::set<Key> keys() const { virtual std::set<Key> keys() const {
return expression_->keys(); return expressionA_->keys();
} }
/// Return value and optional derivatives /// Return value
virtual T value(const Values& values, virtual T value(const Values& values) const {
boost::optional<JacobianMap&> jacobians = boost::none) const { return f_(expressionA_->value(values), boost::none);
}
T value; /// Return value and derivatives
if (jacobians) { virtual Augmented<T> augmented(const Values& values) const {
Eigen::MatrixXd H; using boost::none;
value = f_(expression_->value(values, jacobians), H); Augmented<A> argument = expressionA_->augmented(values);
JacobianMap::iterator it = jacobians->begin(); Matrix H;
for (; it != jacobians->end(); ++it) { T t = f_(argument.value(),
it->second = H * it->second; argument.constant() ? none : boost::optional<Matrix&>(H));
} return Augmented<T>(t, H, argument.jacobians());
} else {
value = f_(expression_->value(values), boost::none);
}
return value;
} }
}; };
@ -225,27 +272,25 @@ public:
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/// Binary Expression /// Binary Expression
template<class T, class E1, class E2> template<class T, class A1, class A2>
class BinaryExpression: public ExpressionNode<T> { class BinaryExpression: public ExpressionNode<T> {
public: public:
typedef std::map<Key, Matrix> JacobianMap;
typedef boost::function< typedef boost::function<
T(const E1&, const E2&, boost::optional<Matrix&>, T(const A1&, const A2&, boost::optional<Matrix&>,
boost::optional<Matrix&>)> function; boost::optional<Matrix&>)> function;
private: private:
boost::shared_ptr<ExpressionNode<E1> > expression1_; boost::shared_ptr<ExpressionNode<A1> > expressionA1_;
boost::shared_ptr<ExpressionNode<E2> > expression2_; boost::shared_ptr<ExpressionNode<A2> > expressionA2_;
function f_; function f_;
/// Constructor with a binary function f, and two input arguments /// Constructor with a binary function f, and two input arguments
BinaryExpression(function f, // BinaryExpression(function f, //
const Expression<E1>& e1, const Expression<E2>& e2) : const Expression<A1>& e1, const Expression<A2>& e2) :
expression1_(e1.root()), expression2_(e2.root()), f_(f) { expressionA1_(e1.root()), expressionA2_(e2.root()), f_(f) {
} }
friend class Expression<T> ; friend class Expression<T> ;
@ -258,31 +303,29 @@ public:
/// Return keys that play in this expression /// Return keys that play in this expression
virtual std::set<Key> keys() const { virtual std::set<Key> keys() const {
std::set<Key> keys1 = expression1_->keys(); std::set<Key> keys1 = expressionA1_->keys();
std::set<Key> keys2 = expression2_->keys(); std::set<Key> keys2 = expressionA2_->keys();
keys1.insert(keys2.begin(), keys2.end()); keys1.insert(keys2.begin(), keys2.end());
return keys1; return keys1;
} }
/// Return value and optional derivatives /// Return value
virtual T value(const Values& values, virtual T value(const Values& values) const {
boost::optional<JacobianMap&> jacobians = boost::none) const { using boost::none;
T val; return f_(expressionA1_->value(values), expressionA2_->value(values), none,
if (jacobians) { none);
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<Matrix&>(H1),
terms2.empty() ? boost::none : boost::optional<Matrix&>(H2));
ExpressionNode<T>::add(H1, terms1, *jacobians);
ExpressionNode<T>::add(H2, terms2, *jacobians);
} else {
val = f_(expression1_->value(values), expression2_->value(values),
boost::none, boost::none);
} }
return val;
/// Return value and derivatives
virtual Augmented<T> augmented(const Values& values) const {
using boost::none;
Augmented<A1> argument1 = expressionA1_->augmented(values);
Augmented<A2> argument2 = expressionA2_->augmented(values);
Matrix H1, H2;
T t = f_(argument1.value(), argument2.value(),
argument1.constant() ? none : boost::optional<Matrix&>(H1),
argument2.constant() ? none : boost::optional<Matrix&>(H2));
return Augmented<T>(t, H1, argument1.jacobians(), H2, argument2.jacobians());
} }
}; };
@ -290,25 +333,23 @@ public:
//----------------------------------------------------------------------------- //-----------------------------------------------------------------------------
/// Binary Expression /// Binary Expression
template<class T, class E1, class E2> template<class T, class A1, class A2>
class MethodExpression: public ExpressionNode<T> { class MethodExpression: public ExpressionNode<T> {
public: public:
typedef std::map<Key, Matrix> JacobianMap; typedef T (A1::*method)(const A2&, boost::optional<Matrix&>,
typedef T (E1::*method)(const E2&, boost::optional<Matrix&>,
boost::optional<Matrix&>) const; boost::optional<Matrix&>) const;
private: private:
boost::shared_ptr<ExpressionNode<E1> > expression1_; boost::shared_ptr<ExpressionNode<A1> > expressionA1_;
boost::shared_ptr<ExpressionNode<E2> > expression2_; boost::shared_ptr<ExpressionNode<A2> > expressionA2_;
method f_; method f_;
/// Constructor with a binary function f, and two input arguments /// Constructor with a binary function f, and two input arguments
MethodExpression(const Expression<E1>& e1, method f, const Expression<E2>& e2) : MethodExpression(const Expression<A1>& e1, method f, const Expression<A2>& e2) :
expression1_(e1.root()), expression2_(e2.root()), f_(f) { expressionA1_(e1.root()), expressionA2_(e2.root()), f_(f) {
} }
friend class Expression<T> ; friend class Expression<T> ;
@ -321,31 +362,29 @@ public:
/// Return keys that play in this expression /// Return keys that play in this expression
virtual std::set<Key> keys() const { virtual std::set<Key> keys() const {
std::set<Key> keys1 = expression1_->keys(); std::set<Key> keys1 = expressionA1_->keys();
std::set<Key> keys2 = expression2_->keys(); std::set<Key> keys2 = expressionA2_->keys();
keys1.insert(keys2.begin(), keys2.end()); keys1.insert(keys2.begin(), keys2.end());
return keys1; return keys1;
} }
/// Return value and optional derivatives /// Return value
virtual T value(const Values& values, virtual T value(const Values& values) const {
boost::optional<JacobianMap&> jacobians = boost::none) const { using boost::none;
T val; return (expressionA1_->value(values).*(f_))(expressionA2_->value(values),
if (jacobians) { none, none);
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<Matrix&>(H1),
terms2.empty() ? boost::none : boost::optional<Matrix&>(H2));
ExpressionNode<T>::add(H1, terms1, *jacobians);
ExpressionNode<T>::add(H2, terms2, *jacobians);
} else {
val = (expression1_->value(values).*(f_))(expression2_->value(values),
boost::none, boost::none);
} }
return val;
/// Return value and derivatives
virtual Augmented<T> augmented(const Values& values) const {
using boost::none;
Augmented<A1> argument1 = expressionA1_->augmented(values);
Augmented<A2> argument2 = expressionA2_->augmented(values);
Matrix H1, H2;
T t = (argument1.value().*(f_))(argument2.value(),
argument1.constant() ? none : boost::optional<Matrix&>(H1),
argument2.constant() ? none : boost::optional<Matrix&>(H2));
return Augmented<T>(t, H1, argument1.jacobians(), H2, argument2.jacobians());
} }
}; };

10
gtsam_unstable/nonlinear/Expression.h
View File

@ -83,9 +83,13 @@ public:
} }
/// Return value and optional derivatives /// Return value and optional derivatives
T value(const Values& values, T value(const Values& values) const {
boost::optional<std::map<Key, Matrix>&> jacobians = boost::none) const { return root_->value(values);
return root_->value(values, jacobians); }
/// Return value and derivatives
Augmented<T> augmented(const Values& values) const {
return root_->augmented(values);
} }
const boost::shared_ptr<ExpressionNode<T> >& root() const { const boost::shared_ptr<ExpressionNode<T> >& root() const {

24
gtsam_unstable/nonlinear/tests/testExpression.cpp
View File

@ -38,6 +38,30 @@ Point2 uncalibrate(const CAL& K, const Point2& p, boost::optional<Matrix&> Dcal,
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Expression, constant) {
Expression<Rot3> R(Rot3::identity());
Values values;
Augmented<Rot3> a = R.augmented(values);
EXPECT(assert_equal(Rot3::identity(), a.value()));
JacobianMap expected;
EXPECT(a.jacobians() == expected);
}
/* ************************************************************************* */
TEST(Expression, leaf) {
Expression<Rot3> R(100);
Values values;
values.insert(100,Rot3::identity());
Augmented<Rot3> 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(Expression, test) {
// Test Constant expression // Test Constant expression