Merged in feature/fixIssue165 (pull request #48)

Safer public interface

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,17 +20,31 @@
 #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>
+
+class ExpressionFactorShallowTest;
 
 namespace gtsam {
 
+// Forward declare
+template<typename T> class ExpressionFactor;
+
 /**
  * Expression class that supports automatic differentiation
  */
 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
@@ -87,8 +101,7 @@ public:
   template<typename A1, typename A2>
   Expression(typename BinaryExpression<T, A1, A2>::Function function,
       const Expression<A1>& expression1, const Expression<A2>& expression2) :
-      root_(
+      root_(new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
-          new BinaryExpression<T, A1, A2>(function, expression1, expression2)) {
   }
 
   /// Construct a ternary function expression
@@ -101,14 +114,9 @@ public:
               expression2, expression3)) {
   }
 
-  /// Return keys that play in this expression
+  /// Return root
-  std::set<Key> keys() const {
+  const boost::shared_ptr<ExpressionNode<T> >& root() const {
-    return root_->keys();
+    return root_;
-  }
-
-  /// Return dimensions for each argument, as a map
-  void dims(std::map<Key, size_t>& map) const {
-    root_->dims(map);
   }
 
   // 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<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);
+  }
+
+  /**
+   * @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 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<Key>, FastVector<int> > KeysAndDims;
+  KeysAndDims keysAndDims() const {
+    std::map<Key, int> map;
+    dims(map);
+    size_t n = map.size();
+    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 FastVector<Key>& keys,
+      const FastVector<int>& dims, std::vector<Matrix>& H) const {
+
+    // 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[i] = Ab(i);
+
+    return result;
+  }
+
+  /// 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 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
+  // be very selective on who can access these private methods:
-  T value(const Values& values) const {
+  friend class ExpressionFactor<T> ;
-    return root_->value(values);
+  friend class ::ExpressionFactorShallowTest;
-  }
 
-  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> dims_; ///< dimensions of the Jacobian matrices
   size_t augmentedCols_; ///< total number of columns + 1 (for RHS)
 
   static const int Dim = traits::dimension<T>::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<Key, size_t> map;
+    boost::tie(keys_, dims_) = expression_.keysAndDims();
-    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());
 
     // 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<T> chart;
     if (H) {
-      // H should be pre-allocated
+      const T value = expression_.value(x, keys_, dims_, *H);
-      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<DenseIndex>(size()); i++)
-        H->at(i) = Ab(i);
-
       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<GaussianFactor> linearize(const Values& x) const {
-    // TODO(PTF) Is this a place for custom charts?
-    DefaultChart<T> chart;
     // Only linearize if the factor is active
     if (!active(x))
       return boost::shared_ptr<JacobianFactor>();
@@ -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<JacobianFactor> factor(
-        constrained ? new JacobianFactor(keys_, dimensions_, Dim,
+        constrained ? new JacobianFactor(keys_, dims_, Dim,
             boost::static_pointer_cast<noiseModel::Constrained>(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
+    // Get value and Jacobians, writing directly into JacobianFactor
-    T value = expression_.value(x, map); // <<< Reverse AD happens here !
+    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<T> 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;
   }

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
+  // Check dims as map
-  FastVector<Key> keys;
+  std::map<Key, int> map;
-  keys.push_back(67);
+  norm.dims(map);
-  FastVector<int> dims;
+  LONGS_EQUAL(1,map.size());
-  dims.push_back(3);
-  VerticalBlockMatrix Ab(dims, 1);
-  JacobianMap map(keys, Ab);
 
-  // Get value and Jacobian
+  // Get value and Jacobians
-  double actual = norm.value(values, map);
+  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 +157,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 +188,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,
+  map<Key, int> actual, expected = map_list_of<Key, int>(1, 6)(2, 3)(3, 5);
-      5);
   tree::uv_hat.dims(actual);
   EXPECT(actual==expected);
 }

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<Key> keys;
+  FastVector<int> 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<Point2, Point3> Unary;
   typedef BinaryExpression<Point3, Pose3, Point3> Binary;