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..68a79ed6b 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/gtsam_unstable/nonlinear/Expression.h @@ -20,17 +20,31 @@ #pragma once #include "Expression-inl.h" +#include #include + #include +#include +#include + +class ExpressionFactorShallowTest; namespace gtsam { +// Forward declare +template class ExpressionFactor; + /** * Expression class that supports automatic differentiation */ 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 +69,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 +101,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 +114,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 +124,81 @@ 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); + } + + /** + * @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 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); + } + +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(); + 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 FastVector& keys, + const FastVector& dims, std::vector& H) const { + + // 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 { return root_->traceExecution(values, trace, traceStorage); } - /// Return value and derivatives, reverse AD version + /** + * @brief Return value and derivatives, reverse AD version + * 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 { // 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 +213,10 @@ public: return value; } - /// Return value - T value(const Values& values) const { - return root_->value(values); - } + // be very selective on who can access these private methods: + friend class ExpressionFactor ; + friend class ::ExpressionFactorShallowTest; - 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..069fb5e2e 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 dims_; ///< dimensions of the Jacobian matrices size_t augmentedCols_; ///< total number of columns + 1 (for RHS) static const int Dim = traits::dimension::value; @@ -54,23 +52,15 @@ 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_, 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 @@ -86,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, 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(); @@ -120,24 +93,28 @@ 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(); - 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 ! + // 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; } diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp index 4e032b9f2..6156d103c 100644 --- a/gtsam_unstable/nonlinear/tests/testExpression.cpp +++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp @@ -114,22 +114,20 @@ 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 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 +157,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 +188,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); } 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;