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

Addressed code review by @hannessommer

release/4.3a0
dellaert 2014-11-25 16:13:30 +01:00
parent df91cf7fad
commit dc4c0b54c0
2 changed files with 13 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
gtsam_unstable/nonlinear/Expression.h
View File

@ -137,14 +137,16 @@ public:
/** /**
* @brief Return value and optional derivatives, reverse AD version * @brief Return value and optional derivatives, reverse AD version
* Notes: this is not terribly efficient, and H should have correct size. * 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 = T value(const Values& values, boost::optional<std::vector<Matrix>&> H =
boost::none) const { boost::none) const {
if (H) if (H) {
// Call provate version that returns derivatives in H // Call private version that returns derivatives in H
return value(values, keysAndDims(), *H); KeysAndDims pair = keysAndDims();
else return value(values, pair.first, pair.second, *H);
} else
// no derivatives needed, just return value // no derivatives needed, just return value
return root_->value(values); return root_->value(values);
} }
@ -157,19 +159,15 @@ private:
std::map<Key, int> map; std::map<Key, int> map;
dims(map); dims(map);
size_t n = map.size(); size_t n = map.size();
FastVector<Key> keys(n); KeysAndDims pair = std::make_pair(FastVector<Key>(n), FastVector<int>(n));
boost::copy(map | boost::adaptors::map_keys, keys.begin()); boost::copy(map | boost::adaptors::map_keys, pair.first.begin());
FastVector<int> dims(n); boost::copy(map | boost::adaptors::map_values, pair.second.begin());
boost::copy(map | boost::adaptors::map_values, dims.begin()); return pair;
return make_pair(keys, dims);
} }
/// private version that takes keys and dimensions, returns derivatives /// private version that takes keys and dimensions, returns derivatives
T value(const Values& values, const KeysAndDims& keysAndDims, T value(const Values& values, const FastVector<Key>& keys,
std::vector<Matrix>& H) const { const FastVector<int>& dims, 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());

2
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -76,7 +76,7 @@ public:
// TODO(PTF) Is this a place for custom charts? // TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart; DefaultChart<T> chart;
if (H) { if (H) {
const T value = expression_.value(x, std::make_pair(keys_, dims_), *H); const T value = expression_.value(x, keys_, dims_, *H);
return chart.local(measurement_, value); return chart.local(measurement_, value);
} else { } else {
const T value = expression_.value(x); const T value = expression_.value(x);