From 2092705d12aa7f974a97b43a5644ea8644a4e5d1 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 15 Oct 2014 00:27:43 +0200
Subject: [PATCH 1/4] Allow for other Eigen matrix types

---
 gtsam/base/VerticalBlockMatrix.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h
index 029f55c58..c09cc7577 100644
--- a/gtsam/base/VerticalBlockMatrix.h
+++ b/gtsam/base/VerticalBlockMatrix.h
@@ -74,8 +74,8 @@ namespace gtsam {
     }
 
     /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
-    template<typename CONTAINER>
-    VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) :
+    template<typename CONTAINER, typename DERIVED>
+    VerticalBlockMatrix(const CONTAINER& dimensions, const Eigen::MatrixBase<DERIVED>& matrix, bool appendOneDimension = false) :
       matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0)
     {
       fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);

From 0f055f7910cec302aeef381738df753eeec0a8b7 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 15 Oct 2014 00:28:53 +0200
Subject: [PATCH 2/4] Pass matrix to VerticalBlockMatrix constructor

---
 gtsam_unstable/nonlinear/ExpressionFactor.h | 34 +++++++++------------
 1 file changed, 14 insertions(+), 20 deletions(-)

diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h
index a2e9fb273..42d9ad598 100644
--- a/gtsam_unstable/nonlinear/ExpressionFactor.h
+++ b/gtsam_unstable/nonlinear/ExpressionFactor.h
@@ -20,6 +20,7 @@
 #include <gtsam_unstable/nonlinear/Expression.h>
 #include <gtsam/nonlinear/NonlinearFactor.h>
 #include <gtsam/base/Testable.h>
+#include <numeric>
 
 namespace gtsam {
 
@@ -78,30 +79,23 @@ public:
     }
   }
 
-  // Internal function to allocate a VerticalBlockMatrix and
-  // create Eigen::Block<Matrix> views into it
-  VerticalBlockMatrix prepareBlocks(JacobianMap& blocks) const {
-
-    // Get dimensions of Jacobian matrices
-    std::vector<size_t> dims = expression_.dimensions();
-
-    // Construct block matrix, is of right size but un-initialized
-    VerticalBlockMatrix Ab(dims, T::dimension, true);
-    Ab.matrix().setZero(); // zero out
-
-    // Create blocks to be passed to expression_
-    for(DenseIndex i=0;i<size();i++)
-      blocks.insert(std::make_pair(keys_[i], Ab(i)));
-
-    return Ab;
-  }
-
   virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
 
     // Construct VerticalBlockMatrix and views into it
-    JacobianMap blocks;
-    VerticalBlockMatrix Ab = prepareBlocks(blocks);
 
+    // Get dimensions of Jacobian matrices
+    std::vector<size_t> dims = expression_.dimensions();
+    size_t m = std::accumulate(dims.rend(),dims.rbegin(),0);
+    Matrix matrix(T::dimension,m);
+
+    // Construct block matrix, is of right size but un-initialized
+    VerticalBlockMatrix Ab(dims, matrix, true);
+    Ab.matrix().setZero(); // zero out
+
+    // Create blocks to be passed to expression_
+    JacobianMap blocks;
+    for(DenseIndex i=0;i<size();i++)
+      blocks.insert(std::make_pair(keys_[i], Ab(i)));
     // Evaluate error to get Jacobians and RHS vector b
     T value = expression_.value(x, blocks);
     Ab(size()).col(0) = -measurement_.localCoordinates(value);

From c88b2a59022ec9d41b81ea0c5f5e66b4fcbe92bb Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 15 Oct 2014 00:34:28 +0200
Subject: [PATCH 3/4] Fixed bizarre link erro as well as off-by-1 bug

---
 gtsam_unstable/nonlinear/ExpressionFactor.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h
index 42d9ad598..bc0edbbb5 100644
--- a/gtsam_unstable/nonlinear/ExpressionFactor.h
+++ b/gtsam_unstable/nonlinear/ExpressionFactor.h
@@ -85,8 +85,8 @@ public:
 
     // Get dimensions of Jacobian matrices
     std::vector<size_t> dims = expression_.dimensions();
-    size_t m = std::accumulate(dims.rend(),dims.rbegin(),0);
-    Matrix matrix(T::dimension,m);
+    size_t m1 = std::accumulate(dims.begin(),dims.end(),1);
+    Matrix matrix = Matrix::Identity(T::dimension,m1);
 
     // Construct block matrix, is of right size but un-initialized
     VerticalBlockMatrix Ab(dims, matrix, true);

From 9b1c9bbf37505a35606f648546fed6a0bd4a2911 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Wed, 15 Oct 2014 00:56:06 +0200
Subject: [PATCH 4/4] Allocate temporary matrix on the stack rather tahn on
 heap, and give VerticalBlockMatrix a view on it.

---
 gtsam_unstable/nonlinear/ExpressionFactor.h | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h
index bc0edbbb5..b6bfba27f 100644
--- a/gtsam_unstable/nonlinear/ExpressionFactor.h
+++ b/gtsam_unstable/nonlinear/ExpressionFactor.h
@@ -85,12 +85,15 @@ public:
 
     // Get dimensions of Jacobian matrices
     std::vector<size_t> dims = expression_.dimensions();
+
+    // Allocate memory on stack and create a view on it (saves a malloc)
     size_t m1 = std::accumulate(dims.begin(),dims.end(),1);
-    Matrix matrix = Matrix::Identity(T::dimension,m1);
+    double memory[T::dimension*m1];
+    Eigen::Map<Eigen::Matrix<double,T::dimension,Eigen::Dynamic> > matrix(memory,T::dimension,m1);
+    matrix.setZero(); // zero out
 
     // Construct block matrix, is of right size but un-initialized
     VerticalBlockMatrix Ab(dims, matrix, true);
-    Ab.matrix().setZero(); // zero out
 
     // Create blocks to be passed to expression_
     JacobianMap blocks;