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
* 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 provate version that returns derivatives in H
return value(values, keysAndDims(), *H);
else
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);
}
@ -157,19 +159,15 @@ private:
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);
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 KeysAndDims& keysAndDims,
std::vector<Matrix>& H) const {
const FastVector<Key>& keys = keysAndDims.first;
const FastVector<int>& dims = keysAndDims.second;
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());

2
gtsam_unstable/nonlinear/ExpressionFactor.h
View File

@ -76,7 +76,7 @@ public:
// TODO(PTF) Is this a place for custom charts?
DefaultChart<T> chart;
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);
} else {
const T value = expression_.value(x);