From 0bcca2c386975d6f694322646bf459bb93683f2b Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Thu, 16 Oct 2014 12:01:20 +0200
Subject: [PATCH] Drastic reduction in allocations at ExpressionFactor
 construction by having dims constructed imperatively, and using it for both
 keys_ and dimensions_

---
 gtsam_unstable/nonlinear/Expression-inl.h     | 26 ++++---------------
 gtsam_unstable/nonlinear/Expression.h         |  6 ++---
 gtsam_unstable/nonlinear/ExpressionFactor.h   | 14 ++++++++--
 .../nonlinear/tests/testExpression.cpp        | 12 ++++-----
 4 files changed, 26 insertions(+), 32 deletions(-)

diff --git a/gtsam_unstable/nonlinear/Expression-inl.h b/gtsam_unstable/nonlinear/Expression-inl.h
index 7e406cf6d..5114ac911 100644
--- a/gtsam_unstable/nonlinear/Expression-inl.h
+++ b/gtsam_unstable/nonlinear/Expression-inl.h
@@ -24,8 +24,6 @@
 #include <gtsam/base/Testable.h>
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
-#include <boost/range/adaptor/map.hpp>
-#include <boost/range/algorithm.hpp>
 
 // template meta-programming headers
 #include <boost/mpl/vector.hpp>
@@ -231,17 +229,7 @@ public:
   }
 
   /// Return dimensions for each argument, as a map
-  virtual std::map<Key, size_t> dims() const {
-    std::map<Key, size_t> map;
-    return map;
-  }
-
-  /// Return dimensions as vector, ordered as keys
-  std::vector<size_t> dimensions() const {
-    std::map<Key,size_t> map = dims();
-    std::vector<size_t> dims(map.size());
-    boost::copy(map | boost::adaptors::map_values, dims.begin());
-    return dims;
+  virtual void dims(std::map<Key, size_t>& map) const {
   }
 
   // Return size needed for memory buffer in traceExecution
@@ -311,10 +299,8 @@ public:
   }
 
   /// Return dimensions for each argument
-  virtual std::map<Key, size_t> dims() const {
-    std::map<Key, size_t> map;
+  virtual void dims(std::map<Key, size_t>& map) const {
     map[key_] = T::dimension;
-    return map;
   }
 
   /// Return value
@@ -429,11 +415,9 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
   }
 
   /// Return dimensions for each argument
-  virtual std::map<Key, size_t> dims() const {
-    std::map<Key, size_t> map = Base::dims();
-    std::map<Key, size_t> myMap = This::expression->dims();
-    map.insert(myMap.begin(), myMap.end());
-    return map;
+  virtual void dims(std::map<Key, size_t>& map) const {
+    Base::dims(map);
+    This::expression->dims(map);
   }
 
   /// Recursive Record Class for Functional Expressions
diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h
index 78c4f0707..1ab69880e 100644
--- a/gtsam_unstable/nonlinear/Expression.h
+++ b/gtsam_unstable/nonlinear/Expression.h
@@ -106,9 +106,9 @@ public:
     return root_->keys();
   }
 
-  /// Return dimensions for each argument, must be same order as keys
-  std::vector<size_t> dimensions() const {
-    return root_->dimensions();
+  /// 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
diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h
index 013623bf5..8030165b9 100644
--- a/gtsam_unstable/nonlinear/ExpressionFactor.h
+++ b/gtsam_unstable/nonlinear/ExpressionFactor.h
@@ -20,6 +20,8 @@
 #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 {
@@ -40,17 +42,25 @@ public:
   /// Constructor
   ExpressionFactor(const SharedNoiseModel& noiseModel, //
       const T& measurement, const Expression<T>& expression) :
-      NoiseModelFactor(noiseModel, expression.keys()), //
       measurement_(measurement), expression_(expression) {
     if (!noiseModel)
       throw std::invalid_argument("ExpressionFactor: no NoiseModel.");
     if (noiseModel->dim() != T::dimension)
       throw std::invalid_argument(
           "ExpressionFactor was created with a NoiseModel of incorrect dimension.");
+    noiseModel_ = noiseModel;
 
     // Get dimensions of Jacobian matrices
     // An Expression is assumed unmutable, so we do this now
-    dimensions_ = expression_.dimensions();
+    std::map<Key, size_t> map;
+    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);
diff --git a/gtsam_unstable/nonlinear/tests/testExpression.cpp b/gtsam_unstable/nonlinear/tests/testExpression.cpp
index db6dcf367..2e6d52545 100644
--- a/gtsam_unstable/nonlinear/tests/testExpression.cpp
+++ b/gtsam_unstable/nonlinear/tests/testExpression.cpp
@@ -112,9 +112,9 @@ TEST(Expression, BinaryKeys) {
 /* ************************************************************************* */
 // dimensions
 TEST(Expression, BinaryDimensions) {
-  vector<size_t> expected = list_of(6)(3), //
-  actual = binary::p_cam.dimensions();
-  EXPECT(expected==actual);
+  map<Key,size_t> actual, expected = map_list_of<Key,size_t>(1,6)(2,3);
+  binary::p_cam.dims(actual);
+  EXPECT(actual==expected);
 }
 /* ************************************************************************* */
 // Binary(Leaf,Unary(Binary(Leaf,Leaf)))
@@ -136,9 +136,9 @@ TEST(Expression, TreeKeys) {
 /* ************************************************************************* */
 // dimensions
 TEST(Expression, TreeDimensions) {
-  vector<size_t> expected = list_of(6)(3)(5), //
-  actual = tree::uv_hat.dimensions();
-  EXPECT(expected==actual);
+  map<Key,size_t> actual, expected = map_list_of<Key,size_t>(1,6)(2,3)(3,5);
+  tree::uv_hat.dims(actual);
+  EXPECT(actual==expected);
 }
 /* ************************************************************************* */