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

Only ExpressionFactor needs dimensions! Also, add dimensions at construction -> speeds up linearize.

release/4.3a0
dellaert 2014-10-16 11:15:47 +02:00
parent 3413b98331
commit 944422e295
2 changed files with 39 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
gtsam_unstable/nonlinear/Expression.h
View File

@ -34,51 +34,42 @@ class Expression {
private:
// Paul's trick shared pointer, polymorphic root of entire expression tree
const boost::shared_ptr<ExpressionNode<T> > root_;
// Fixed dimensions: an Expression is assumed unmutable
const std::vector<size_t> dimensions_;
boost::shared_ptr<ExpressionNode<T> > root_;
public:
// Construct a constant expression
Expression(const T& value) :
root_(new ConstantExpression<T>(value)), //
dimensions_(root_->dimensions()) {
root_(new ConstantExpression<T>(value)) {
}
// Construct a leaf expression, with Key
Expression(const Key& key) :
root_(new LeafExpression<T>(key)), //
dimensions_(root_->dimensions()) {
root_(new LeafExpression<T>(key)) {
}
// Construct a leaf expression, with Symbol
Expression(const Symbol& symbol) :
root_(new LeafExpression<T>(symbol)), //
dimensions_(root_->dimensions()) {
root_(new LeafExpression<T>(symbol)) {
}
// Construct a leaf expression, creating Symbol
Expression(unsigned char c, size_t j) :
root_(new LeafExpression<T>(Symbol(c, j))), //
dimensions_(root_->dimensions()) {
root_(new LeafExpression<T>(Symbol(c, j))) {
}
/// Construct a nullary method expression
template<typename A>
Expression(const Expression<A>& expression,
T (A::*method)(typename Optional<T, A>::type) const) :
root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)), //
dimensions_(root_->dimensions()) {
root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)) {
}
/// 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)), //
dimensions_(root_->dimensions()) {
root_(new UnaryExpression<T, A>(function, expression)) {
}
/// Construct a unary method expression
@ -89,8 +80,7 @@ public:
const Expression<A2>& expression2) :
root_(
new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
expression1, expression2)), //
dimensions_(root_->dimensions()) {
expression1, expression2)) {
}
/// Construct a binary function expression
@ -98,8 +88,7 @@ public:
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)), //
dimensions_(root_->dimensions()) {
new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
}
/// Construct a ternary function expression
@ -109,8 +98,7 @@ public:
const Expression<A3>& expression3) :
root_(
new TernaryExpression<T, A1, A2, A3>(function, expression1,
expression2, expression3)), //
dimensions_(root_->dimensions()) {
expression2, expression3)) {
}
/// Return keys that play in this expression
@ -119,8 +107,8 @@ public:
}
/// Return dimensions for each argument, must be same order as keys
const std::vector<size_t>& dimensions() const {
return dimensions_;
std::vector<size_t> dimensions() const {
return root_->dimensions();
}
// Return size needed for memory buffer in traceExecution

46
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -30,8 +30,10 @@ namespace gtsam {
template<class T>
class ExpressionFactor: public NoiseModelFactor {
const T measurement_;
const Expression<T> expression_;
T measurement_; ///< the measurement to be compared with the expression
Expression<T> expression_; ///< the expression that is AD enabled
std::vector<size_t> dimensions_; ///< dimensions of the Jacobian matrices
size_t augmentedCols_; ///< total number of columns + 1 (for RHS)
public:
@ -45,6 +47,19 @@ public:
if (noiseModel->dim() != T::dimension)
throw std::invalid_argument(
"ExpressionFactor was created with a NoiseModel of incorrect dimension.");
// Get dimensions of Jacobian matrices
// An Expression is assumed unmutable, so we do this now
dimensions_ = expression_.dimensions();
// Add sizes to know how much memory to allocate on stack in linearize
augmentedCols_ = std::accumulate(dimensions_.begin(), dimensions_.end(), 1);
#ifdef DEBUG_ExpressionFactor
BOOST_FOREACH(size_t d, dimensions_)
std::cout << d << " ";
std::cout << " -> " << T::dimension << "x" << augmentedCols_ << std::endl;
#endif
}
/**
@ -58,16 +73,14 @@ public:
// H should be pre-allocated
assert(H->size()==size());
// Get dimensions of Jacobian matrices
std::vector<size_t> dims = expression_.dimensions();
// Create and zero out blocks to be passed to expression_
JacobianMap blocks;
for(DenseIndex i=0;i<size();i++) {
for (DenseIndex i = 0; i < size(); i++) {
Matrix& Hi = H->at(i);
Hi.resize(T::dimension, dims[i]);
Hi.resize(T::dimension, dimensions_[i]);
Hi.setZero(); // zero out
Eigen::Block<Matrix> block = Hi.block(0,0,T::dimension, dims[i]);
Eigen::Block<Matrix> block = Hi.block(0, 0, T::dimension,
dimensions_[i]);
blocks.insert(std::make_pair(keys_[i], block));
}
@ -81,23 +94,18 @@ public:
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// Construct VerticalBlockMatrix and views into it
// Get dimensions of Jacobian matrices
std::vector<size_t> dims = expression_.dimensions();
// Allocate memory on stack and create a view on it (saves a malloc)
size_t m1 = std::accumulate(dims.begin(),dims.end(),1);
double memory[T::dimension*m1];
Eigen::Map<Eigen::Matrix<double,T::dimension,Eigen::Dynamic> > matrix(memory,T::dimension,m1);
double memory[T::dimension * augmentedCols_];
Eigen::Map<Eigen::Matrix<double, T::dimension, Eigen::Dynamic> > //
matrix(memory, T::dimension, augmentedCols_);
matrix.setZero(); // zero out
// Construct block matrix, is of right size but un-initialized
VerticalBlockMatrix Ab(dims, matrix, true);
VerticalBlockMatrix Ab(dimensions_, matrix, true);
// Create blocks to be passed to expression_
// Create blocks into Ab_ to be passed to expression_
JacobianMap blocks;
for(DenseIndex i=0;i<size();i++)
for (DenseIndex i = 0; i < size(); i++)
blocks.insert(std::make_pair(keys_[i], Ab(i)));
// Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, blocks);