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

Merged in feature/fixIssue165 (pull request #48) 

Safer public interface
release/4.3a0
Frank Dellaert 2014-11-25 16:13:50 +01:00
parent 7b1a9ba371 dc4c0b54c0
commit 50c93ec954
5 changed files with 134 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -252,7 +252,7 @@ public:
}
/// Return dimensions for each argument, as a map
virtual void dims(std::map<Key, size_t>& map) const {
virtual void dims(std::map<Key, int>& map) const {
}
// Return size needed for memory buffer in traceExecution
@ -324,7 +324,7 @@ public:
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, size_t>& map) const {
virtual void dims(std::map<Key, int>& map) const {
// get dimension from the chart; only works for fixed dimension charts
map[key_] = traits::dimension<Chart>::value;
}
@ -371,7 +371,7 @@ public:
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, size_t>& map) const {
virtual void dims(std::map<Key, int>& map) const {
map[key_] = traits::dimension<T>::value;
}
@ -523,7 +523,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
}
/// Return dimensions for each argument
virtual void dims(std::map<Key, size_t>& map) const {
virtual void dims(std::map<Key, int>& map) const {
Base::dims(map);
This::expression->dims(map);
}

115
gtsam_unstable/nonlinear/Expression.h
View File

@ -20,17 +20,31 @@
#pragma once
#include "Expression-inl.h"
#include <gtsam/base/FastVector.h>
#include <gtsam/inference/Symbol.h>
#include <boost/bind.hpp>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm.hpp>
class ExpressionFactorShallowTest;
namespace gtsam {
// Forward declare
template<typename T> class ExpressionFactor;
/**
* Expression class that supports automatic differentiation
*/
template<typename T>
class Expression {
public:
/// Define type so we can apply it as a meta-function
typedef Expression<T> type;
private:
// Paul's trick shared pointer, polymorphic root of entire expression tree
@ -55,7 +69,7 @@ public:
// Construct a leaf expression, creating Symbol
Expression(unsigned char c, size_t j) :
root_(new LeafExpression<T>(Symbol(c, j))) {
root_(new LeafExpression<T>(Symbol(c, j))) {
}
/// Construct a nullary method expression
@ -87,8 +101,7 @@ public:
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)) {
root_(new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
}
/// Construct a ternary function expression
@ -101,14 +114,9 @@ public:
expression2, expression3)) {
}
/// Return keys that play in this expression
std::set<Key> keys() const {
return root_->keys();
}
/// Return dimensions for each argument, as a map
void dims(std::map<Key, size_t>& map) const {
root_->dims(map);
/// Return root
const boost::shared_ptr<ExpressionNode<T> >& root() const {
return root_;
}
// Return size needed for memory buffer in traceExecution
@ -116,13 +124,81 @@ public:
return root_->traceSize();
}
/// trace execution, very unsafe, for testing purposes only //TODO this is not only used for testing, but in value() below!
/// Return keys that play in this expression
std::set<Key> keys() const {
return root_->keys();
}
/// Return dimensions for each argument, as a map
void dims(std::map<Key, int>& map) const {
root_->dims(map);
}
/**
* @brief Return value and optional derivatives, reverse AD version
* Notes: this is not terribly efficient, and H should have correct size.
* The order of the Jacobians is same as keys in either keys() or dims()
*/
T value(const Values& values, boost::optional<std::vector<Matrix>&> H =
boost::none) const {
if (H) {
// Call private version that returns derivatives in H
KeysAndDims pair = keysAndDims();
return value(values, pair.first, pair.second, *H);
} else
// no derivatives needed, just return value
return root_->value(values);
}
private:
/// Vaguely unsafe keys and dimensions in same order
typedef std::pair<FastVector<Key>, FastVector<int> > KeysAndDims;
KeysAndDims keysAndDims() const {
std::map<Key, int> map;
dims(map);
size_t n = map.size();
KeysAndDims pair = std::make_pair(FastVector<Key>(n), FastVector<int>(n));
boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
boost::copy(map | boost::adaptors::map_values, pair.second.begin());
return pair;
}
/// 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::dimension<T>::value;
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;
}
/// trace execution, very unsafe
T traceExecution(const Values& values, ExecutionTrace<T>& trace,
ExecutionTraceStorage* traceStorage) const {
return root_->traceExecution(values, trace, traceStorage);
}
/// Return value and derivatives, reverse AD version
/**
* @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
@ -137,17 +213,10 @@ public:
return value;
}
/// Return value
T value(const Values& values) const {
return root_->value(values);
}
// be very selective on who can access these private methods:
friend class ExpressionFactor<T> ;
friend class ::ExpressionFactorShallowTest;
const boost::shared_ptr<ExpressionNode<T> >& root() const {
return root_;
}
/// Define type so we can apply it as a meta-function
typedef Expression<T> type;
};
// http://stackoverflow.com/questions/16260445/boost-bind-to-operator

57
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -22,8 +22,6 @@
#include <gtsam_unstable/nonlinear/Expression.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <boost/range/adaptor/map.hpp>
#include <boost/range/algorithm.hpp>
#include <numeric>
namespace gtsam {
@ -36,7 +34,7 @@ class ExpressionFactor: public NoiseModelFactor {
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
FastVector<int> dims_; ///< dimensions of the Jacobian matrices
size_t augmentedCols_; ///< total number of columns + 1 (for RHS)
static const int Dim = traits::dimension<T>::value;
@ -54,23 +52,15 @@ public:
"ExpressionFactor was created with a NoiseModel of incorrect dimension.");
noiseModel_ = noiseModel;
// Get dimensions of Jacobian matrices
// Get keys and dimensions for Jacobian matrices
// An Expression is assumed unmutable, so we do this now
std::map<Key, size_t> map;
expression_.dims(map);
size_t n = map.size();
keys_.resize(n);
boost::copy(map | boost::adaptors::map_keys, keys_.begin());
dimensions_.resize(n);
boost::copy(map | boost::adaptors::map_values, dimensions_.begin());
boost::tie(keys_, dims_) = expression_.keysAndDims();
// Add sizes to know how much memory to allocate on stack in linearize
augmentedCols_ = std::accumulate(dimensions_.begin(), dimensions_.end(), 1);
augmentedCols_ = std::accumulate(dims_.begin(), dims_.end(), 1);
#ifdef DEBUG_ExpressionFactor
BOOST_FOREACH(size_t d, dimensions_)
BOOST_FOREACH(size_t d, dims_)
std::cout << d << " ";
std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl;
#endif
@ -86,32 +76,15 @@ public:
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
if (H) {
// H should be pre-allocated
assert(H->size()==size());
VerticalBlockMatrix Ab(dimensions_, Dim);
// Wrap keys and VerticalBlockMatrix into structure passed to expression_
JacobianMap map(keys_, Ab);
Ab.matrix().setZero();
// Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, map); // <<< Reverse AD happens here !
// Copy blocks into the vector of jacobians passed in
for (DenseIndex i = 0; i < static_cast<DenseIndex>(size()); i++)
H->at(i) = Ab(i);
const T value = expression_.value(x, keys_, dims_, *H);
return chart.local(measurement_, value);
} else {
const T& value = expression_.value(x);
const T value = expression_.value(x);
return chart.local(measurement_, value);
}
}
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
// Only linearize if the factor is active
if (!active(x))
return boost::shared_ptr<JacobianFactor>();
@ -120,24 +93,28 @@ public:
// In case noise model is constrained, we need to provide a noise model
bool constrained = noiseModel_->is_constrained();
boost::shared_ptr<JacobianFactor> factor(
constrained ? new JacobianFactor(keys_, dimensions_, Dim,
constrained ? new JacobianFactor(keys_, dims_, Dim,
boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()) :
new JacobianFactor(keys_, dimensions_, Dim));
new JacobianFactor(keys_, dims_, Dim));
// Wrap keys and VerticalBlockMatrix into structure passed to expression_
VerticalBlockMatrix& Ab = factor->matrixObject();
JacobianMap map(keys_, Ab);
JacobianMap jacobianMap(keys_, Ab);
// Zero out Jacobian so we can simply add to it
Ab.matrix().setZero();
// Evaluate error to get Jacobians and RHS vector b
T value = expression_.value(x, map); // <<< Reverse AD happens here !
// Get value and Jacobians, writing directly into JacobianFactor
T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here !
// Evaluate error and set RHS vector b
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
Ab(size()).col(0) = -chart.local(measurement_, value);
// Whiten the corresponding system, Ab already contains RHS
Vector dummy(Dim);
noiseModel_->WhitenSystem(Ab.matrix(),dummy);
noiseModel_->WhitenSystem(Ab.matrix(), dummy);
return factor;
}

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

@ -114,22 +114,20 @@ TEST(Expression, NullaryMethod) {
Values values;
values.insert(67, Point3(3, 4, 5));
// Pre-allocate JacobianMap
FastVector<Key> keys;
keys.push_back(67);
FastVector<int> dims;
dims.push_back(3);
VerticalBlockMatrix Ab(dims, 1);
JacobianMap map(keys, Ab);
// Check dims as map
std::map<Key, int> map;
norm.dims(map);
LONGS_EQUAL(1,map.size());
// Get value and Jacobian
double actual = norm.value(values, map);
// Get value and Jacobians
std::vector<Matrix> H(1);
double actual = norm.value(values, H);
// Check all
EXPECT(actual == sqrt(50));
Matrix expected(1, 3);
expected << 3.0 / sqrt(50.0), 4.0 / sqrt(50.0), 5.0 / sqrt(50.0);
EXPECT(assert_equal(expected,Ab(0)));
EXPECT(assert_equal(expected,H[0]));
}
/* ************************************************************************* */
// Binary(Leaf,Leaf)
@ -159,7 +157,7 @@ TEST(Expression, BinaryKeys) {
/* ************************************************************************* */
// dimensions
TEST(Expression, BinaryDimensions) {
map<Key, size_t> actual, expected = map_list_of<Key, size_t>(1, 6)(2, 3);
map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3);
binary::p_cam.dims(actual);
EXPECT(actual==expected);
}
@ -190,8 +188,7 @@ TEST(Expression, TreeKeys) {
/* ************************************************************************* */
// dimensions
TEST(Expression, TreeDimensions) {
map<Key, size_t> actual, expected = map_list_of<Key, size_t>(1, 6)(2, 3)(3,
5);
map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3)(3, 5);
tree::uv_hat.dims(actual);
EXPECT(actual==expected);
}

11
gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp
View File

@ -202,6 +202,17 @@ TEST(ExpressionFactor, Shallow) {
// Construct expression, concise evrsion
Point2_ expression = project(transform_to(x_, p_));
// Get and check keys and dims
FastVector<Key> keys;
FastVector<int> dims;
boost::tie(keys, dims) = expression.keysAndDims();
LONGS_EQUAL(2,keys.size());
LONGS_EQUAL(2,dims.size());
LONGS_EQUAL(1,keys[0]);
LONGS_EQUAL(2,keys[1]);
LONGS_EQUAL(6,dims[0]);
LONGS_EQUAL(3,dims[1]);
// traceExecution of shallow tree
typedef UnaryExpression<Point2, Point3> Unary;
typedef BinaryExpression<Point3, Pose3, Point3> Binary;