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

Moved a lot of things to Expression-inl.h, made interface cleaner and more encapsulated. With Jing on skype...

release/4.3a0
dellaert 2015-05-04 10:06:55 -07:00
parent 0a19c078e7
commit e20a704a96
2 changed files with 294 additions and 183 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

293
gtsam/nonlinear/Expression-inl.h
View File

@ -40,7 +40,9 @@ class ExpressionFactorBinaryTest;
namespace gtsam {
const unsigned TraceAlignment = 16;
//-----------------------------------------------------------------------------
// ExecutionTrace.h
//-----------------------------------------------------------------------------
template<typename T>
T & upAlign(T & value, unsigned requiredAlignment = TraceAlignment) {
@ -60,30 +62,6 @@ T upAligned(T value, unsigned requiredAlignment = TraceAlignment) {
return upAlign(value, requiredAlignment);
}
template<typename T>
class Expression;
/**
* Expressions are designed to write their derivatives into an already allocated
* Jacobian of the correct size, of type VerticalBlockMatrix.
* The JacobianMap provides a mapping from keys to the underlying blocks.
*/
class JacobianMap {
const FastVector<Key>& keys_;
VerticalBlockMatrix& Ab_;
public:
JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab) :
keys_(keys), Ab_(Ab) {
}
/// Access via key
VerticalBlockMatrix::Block operator()(Key key) {
FastVector<Key>::const_iterator it = std::find(keys_.begin(), keys_.end(),
key);
DenseIndex block = it - keys_.begin();
return Ab_(block);
}
};
//-----------------------------------------------------------------------------
namespace internal {
@ -215,12 +193,10 @@ public:
typedef ExecutionTrace<T> type;
};
/// Storage type for the execution trace.
/// It enforces the proper alignment in a portable way.
/// Provide a traceSize() sized array of this type to traceExecution as traceStorage.
typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage;
//-----------------------------------------------------------------------------
// ExpressionNode.h
//-----------------------------------------------------------------------------
/**
* Expression node. The superclass for objects that do the heavy lifting
* An Expression<T> has a pointer to an ExpressionNode<T> underneath
@ -247,10 +223,12 @@ public:
}
/// Streaming
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os,
GTSAM_EXPORT
friend std::ostream &operator<<(std::ostream &os,
const ExpressionNode& node) {
os << "Expression of type " << typeid(T).name();
if (node.traceSize_>0) os << ", trace size = " << node.traceSize_;
if (node.traceSize_ > 0)
os << ", trace size = " << node.traceSize_;
os << "\n";
return os;
}
@ -307,11 +285,10 @@ public:
}
};
//-----------------------------------------------------------------------------
/// Leaf Expression, if no chart is given, assume default chart and value_type is just the plain value
template<typename T>
class LeafExpression : public ExpressionNode<T> {
class LeafExpression: public ExpressionNode<T> {
typedef T value_type;
/// The key into values
@ -412,15 +389,7 @@ public:
/// meta-function to generate fixed-size JacobianTA type
template<class T, class A>
struct Jacobian {
typedef Eigen::Matrix<double, traits<T>::dimension,
traits<A>::dimension> type;
};
/// meta-function to generate JacobianTA optional reference
template<class T, class A>
struct MakeOptionalJacobian {
typedef OptionalJacobian<traits<T>::dimension,
traits<A>::dimension> type;
typedef Eigen::Matrix<double, traits<T>::dimension, traits<A>::dimension> type;
};
/**
@ -524,8 +493,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
/// Given df/dT, multiply in dT/dA and continue reverse AD process
// Cols is always known at compile time
template<typename SomeMatrix>
void reverseAD4(const SomeMatrix & dFdT,
JacobianMap& jacobians) const {
void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
Base::Record::reverseAD4(dFdT, jacobians);
This::trace.reverseAD1(dFdT * This::dTdA, jacobians);
}
@ -629,16 +597,9 @@ struct FunctionalNode {
/// Unary Function Expression
template<class T, class A1>
class UnaryExpression : public ExpressionNode<T> {
typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
public:
typedef boost::function<T(const A1&, OJ1)> Function;
private:
class UnaryExpression: public ExpressionNode<T> {
typedef typename UnaryFunction<T,A1>::type Function;
Function function_;
boost::shared_ptr<ExpressionNode<A1> > expression1_;
@ -660,6 +621,20 @@ public:
return function_(this->expression1_->value(values), boost::none);
}
/// Return keys that play in this expression
virtual std::set<Key> keys() const {
std::set<Key> keys; // = Base::keys();
std::set<Key> myKeys = this->expression1_->keys();
keys.insert(myKeys.begin(), myKeys.end());
return keys;
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, int>& map) const {
// Base::dims(map);
this->expression1_->dims(map);
}
// Inner Record Class
// The reason we inherit from JacobianTrace<T, A, N> is because we can then
// case to this unique signature to retrieve the value/trace at any level
@ -706,8 +681,7 @@ public:
/// Given df/dT, multiply in dT/dA and continue reverse AD process
// Cols is always known at compile time
template<typename SomeMatrix>
void reverseAD4(const SomeMatrix & dFdT,
JacobianMap& jacobians) const {
void reverseAD4(const SomeMatrix & dFdT, JacobianMap& jacobians) const {
This::trace.reverseAD1(dFdT * This::dTdA, jacobians);
}
};
@ -748,7 +722,6 @@ public:
ExecutionTraceStorage* traceStorage) const {
Record* record = this->trace(values, traceStorage);
record->print("record: ");
trace.setFunction(record);
return function_(record->template value<A1, 1>(),
@ -760,20 +733,15 @@ public:
/// Binary Expression
template<class T, class A1, class A2>
class BinaryExpression:
public FunctionalNode<T, boost::mpl::vector<A1, A2> >::type {
typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
typedef typename MakeOptionalJacobian<T, A2>::type OJ2;
class BinaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2> >::type {
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
public:
typedef boost::function<T(const A1&, const A2&, OJ1, OJ2)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2> >::type Base;
typedef typename Base::Record Record;
private:
typedef typename BinaryFunction<T,A1,A2>::type Function;
Function function_;
/// Constructor with a ternary function f, and three input arguments
@ -801,14 +769,14 @@ public:
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
ExecutionTraceStorage* traceStorage) const {
Record* record = Base::trace(values, traceStorage);
trace.setFunction(record);
return function_(record->template value<A1, 1>(),
record->template value<A2,2>(), record->template jacobian<A1, 1>(),
record->template jacobian<A2, 2>());
record->template value<A2,2>(), record->template jacobian<A1, 1>(),
record->template jacobian<A2, 2>());
}
};
@ -816,21 +784,14 @@ public:
/// Ternary Expression
template<class T, class A1, class A2, class A3>
class TernaryExpression:
public FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type {
class TernaryExpression: public FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type {
typedef typename MakeOptionalJacobian<T, A1>::type OJ1;
typedef typename MakeOptionalJacobian<T, A2>::type OJ2;
typedef typename MakeOptionalJacobian<T, A3>::type OJ3;
public:
typedef boost::function<T(const A1&, const A2&, const A3&, OJ1, OJ2, OJ3)> Function;
typedef typename FunctionalNode<T, boost::mpl::vector<A1, A2, A3> >::type Base;
typedef typename Base::Record Record;
private:
typedef typename TernaryFunction<T,A1,A2,A3>::type Function;
Function function_;
/// Constructor with a ternary function f, and three input arguments
@ -860,17 +821,187 @@ public:
/// Construct an execution trace for reverse AD
virtual T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
ExecutionTraceStorage* traceStorage) const {
Record* record = Base::trace(values, traceStorage);
trace.setFunction(record);
return function_(
record->template value<A1, 1>(), record->template value<A2, 2>(),
record->template value<A3, 3>(), record->template jacobian<A1, 1>(),
record->template jacobian<A2, 2>(), record->template jacobian<A3, 3>());
record->template value<A1, 1>(), record->template value<A2, 2>(),
record->template value<A3, 3>(), record->template jacobian<A1, 1>(),
record->template jacobian<A2, 2>(), record->template jacobian<A3, 3>());
}
};
//-----------------------------------------------------------------------------
// Esxpression-inl.h
//-----------------------------------------------------------------------------
/// Print
template<typename T>
void Expression<T>::print(const std::string& s) const {
std::cout << s << *root_ << std::endl;
}
// Construct a constant expression
template<typename T>
Expression<T>::Expression(const T& value) :
root_(new ConstantExpression<T>(value)) {
}
// Construct a leaf expression, with Key
template<typename T>
Expression<T>::Expression(const Key& key) :
root_(new LeafExpression<T>(key)) {
}
// Construct a leaf expression, with Symbol
template<typename T>
Expression<T>::Expression(const Symbol& symbol) :
root_(new LeafExpression<T>(symbol)) {
}
// Construct a leaf expression, creating Symbol
template<typename T>
Expression<T>::Expression(unsigned char c, size_t j) :
root_(new LeafExpression<T>(Symbol(c, j))) {
}
/// Construct a nullary method expression
template<typename T>
template<typename A>
Expression<T>::Expression(const Expression<A>& expression,
T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)) {
}
/// Construct a unary function expression
template<typename T>
template<typename A>
Expression<T>::Expression(typename UnaryFunction<T, A>::type function,
const Expression<A>& expression) :
root_(new UnaryExpression<T, A>(function, expression)) {
}
/// Construct a unary method expression
template<typename T>
template<typename A1, typename A2>
Expression<T>::Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type) const,
const Expression<A2>& expression2) :
root_(
new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
expression1, expression2)) {
}
/// Construct a binary function expression
template<typename T>
template<typename A1, typename A2>
Expression<T>::Expression(typename BinaryFunction<T, A1, A2>::type function,
const Expression<A1>& expression1, const Expression<A2>& expression2) :
root_(new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
}
/// Construct a binary method expression
template<typename T>
template<typename A1, typename A2, typename A3>
Expression<T>::Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, const A3&,
typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type,
typename MakeOptionalJacobian<T, A3>::type) const,
const Expression<A2>& expression2, const Expression<A3>& expression3) :
root_(
new TernaryExpression<T, A1, A2, A3>(
boost::bind(method, _1, _2, _3, _4, _5, _6), expression1,
expression2, expression3)) {
}
/// Construct a ternary function expression
template<typename T>
template<typename A1, typename A2, typename A3>
Expression<T>::Expression(
typename TernaryFunction<T, A1, A2, A3>::type function,
const Expression<A1>& expression1, const Expression<A2>& expression2,
const Expression<A3>& expression3) :
root_(
new TernaryExpression<T, A1, A2, A3>(function, expression1, expression2,
expression3)) {
}
/// private version that takes keys and dimensions, returns derivatives
template<typename T>
T Expression<T>::value(const Values& values, const FastVector<Key>& keys,
const FastVector<int>& dims, std::vector<Matrix>& H) const {
// H should be pre-allocated
assert(H.size()==keys.size());
// Pre-allocate and zero VerticalBlockMatrix
static const int Dim = traits<T>::dimension;
VerticalBlockMatrix Ab(dims, Dim);
Ab.matrix().setZero();
JacobianMap jacobianMap(keys, Ab);
// Call unsafe version
T result = value(values, jacobianMap);
// Copy blocks into the vector of jacobians passed in
for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
H[i] = Ab(i);
return result;
}
template<typename T>
T Expression<T>::traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
return root_->traceExecution(values, trace, traceStorage);
}
template<typename T>
T Expression<T>::value(const Values& values, JacobianMap& jacobians) const {
// The following piece of code is absolutely crucial for performance.
// We allocate a block of memory on the stack, which can be done at runtime
// with modern C++ compilers. The traceExecution then fills this memory
// with an execution trace, made up entirely of "Record" structs, see
// the FunctionalNode class in expression-inl.h
size_t size = traceSize();
// Windows does not support variable length arrays, so memory must be dynamically
// allocated on Visual Studio. For more information see the issue below
// https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
#ifdef _MSC_VER
ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size];
#else
ExecutionTraceStorage traceStorage[size];
#endif
ExecutionTrace<T> trace;
T value(this->traceExecution(values, trace, traceStorage));
trace.startReverseAD1(jacobians);
#ifdef _MSC_VER
delete[] traceStorage;
#endif
return value;
}
// JacobianMap:
JacobianMap::JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab) :
keys_(keys), Ab_(Ab) {
}
VerticalBlockMatrix::Block JacobianMap::operator()(Key key) {
FastVector<Key>::const_iterator it = std::find(keys_.begin(), keys_.end(),
key);
DenseIndex block = it - keys_.begin();
return Ab_(block);
}
//-----------------------------------------------------------------------------
}

184
gtsam/nonlinear/Expression.h
View File

@ -19,9 +19,10 @@
#pragma once
#include <gtsam/nonlinear/Expression-inl.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/OptionalJacobian.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <boost/bind.hpp>
#include <boost/range/adaptor/map.hpp>
@ -31,9 +32,46 @@ class ExpressionFactorShallowTest;
namespace gtsam {
// Forward declare
// Forward declares
class Values;
template<typename T> class ExecutionTrace;
template<typename T> class ExpressionNode;
template<typename T> class ExpressionFactor;
// A JacobianMap is the primary mechanism by which derivatives are returned.
// For clarity, it is forward declared here but implemented at the end of this header.
class JacobianMap;
// Expressions wrap trees of functions that can evaluate their own derivatives.
// The meta-functions below provide a handy to specify the type of those functions
template<class T, class A1>
struct UnaryFunction {
typedef boost::function<
T(const A1&, typename MakeOptionalJacobian<T, A1>::type)> type;
};
template<class T, class A1, class A2>
struct BinaryFunction {
typedef boost::function<
T(const A1&, const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type)> type;
};
template<class T, class A1, class A2, class A3>
struct TernaryFunction {
typedef boost::function<
T(const A1&, const A2&, const A3&,
typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type,
typename MakeOptionalJacobian<T, A3>::type)> type;
};
/// Storage type for the execution trace.
/// It enforces the proper alignment in a portable way.
/// Provide a traceSize() sized array of this type to traceExecution as traceStorage.
const unsigned TraceAlignment = 16;
typedef boost::aligned_storage<1, TraceAlignment>::type ExecutionTraceStorage;
/**
* Expression class that supports automatic differentiation
*/
@ -53,85 +91,56 @@ private:
public:
/// Print
void print(const std::string& s) const {
std::cout << s << *root_ << std::endl;
}
void print(const std::string& s) const;
// Construct a constant expression
Expression(const T& value) :
root_(new ConstantExpression<T>(value)) {
}
/// Construct a constant expression
Expression(const T& value);
// Construct a leaf expression, with Key
Expression(const Key& key) :
root_(new LeafExpression<T>(key)) {
}
/// Construct a leaf expression, with Key
Expression(const Key& key);
// Construct a leaf expression, with Symbol
Expression(const Symbol& symbol) :
root_(new LeafExpression<T>(symbol)) {
}
/// Construct a leaf expression, with Symbol
Expression(const Symbol& symbol);
// Construct a leaf expression, creating Symbol
Expression(unsigned char c, size_t j) :
root_(new LeafExpression<T>(Symbol(c, j))) {
}
/// Construct a leaf expression, creating Symbol
Expression(unsigned char c, size_t j);
/// Construct a nullary method expression
template<typename A>
Expression(const Expression<A>& expression,
T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const) :
root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)) {
}
T (A::*method)(typename MakeOptionalJacobian<T, A>::type) const);
/// Construct a unary function expression
template<typename A>
Expression(typename UnaryExpression<T, A>::Function function,
const Expression<A>& expression) :
root_(new UnaryExpression<T, A>(function, expression)) {
}
Expression(typename UnaryFunction<T, A>::type function,
const Expression<A>& expression);
/// Construct a unary method expression
template<typename A1, typename A2>
Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type) const,
const Expression<A2>& expression2) :
root_(
new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
expression1, expression2)) {
}
const Expression<A2>& expression2);
/// Construct a binary function expression
template<typename A1, typename A2>
Expression(typename BinaryExpression<T, A1, A2>::Function function,
const Expression<A1>& expression1, const Expression<A2>& expression2) :
root_(new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
}
Expression(typename BinaryFunction<T, A1, A2>::type function,
const Expression<A1>& expression1, const Expression<A2>& expression2);
/// Construct a binary method expression
template<typename A1, typename A2, typename A3>
Expression(const Expression<A1>& expression1,
T (A1::*method)(const A2&, const A3&,
typename TernaryExpression<T, A1, A2, A3>::OJ1,
typename TernaryExpression<T, A1, A2, A3>::OJ2,
typename TernaryExpression<T, A1, A2, A3>::OJ3) const,
const Expression<A2>& expression2, const Expression<A3>& expression3) :
root_(
new TernaryExpression<T, A1, A2, A3>(
boost::bind(method, _1, _2, _3, _4, _5, _6), expression1,
expression2, expression3)) {
}
typename MakeOptionalJacobian<T, A1>::type,
typename MakeOptionalJacobian<T, A2>::type,
typename MakeOptionalJacobian<T, A3>::type) const,
const Expression<A2>& expression2, const Expression<A3>& expression3);
/// Construct a ternary function expression
template<typename A1, typename A2, typename A3>
Expression(typename TernaryExpression<T, A1, A2, A3>::Function function,
Expression(typename TernaryFunction<T, A1, A2, A3>::type function,
const Expression<A1>& expression1, const Expression<A2>& expression2,
const Expression<A3>& expression3) :
root_(
new TernaryExpression<T, A1, A2, A3>(function, expression1,
expression2, expression3)) {
}
const Expression<A3>& expression3);
/// Return root
const boost::shared_ptr<ExpressionNode<T> >& root() const {
@ -195,65 +204,18 @@ private:
/// private version that takes keys and dimensions, returns derivatives
T value(const Values& values, const FastVector<Key>& keys,
const FastVector<int>& dims, std::vector<Matrix>& H) const {
// H should be pre-allocated
assert(H.size()==keys.size());
// Pre-allocate and zero VerticalBlockMatrix
static const int Dim = traits<T>::dimension;
VerticalBlockMatrix Ab(dims, Dim);
Ab.matrix().setZero();
JacobianMap jacobianMap(keys, Ab);
// Call unsafe version
T result = value(values, jacobianMap);
// Copy blocks into the vector of jacobians passed in
for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
H[i] = Ab(i);
return result;
}
const FastVector<int>& dims, std::vector<Matrix>& H) const;
/// trace execution, very unsafe
T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
return root_->traceExecution(values, trace, traceStorage);
}
ExecutionTraceStorage* traceStorage) const;
/**
* @brief Return value and derivatives, reverse AD version
* This very unsafe method needs a JacobianMap with correctly allocated
* and initialized VerticalBlockMatrix, hence is declared private.
*/
T value(const Values& values, JacobianMap& jacobians) const {
// The following piece of code is absolutely crucial for performance.
// We allocate a block of memory on the stack, which can be done at runtime
// with modern C++ compilers. The traceExecution then fills this memory
// with an execution trace, made up entirely of "Record" structs, see
// the FunctionalNode class in expression-inl.h
size_t size = traceSize();
// Windows does not support variable length arrays, so memory must be dynamically
// allocated on Visual Studio. For more information see the issue below
// https://bitbucket.org/gtborg/gtsam/issue/178/vlas-unsupported-in-visual-studio
#ifdef _MSC_VER
ExecutionTraceStorage* traceStorage = new ExecutionTraceStorage[size];
#else
ExecutionTraceStorage traceStorage[size];
#endif
ExecutionTrace<T> trace;
T value(traceExecution(values, trace, traceStorage));
trace.startReverseAD1(jacobians);
#ifdef _MSC_VER
delete[] traceStorage;
#endif
return value;
}
T value(const Values& values, JacobianMap& jacobians) const;
// be very selective on who can access these private methods:
friend class ExpressionFactor<T> ;
@ -261,6 +223,22 @@ private:
};
// Expressions are designed to write their derivatives into an already allocated
// Jacobian of the correct size, of type VerticalBlockMatrix.
// The JacobianMap provides a mapping from keys to the underlying blocks.
class JacobianMap {
private:
const FastVector<Key>& keys_;
VerticalBlockMatrix& Ab_;
public:
/// Construct a JacobianMap for writing into a VerticalBlockMatrix Ab
JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab);
/// Access blocks of via key
VerticalBlockMatrix::Block operator()(Key key);
};
// http://stackoverflow.com/questions/16260445/boost-bind-to-operator
template<class T>
struct apply_compose {
@ -292,3 +270,5 @@ std::vector<Expression<T> > createUnknowns(size_t n, char c, size_t start = 0) {
}
#include <gtsam/nonlinear/Expression-inl.h>