diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 4684184b8..68a79ed6b 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -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&> 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 map; dims(map); size_t n = map.size(); - FastVector keys(n); - boost::copy(map | boost::adaptors::map_keys, keys.begin()); - FastVector dims(n); - boost::copy(map | boost::adaptors::map_values, dims.begin()); - return make_pair(keys, dims); + KeysAndDims pair = std::make_pair(FastVector(n), FastVector(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& H) const { - - const FastVector& keys = keysAndDims.first; - const FastVector& dims = keysAndDims.second; + T value(const Values& values, const FastVector& keys, + const FastVector& dims, std::vector& H) const { // H should be pre-allocated assert(H->size()==keys.size()); diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 61eb94c89..069fb5e2e 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -76,7 +76,7 @@ public: // TODO(PTF) Is this a place for custom charts? DefaultChart 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);