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

Yet another indirection makes public code a bit cleaner.

release/4.3a0
dellaert 2014-11-25 11:23:38 +01:00
parent 07e5475b6b
commit 2c35cda71f
1 changed files with 35 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

35
gtsam_unstable/nonlinear/Expression.h
View File

@ -45,6 +45,8 @@ public:
/// Define type so we can apply it as a meta-function /// Define type so we can apply it as a meta-function
typedef Expression<T> type; typedef Expression<T> type;
typedef std::pair<FastVector<Key>, FastVector<int> > KeysAndDims;
private: private:
// Paul's trick shared pointer, polymorphic root of entire expression tree // Paul's trick shared pointer, polymorphic root of entire expression tree
@ -135,7 +137,7 @@ public:
} }
/// return keys and dimensions as vectors in same order /// return keys and dimensions as vectors in same order
std::pair<FastVector<Key>, FastVector<int> > keysAndDims() const { KeysAndDims keysAndDims() const {
std::map<Key, int> map; std::map<Key, int> map;
dims(map); dims(map);
size_t n = map.size(); size_t n = map.size();
@ -153,11 +155,22 @@ public:
T value(const Values& values, boost::optional<std::vector<Matrix>&> H = T value(const Values& values, boost::optional<std::vector<Matrix>&> H =
boost::none) const { boost::none) const {
if (H) { if (H)
// Get keys and dimensions // Call provate version that returns derivatives in H
FastVector<Key> keys; return value(values, keysAndDims(), *H);
FastVector<int> dims; else
boost::tie(keys, dims) = keysAndDims(); // no derivatives needed, just return value
return root_->value(values);
}
private:
/// private version that takes keys and dimensions, returns derivatives
T value(const Values& values, const KeysAndDims& keysAndDims,
std::vector<Matrix>& H) const {
const FastVector<Key>& keys = keysAndDims.first;
const FastVector<int>& dims = keysAndDims.second;
// H should be pre-allocated // H should be pre-allocated
assert(H->size()==keys.size()); assert(H->size()==keys.size());
@ -173,16 +186,10 @@ public:
// Copy blocks into the vector of jacobians passed in // Copy blocks into the vector of jacobians passed in
for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++) for (DenseIndex i = 0; i < static_cast<DenseIndex>(keys.size()); i++)
H->at(i) = Ab(i); H[i] = Ab(i);
return result; return result;
} else {
// no derivatives needed, just return value
return root_->value(values);
} }
}
private:
/// trace execution, very unsafe /// trace execution, very unsafe
T traceExecution(const Values& values, ExecutionTrace<T>& trace, T traceExecution(const Values& values, ExecutionTrace<T>& trace,
@ -192,7 +199,7 @@ private:
/** /**
* @brief 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 * This very unsafe method needs a JacobianMap with correctly allocated
* and initialized VerticalBlockMatrix, hence is declared private. * and initialized VerticalBlockMatrix, hence is declared private.
*/ */
T value(const Values& values, JacobianMap& jacobians) const { T value(const Values& values, JacobianMap& jacobians) const {