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

Created keysAndDims and safe version of values

release/4.3a0
dellaert 2014-11-25 10:53:26 +01:00
parent ce033f5594
commit e0248c3ca7
4 changed files with 108 additions and 56 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);
}

104
gtsam_unstable/nonlinear/Expression.h
View File

@ -20,8 +20,12 @@
#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>
namespace gtsam {
@ -31,6 +35,11 @@ namespace gtsam {
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 +64,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 +96,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 +109,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 +119,77 @@ 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);
}
/// return keys and dimensions as vectors in same order
std::pair<FastVector<Key>, FastVector<int> > keysAndDims() const {
std::map<Key, int> map;
dims(map);
size_t n = map.size();
FastVector<Key> keys(n);
boost::copy(map | boost::adaptors::map_keys, keys.begin());
FastVector<int> dims(n);
boost::copy(map | boost::adaptors::map_values, dims.begin());
return make_pair(keys, dims);
}
/**
* @brief Return value and optional derivatives, reverse AD version
* Notes: this is not terribly efficient, and H should have correct size.
*/
T value(const Values& values, boost::optional<std::vector<Matrix>&> H =
boost::none) const {
if (H) {
// Get keys and dimensions
FastVector<Key> keys;
FastVector<int> dims;
boost::tie(keys, dims) = keysAndDims();
// 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->at(i) = Ab(i);
return result;
} else {
// no derivatives needed, just return value
return root_->value(values);
}
}
private:
/// 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 fairly 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 +204,6 @@ public:
return value;
}
/// Return value
T value(const Values& values) const {
return root_->value(values);
}
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

20
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> dimensions_; ///< dimensions of the Jacobian matrices
size_t augmentedCols_; ///< total number of columns + 1 (for RHS)
static const int Dim = traits::dimension<T>::value;
@ -54,17 +52,9 @@ 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_,dimensions_) = expression_.keysAndDims();
// Add sizes to know how much memory to allocate on stack in linearize
augmentedCols_ = std::accumulate(dimensions_.begin(), dimensions_.end(), 1);
@ -126,13 +116,13 @@ public:
// 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 !
T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here !
Ab(size()).col(0) = -chart.local(measurement_, value);
// Whiten the corresponding system, Ab already contains RHS

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

@ -114,22 +114,29 @@ 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 and check keys and dims
FastVector<Key> keys;
FastVector<int> dims;
boost::tie(keys, dims) = norm.keysAndDims();
LONGS_EQUAL(1,keys.size());
LONGS_EQUAL(1,dims.size());
LONGS_EQUAL(67,keys[0]);
LONGS_EQUAL(3,dims[0]);
// 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 +166,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 +197,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);
}