Created keysAndDims and safe version of values

gtsam_unstable/nonlinear/Expression-inl.h

@@ -252,7 +252,7 @@ public:
   }
 
   /// Return dimensions for each argument, as a map
-  virtual void dims(std::map<Key, size_t>& map) const {
+  virtual void dims(std::map<Key, int>& 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<Key, size_t>& map) const {
+  virtual void dims(std::map<Key, int>& map) const {
     // get dimension from the chart; only works for fixed dimension charts
     map[key_] = traits::dimension<Chart>::value;
   }
@@ -371,7 +371,7 @@ public:
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, size_t>& map) const {
+  virtual void dims(std::map<Key, int>& map) const {
     map[key_] = traits::dimension<T>::value;
   }
 
@@ -523,7 +523,7 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
   }
 
   /// Return dimensions for each argument
-  virtual void dims(std::map<Key, size_t>& map) const {
+  virtual void dims(std::map<Key, int>& map) const {
     Base::dims(map);
     This::expression->dims(map);
   }

gtsam_unstable/nonlinear/Expression.h

@@ -20,8 +20,12 @@
 #pragma once
 
 #include "Expression-inl.h"
+#include <gtsam/base/FastVector.h>
 #include <gtsam/inference/Symbol.h>
+
 #include <boost/bind.hpp>
+#include <boost/range/adaptor/map.hpp>
+#include <boost/range/algorithm.hpp>
 
 namespace gtsam {
 
@@ -31,6 +35,11 @@ namespace gtsam {
 template<typename T>
 class Expression {
 
+public:
+
+  /// Define type so we can apply it as a meta-function
+  typedef Expression<T> 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<T>(Symbol(c, j)))  {
+      root_(new LeafExpression<T>(Symbol(c, j))) {
   }
 
   /// Construct a nullary method expression
@@ -87,8 +96,7 @@ public:
   template<typename A1, typename A2>
   Expression(typename BinaryExpression<T, A1, A2>::Function function,
       const Expression<A1>& expression1, const Expression<A2>& expression2) :
-      root_(
-          new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
+      root_(new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
   }
 
   /// Construct a ternary function expression
@@ -101,14 +109,9 @@ public:
               expression2, expression3)) {
   }
 
-  /// Return keys that play in this expression
-  std::set<Key> keys() const {
-    return root_->keys();
-  }
-
-  /// Return dimensions for each argument, as a map
-  void dims(std::map<Key, size_t>& map) const {
-    root_->dims(map);
+  /// Return root
+  const boost::shared_ptr<ExpressionNode<T> >& 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<Key> keys() const {
+    return root_->keys();
+  }
+
+  /// Return dimensions for each argument, as a map
+  void dims(std::map<Key, int>& map) const {
+    root_->dims(map);
+  }
+
+  /// return keys and dimensions as vectors in same order
+  std::pair<FastVector<Key>, FastVector<int> > keysAndDims() const {
+    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);
+  }
+
+  /**
+   * @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<std::vector<Matrix>&> H =
+      boost::none) const {
+
+    if (H) {
+      // Get keys and dimensions
+      FastVector<Key> keys;
+      FastVector<int> 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<T>::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<DenseIndex>(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<T>& 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<ExpressionNode<T> >& root() const {
-    return root_;
-  }
-
-  /// Define type so we can apply it as a meta-function
-  typedef Expression<T> type;
 };
 
 // http://stackoverflow.com/questions/16260445/boost-bind-to-operator

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -22,8 +22,6 @@
 #include <gtsam_unstable/nonlinear/Expression.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/Testable.h>
-#include <boost/range/adaptor/map.hpp>
-#include <boost/range/algorithm.hpp>
 #include <numeric>
 
 namespace gtsam {
@@ -36,7 +34,7 @@ class ExpressionFactor: public NoiseModelFactor {
 
   T measurement_; ///< the measurement to be compared with the expression
   Expression<T> expression_; ///< the expression that is AD enabled
-  std::vector<size_t> dimensions_; ///< dimensions of the Jacobian matrices
+  FastVector<int> dimensions_; ///< dimensions of the Jacobian matrices
   size_t augmentedCols_; ///< total number of columns + 1 (for RHS)
 
   static const int Dim = traits::dimension<T>::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<Key, size_t> 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

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<Key> keys;
-  keys.push_back(67);
-  FastVector<int> dims;
-  dims.push_back(3);
-  VerticalBlockMatrix Ab(dims, 1);
-  JacobianMap map(keys, Ab);
+  // Check dims as map
+  std::map<Key, int> 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<Key> keys;
+  FastVector<int> 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<Matrix> 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<Key, size_t> actual, expected = map_list_of<Key, size_t>(1, 6)(2, 3);
+  map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3);
   binary::p_cam.dims(actual);
   EXPECT(actual==expected);
 }
@@ -190,8 +197,7 @@ TEST(Expression, TreeKeys) {
 /* ************************************************************************* */
 // dimensions
 TEST(Expression, TreeDimensions) {
-  map<Key, size_t> actual, expected = map_list_of<Key, size_t>(1, 6)(2, 3)(3,
-      5);
+  map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3)(3, 5);
   tree::uv_hat.dims(actual);
   EXPECT(actual==expected);
 }