Make JacobianMap a wrapper around a VerticalBlockMatrix, which avoids us having to make a vector of references into it

gtsam_unstable/nonlinear/Expression-inl.h

@@ -23,6 +23,7 @@
 #include <gtsam/base/Matrix.h>
 #include <gtsam/base/Testable.h>
 #include <gtsam/base/Manifold.h>
+#include <gtsam/base/VerticalBlockMatrix.h>
 
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
@@ -49,8 +50,25 @@ namespace gtsam {
 template<typename T>
 class Expression;
 
-typedef std::pair<Key, Eigen::Block<Matrix> > JacobianPair;
-typedef std::vector<JacobianPair> JacobianMap;
+/**
+ * Expressions are designed to write their derivatives into an already allocated
+ * Jacobian of the correct size, of type VerticalBlockMatrix.
+ * The JacobianMap provides a mapping from keys to the underlying blocks.
+ */
+class JacobianMap {
+  const FastVector<Key>& keys_;
+  VerticalBlockMatrix& Ab_;
+public:
+  JacobianMap(const FastVector<Key>& keys, VerticalBlockMatrix& Ab) :
+      keys_(keys), Ab_(Ab) {
+  }
+  /** Access a single block in the underlying matrix with read/write access */
+  VerticalBlockMatrix::Block operator()(Key key) {
+    FastVector<Key>::const_iterator it = std::find(keys_.begin(),keys_.end(),key);
+    DenseIndex block = it - keys_.begin();
+    return Ab_(block);
+  }
+};
 
 //-----------------------------------------------------------------------------
 /**
@@ -80,20 +98,14 @@ struct CallRecord {
 template<int ROWS, int COLS>
 void handleLeafCase(const Eigen::Matrix<double, ROWS, COLS>& dTdA,
     JacobianMap& jacobians, Key key) {
-  JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(),
-      boost::bind(&JacobianPair::first, _1) == key);
-  assert(it!=jacobians.end());
-  it->second.block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference
+  jacobians(key).block < ROWS, COLS > (0, 0) += dTdA; // block makes HUGE difference
 }
 /// Handle Leaf Case for Dynamic Matrix type (slower)
 template<>
 void handleLeafCase(
     const Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>& dTdA,
     JacobianMap& jacobians, Key key) {
-  JacobianMap::iterator it = std::find_if(jacobians.begin(), jacobians.end(),
-      boost::bind(&JacobianPair::first, _1) == key);
-  assert(it!=jacobians.end());
-  it->second += dTdA;
+  jacobians(key) += dTdA;
 }
 
 //-----------------------------------------------------------------------------

gtsam_unstable/nonlinear/Expression.h

@@ -123,7 +123,7 @@ public:
   }
 
   /// Return value and derivatives, reverse AD version
-  T reverse(const Values& values, JacobianMap& jacobians) const {
+  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
     // with modern C++ compilers. The traceExecution then fills this memory
@@ -142,11 +142,6 @@ public:
     return root_->value(values);
   }
 
-  /// Return value and derivatives
-  T value(const Values& values, JacobianMap& jacobians) const {
-    return reverse(values, jacobians);
-  }
-
   const boost::shared_ptr<ExpressionNode<T> >& root() const {
     return root_;
   }

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -81,27 +81,27 @@ public:
    */
   virtual Vector unwhitenedError(const Values& x,
       boost::optional<std::vector<Matrix>&> H = boost::none) const {
-    if (H) {
-      // H should be pre-allocated
-      assert(H->size()==size());
-
-      // Create and zero out blocks to be passed to expression_
-      JacobianMap blocks;
-      blocks.reserve(size());
-      for (DenseIndex i = 0; i < size(); i++) {
-        Matrix& Hi = H->at(i);
-        Hi.resize(Dim, dimensions_[i]);
-        Hi.setZero(); // zero out
-        Eigen::Block<Matrix> block = Hi.block(0, 0, Dim, dimensions_[i]);
-        blocks.push_back(std::make_pair(keys_[i], block));
-      }
-
-      T value = expression_.value(x, blocks);
-      return measurement_.localCoordinates(value);
-    } else {
+//    if (H) {
+//      // H should be pre-allocated
+//      assert(H->size()==size());
+//
+//      // Create and zero out blocks to be passed to expression_
+//      JacobianMap blocks;
+//      blocks.reserve(size());
+//      for (DenseIndex i = 0; i < size(); i++) {
+//        Matrix& Hi = H->at(i);
+//        Hi.resize(Dim, dimensions_[i]);
+//        Hi.setZero(); // zero out
+//        Eigen::Block<Matrix> block = Hi.block(0, 0, Dim, dimensions_[i]);
+//        blocks.push_back(std::make_pair(keys_[i], block));
+//      }
+//
+//      T value = expression_.value(x, blocks);
+//      return measurement_.localCoordinates(value);
+//    } else {
       const T& value = expression_.value(x);
       return measurement_.localCoordinates(value);
-    }
+//    }
   }
 
   virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
@@ -120,14 +120,11 @@ public:
     // Construct block matrix, is of right size but un-initialized
     VerticalBlockMatrix Ab(dimensions_, matrix, true);
 
-    // Create blocks into Ab_ to be passed to expression_
-    JacobianMap blocks;
-    blocks.reserve(size());
-    for (DenseIndex i = 0; i < size(); i++)
-      blocks.push_back(std::make_pair(keys_[i], Ab(i)));
+    // Wrap keys and VerticalBlockMatrix into structure passed to expression_
+    JacobianMap map(keys_,Ab);
 
     // Evaluate error to get Jacobians and RHS vector b
-    T value = expression_.value(x, blocks); // <<< Reverse AD happens here !
+    T value = expression_.value(x, map); // <<< Reverse AD happens here !
     Ab(size()).col(0) = -measurement_.localCoordinates(value);
 
     // Whiten the corresponding system now