From fea2eb0b5f404eb4c46a12efc0974f3b8a234e76 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 12 Oct 2014 11:05:43 +0200
Subject: [PATCH] Inlined VerticalBlockMatrix construction

---
 gtsam_unstable/nonlinear/ExpressionFactor.h | 35 +++++++++++++++++++--
 1 file changed, 32 insertions(+), 3 deletions(-)

diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h
index 5a678c0e3..9ca54924d 100644
--- a/gtsam_unstable/nonlinear/ExpressionFactor.h
+++ b/gtsam_unstable/nonlinear/ExpressionFactor.h
@@ -68,7 +68,7 @@ public:
 
     // Evaluate error to get Jacobians and RHS vector b
     Augmented<T> augmented = expression_.augmented(x);
-    Vector b = - measurement_.localCoordinates(augmented.value());
+    Vector b = -measurement_.localCoordinates(augmented.value());
     // check(noiseModel_, b); // TODO: use, but defined in NonlinearFactor.cpp
 
     // Whiten the corresponding system now
@@ -76,15 +76,44 @@ public:
 
     // Terms, needed to create JacobianFactor below, are already here!
     const JacobianMap& terms = augmented.jacobians();
+    size_t n = terms.size();
+
+    // Get dimensions of matrices
+    std::vector<size_t> dimensions;
+    dimensions.reserve(n);
+    std::vector<Key> keys;
+    keys.reserve(n);
+    for (JacobianMap::const_iterator it = terms.begin(); it != terms.end();
+        ++it) {
+      const std::pair<Key, Matrix>& term = *it;
+      Key key = term.first;
+      const Matrix& Ai = term.second;
+      dimensions.push_back(Ai.cols());
+      keys.push_back(key);
+    }
+
+    // Construct block matrix
+    VerticalBlockMatrix Ab(dimensions, b.size(), true);
+
+    // Check and add terms
+    DenseIndex i = 0; // For block index
+    for (JacobianMap::const_iterator it = terms.begin(); it != terms.end();
+        ++it) {
+      const std::pair<Key, Matrix>& term = *it;
+      const Matrix& Ai = term.second;
+      Ab(i++) = Ai;
+    }
+
+    Ab(n).col(0) = b;
 
     // TODO pass unwhitened + noise model to Gaussian factor
     // For now, only linearized constrained factors have noise model at linear level!!!
     noiseModel::Constrained::shared_ptr constrained = //
         boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
     if (constrained) {
-      return boost::make_shared<JacobianFactor>(terms, b, constrained->unit());
+      return boost::make_shared<JacobianFactor>(keys, Ab, constrained->unit());
     } else
-      return boost::make_shared<JacobianFactor>(terms, b);
+      return boost::make_shared<JacobianFactor>(keys, Ab);
   }
 };
 // ExpressionFactor