Merge remote-tracking branch 'origin/feature/BAD_VerticalBlockMatrix' into feature/BAD_WriteableJacobianFactor

Conflicts:
	gtsam_unstable/nonlinear/ExpressionFactor.h

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 via key
+  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

@@ -124,7 +124,7 @@ public:
   }
 
   /**
-   * Error function *without* the NoiseModel, \f$ z-h(x) \f$.
+   * Error function *without* the NoiseModel, \f$ h(x)-z \f$.
    * We override this method to provide
    * both the function evaluation and its derivative(s) in H.
    */
@@ -134,18 +134,19 @@ public:
       // 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));
-      }
+      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 < size(); i++)
+        H->at(i) = Ab(i);
 
-      T value = expression_.value(x, blocks);
       return measurement_.localCoordinates(value);
     } else {
       const T& value = expression_.value(x);
@@ -167,14 +168,11 @@ public:
         WriteableJacobianFactor>(keys_, dimensions_,
         traits::dimension<T>::value, model);
 
-    // 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], factor->Ab()(i)));
+    // Wrap keys and VerticalBlockMatrix into structure passed to expression_
+    JacobianMap map(keys_, factor->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 !
     factor->Ab()(size()).col(0) = -measurement_.localCoordinates(value);
 
     // Whiten the corresponding system now

gtsam_unstable/nonlinear/tests/testExpression.cpp

@@ -46,11 +46,8 @@ static const Rot3 someR = Rot3::RzRyRx(1, 2, 3);
 TEST(Expression, constant) {
   Expression<Rot3> R(someR);
   Values values;
-  JacobianMap actualMap;
-  Rot3 actual = R.value(values, actualMap);
+  Rot3 actual = R.value(values);
   EXPECT(assert_equal(someR, actual));
-  JacobianMap expected;
-  EXPECT(actualMap == expected);
   EXPECT_LONGS_EQUAL(0, R.traceSize())
 }
 
@@ -61,15 +58,8 @@ TEST(Expression, Leaf) {
   Values values;
   values.insert(100, someR);
 
-  JacobianMap expected;
-  Matrix H = eye(3);
-  expected.push_back(make_pair(100, H.block(0, 0, 3, 3)));
-
-  JacobianMap actualMap2;
-  actualMap2.push_back(make_pair(100, H.block(0, 0, 3, 3)));
-  Rot3 actual2 = R.reverse(values, actualMap2);
+  Rot3 actual2 = R.value(values);
   EXPECT(assert_equal(someR, actual2));
-  EXPECT(actualMap2 == expected);
 }
 
 /* ************************************************************************* */

gtsam_unstable/timing/timeCameraExpression.cpp

@@ -27,7 +27,7 @@
 using namespace std;
 using namespace gtsam;
 
-#define time timeMultiThreaded
+#define time timeSingleThreaded
 
 boost::shared_ptr<Cal3_S2> fixedK(new Cal3_S2());