From e0248c3ca77095ab376525d6e77fdcae381b512d Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 10:53:26 +0100 Subject: [PATCH 1/6] Created keysAndDims and safe version of values --- gtsam_unstable/nonlinear/Expression-inl.h | 8 +- gtsam_unstable/nonlinear/Expression.h | 104 ++++++++++++++---- gtsam_unstable/nonlinear/ExpressionFactor.h | 20 +--- .../nonlinear/tests/testExpression.cpp | 32 +++--- 4 files changed, 108 insertions(+), 56 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h index 58f6de6b4..08dd18ee3 100644 --- a/gtsam_unstable/nonlinear/Expression-inl.h +++ b/gtsam_unstable/nonlinear/Expression-inl.h @@ -252,7 +252,7 @@ public: } /// Return dimensions for each argument, as a map - virtual void dims(std::map& map) const { + virtual void dims(std::map& 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& map) const { + virtual void dims(std::map& map) const { // get dimension from the chart; only works for fixed dimension charts map[key_] = traits::dimension::value; } @@ -371,7 +371,7 @@ public: } /// Return dimensions for each argument - virtual void dims(std::map& map) const { + virtual void dims(std::map& map) const { map[key_] = traits::dimension::value; } @@ -523,7 +523,7 @@ struct GenerateFunctionalNode: Argument, Base { } /// Return dimensions for each argument - virtual void dims(std::map& map) const { + virtual void dims(std::map& map) const { Base::dims(map); This::expression->dims(map); } diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 5e1d425ed..3b5026348 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -20,8 +20,12 @@ #pragma once #include "Expression-inl.h" +#include #include + #include +#include +#include namespace gtsam { @@ -31,6 +35,11 @@ namespace gtsam { template class Expression { +public: + + /// Define type so we can apply it as a meta-function + typedef Expression 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(Symbol(c, j))) { + root_(new LeafExpression(Symbol(c, j))) { } /// Construct a nullary method expression @@ -87,8 +96,7 @@ public: template Expression(typename BinaryExpression::Function function, const Expression& expression1, const Expression& expression2) : - root_( - new BinaryExpression(function, expression1, expression2)) { + root_(new BinaryExpression(function, expression1, expression2)) { } /// Construct a ternary function expression @@ -101,14 +109,9 @@ public: expression2, expression3)) { } - /// Return keys that play in this expression - std::set keys() const { - return root_->keys(); - } - - /// Return dimensions for each argument, as a map - void dims(std::map& map) const { - root_->dims(map); + /// Return root + const boost::shared_ptr >& 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 keys() const { + return root_->keys(); + } + + /// Return dimensions for each argument, as a map + void dims(std::map& map) const { + root_->dims(map); + } + + /// return keys and dimensions as vectors in same order + std::pair, FastVector > keysAndDims() const { + 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); + } + + /** + * @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&> H = + boost::none) const { + + if (H) { + // Get keys and dimensions + FastVector keys; + FastVector 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::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(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& 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 >& root() const { - return root_; - } - - /// Define type so we can apply it as a meta-function - typedef Expression type; }; // http://stackoverflow.com/questions/16260445/boost-bind-to-operator diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 22a2aefeb..22c2f527f 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -22,8 +22,6 @@ #include #include #include -#include -#include #include namespace gtsam { @@ -36,7 +34,7 @@ class ExpressionFactor: public NoiseModelFactor { T measurement_; ///< the measurement to be compared with the expression Expression expression_; ///< the expression that is AD enabled - std::vector dimensions_; ///< dimensions of the Jacobian matrices + FastVector dimensions_; ///< dimensions of the Jacobian matrices size_t augmentedCols_; ///< total number of columns + 1 (for RHS) static const int Dim = traits::dimension::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 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 diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 4e032b9f2..d02a67b2d 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -114,22 +114,29 @@ TEST(Expression, NullaryMethod) { Values values; values.insert(67, Point3(3, 4, 5)); - // Pre-allocate JacobianMap - FastVector keys; - keys.push_back(67); - FastVector dims; - dims.push_back(3); - VerticalBlockMatrix Ab(dims, 1); - JacobianMap map(keys, Ab); + // Check dims as map + std::map 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 keys; + FastVector 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 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 actual, expected = map_list_of(1, 6)(2, 3); + map actual, expected = map_list_of(1, 6)(2, 3); binary::p_cam.dims(actual); EXPECT(actual==expected); } @@ -190,8 +197,7 @@ TEST(Expression, TreeKeys) { /* ************************************************************************* */ // dimensions TEST(Expression, TreeDimensions) { - map actual, expected = map_list_of(1, 6)(2, 3)(3, - 5); + map actual, expected = map_list_of(1, 6)(2, 3)(3, 5); tree::uv_hat.dims(actual); EXPECT(actual==expected); } From 07e5475b6b47b173e101213fc9bc39c910c2def0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 11:02:54 +0100 Subject: [PATCH 2/6] Making friends... --- gtsam_unstable/nonlinear/Expression.h | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 3b5026348..40b55c410 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -27,8 +27,13 @@ #include #include +class ExpressionFactorShallowTest; + namespace gtsam { +// Forward declare +template class ExpressionFactor; + /** * Expression class that supports automatic differentiation */ @@ -204,6 +209,10 @@ private: return value; } + // be very selective on who can access these private methods: + friend class ExpressionFactor; + friend class ::ExpressionFactorShallowTest; + }; // http://stackoverflow.com/questions/16260445/boost-bind-to-operator From 2c35cda71f7b9750f8a8d9547c14d6e8e4b075ae Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 11:23:38 +0100 Subject: [PATCH 3/6] Yet another indirection makes public code a bit cleaner. --- gtsam_unstable/nonlinear/Expression.h | 63 +++++++++++++++------------ 1 file changed, 35 insertions(+), 28 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 40b55c410..8f1514a22 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -45,6 +45,8 @@ public: /// Define type so we can apply it as a meta-function typedef Expression type; + typedef std::pair, FastVector > KeysAndDims; + private: // 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 - std::pair, FastVector > keysAndDims() const { + KeysAndDims keysAndDims() const { std::map map; dims(map); size_t n = map.size(); @@ -153,37 +155,42 @@ public: T value(const Values& values, boost::optional&> H = boost::none) const { - if (H) { - // Get keys and dimensions - FastVector keys; - FastVector 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::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(keys.size()); i++) - H->at(i) = Ab(i); - - return result; - } else { + if (H) + // Call provate version that returns derivatives in H + return value(values, keysAndDims(), *H); + else // 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& H) const { + + const FastVector& keys = keysAndDims.first; + const FastVector& dims = keysAndDims.second; + + // H should be pre-allocated + assert(H->size()==keys.size()); + + // Pre-allocate and zero VerticalBlockMatrix + static const int Dim = traits::dimension::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(keys.size()); i++) + H[i] = Ab(i); + + return result; + } + /// trace execution, very unsafe T traceExecution(const Values& values, ExecutionTrace& trace, ExecutionTraceStorage* traceStorage) const { @@ -192,7 +199,7 @@ private: /** * @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. */ T value(const Values& values, JacobianMap& jacobians) const { @@ -210,7 +217,7 @@ private: } // be very selective on who can access these private methods: - friend class ExpressionFactor; + friend class ExpressionFactor ; friend class ::ExpressionFactorShallowTest; }; From 2ced73ebe189ef12ea2e6ebfcd763d920098f37e Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 11:29:50 +0100 Subject: [PATCH 4/6] We now use safe version in unwhitenedError --- gtsam_unstable/nonlinear/ExpressionFactor.h | 41 +++++++-------------- 1 file changed, 14 insertions(+), 27 deletions(-) diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 22c2f527f..61eb94c89 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -34,7 +34,7 @@ class ExpressionFactor: public NoiseModelFactor { T measurement_; ///< the measurement to be compared with the expression Expression expression_; ///< the expression that is AD enabled - FastVector dimensions_; ///< dimensions of the Jacobian matrices + FastVector dims_; ///< dimensions of the Jacobian matrices size_t augmentedCols_; ///< total number of columns + 1 (for RHS) static const int Dim = traits::dimension::value; @@ -54,13 +54,13 @@ public: // Get keys and dimensions for Jacobian matrices // An Expression is assumed unmutable, so we do this now - boost::tie(keys_,dimensions_) = expression_.keysAndDims(); + boost::tie(keys_, dims_) = expression_.keysAndDims(); // Add sizes to know how much memory to allocate on stack in linearize - augmentedCols_ = std::accumulate(dimensions_.begin(), dimensions_.end(), 1); + augmentedCols_ = std::accumulate(dims_.begin(), dims_.end(), 1); #ifdef DEBUG_ExpressionFactor - BOOST_FOREACH(size_t d, dimensions_) + BOOST_FOREACH(size_t d, dims_) std::cout << d << " "; std::cout << " -> " << Dim << "x" << augmentedCols_ << std::endl; #endif @@ -76,32 +76,15 @@ public: // TODO(PTF) Is this a place for custom charts? DefaultChart chart; if (H) { - // H should be pre-allocated - assert(H->size()==size()); - - VerticalBlockMatrix Ab(dimensions_, Dim); - - // Wrap keys and VerticalBlockMatrix into structure passed to expression_ - JacobianMap map(keys_, Ab); - Ab.matrix().setZero(); - - // Evaluate error to get Jacobians and RHS vector b - T value = expression_.value(x, map); // <<< Reverse AD happens here ! - - // Copy blocks into the vector of jacobians passed in - for (DenseIndex i = 0; i < static_cast(size()); i++) - H->at(i) = Ab(i); - + const T value = expression_.value(x, std::make_pair(keys_, dims_), *H); return chart.local(measurement_, value); } else { - const T& value = expression_.value(x); + const T value = expression_.value(x); return chart.local(measurement_, value); } } virtual boost::shared_ptr linearize(const Values& x) const { - // TODO(PTF) Is this a place for custom charts? - DefaultChart chart; // Only linearize if the factor is active if (!active(x)) return boost::shared_ptr(); @@ -110,9 +93,9 @@ public: // In case noise model is constrained, we need to provide a noise model bool constrained = noiseModel_->is_constrained(); boost::shared_ptr factor( - constrained ? new JacobianFactor(keys_, dimensions_, Dim, + constrained ? new JacobianFactor(keys_, dims_, Dim, boost::static_pointer_cast(noiseModel_)->unit()) : - new JacobianFactor(keys_, dimensions_, Dim)); + new JacobianFactor(keys_, dims_, Dim)); // Wrap keys and VerticalBlockMatrix into structure passed to expression_ VerticalBlockMatrix& Ab = factor->matrixObject(); @@ -121,13 +104,17 @@ public: // Zero out Jacobian so we can simply add to it Ab.matrix().setZero(); - // Evaluate error to get Jacobians and RHS vector b + // Get value and Jacobians, writing directly into JacobianFactor T value = expression_.value(x, jacobianMap); // <<< Reverse AD happens here ! + + // Evaluate error and set RHS vector b + // TODO(PTF) Is this a place for custom charts? + DefaultChart chart; Ab(size()).col(0) = -chart.local(measurement_, value); // Whiten the corresponding system, Ab already contains RHS Vector dummy(Dim); - noiseModel_->WhitenSystem(Ab.matrix(),dummy); + noiseModel_->WhitenSystem(Ab.matrix(), dummy); return factor; } From df91cf7fadb1e9a8bba3bbd6710e8651f13640d6 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 12:36:52 +0100 Subject: [PATCH 5/6] Made vaguely unsafe keysAndDims private (as it relies on keys and dimensions being in same order), as to not tempt people to use it. --- gtsam_unstable/nonlinear/Expression.h | 27 +++++++++---------- .../nonlinear/tests/testExpression.cpp | 9 ------- .../nonlinear/tests/testExpressionFactor.cpp | 11 ++++++++ 3 files changed, 24 insertions(+), 23 deletions(-) diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 8f1514a22..4684184b8 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -45,8 +45,6 @@ public: /// Define type so we can apply it as a meta-function typedef Expression type; - typedef std::pair, FastVector > KeysAndDims; - private: // Paul's trick shared pointer, polymorphic root of entire expression tree @@ -136,18 +134,6 @@ public: root_->dims(map); } - /// return keys and dimensions as vectors in same order - KeysAndDims keysAndDims() const { - 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); - } - /** * @brief Return value and optional derivatives, reverse AD version * Notes: this is not terribly efficient, and H should have correct size. @@ -165,6 +151,19 @@ public: private: + /// Vaguely unsafe keys and dimensions in same order + typedef std::pair, FastVector > KeysAndDims; + KeysAndDims keysAndDims() const { + 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); + } + /// private version that takes keys and dimensions, returns derivatives T value(const Values& values, const KeysAndDims& keysAndDims, std::vector& H) const { diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index d02a67b2d..6156d103c 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -119,15 +119,6 @@ TEST(Expression, NullaryMethod) { norm.dims(map); LONGS_EQUAL(1,map.size()); - // Get and check keys and dims - FastVector keys; - FastVector 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 H(1); double actual = norm.value(values, H); diff --git a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp index b9d9896d3..d146ea945 100644 --- a/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpressionFactor.cpp @@ -202,6 +202,17 @@ TEST(ExpressionFactor, Shallow) { // Construct expression, concise evrsion Point2_ expression = project(transform_to(x_, p_)); + // Get and check keys and dims + FastVector keys; + FastVector dims; + boost::tie(keys, dims) = expression.keysAndDims(); + LONGS_EQUAL(2,keys.size()); + LONGS_EQUAL(2,dims.size()); + LONGS_EQUAL(1,keys[0]); + LONGS_EQUAL(2,keys[1]); + LONGS_EQUAL(6,dims[0]); + LONGS_EQUAL(3,dims[1]); + // traceExecution of shallow tree typedef UnaryExpression Unary; typedef BinaryExpression Binary; From dc4c0b54c0f0169175fc9b2963edc316e8c299f3 Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 25 Nov 2014 16:13:30 +0100 Subject: [PATCH 6/6] Addressed code review by @hannessommer --- gtsam_unstable/nonlinear/Expression.h | 26 ++++++++++----------- gtsam_unstable/nonlinear/ExpressionFactor.h | 2 +- 2 files changed, 13 insertions(+), 15 deletions(-) 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);