Made dimensions constant property. Now performance is ***blazing***, way past custom factors.

gtsam_unstable/nonlinear/Expression-inl.h

@@ -24,8 +24,8 @@
 #include <gtsam/base/Testable.h>
 #include <boost/foreach.hpp>
 #include <boost/tuple/tuple.hpp>
-#include <new> // for placement new
+#include <boost/range/adaptor/map.hpp>
-struct TestBinaryExpression;
+#include <boost/range/algorithm.hpp>
 
 // template meta-programming headers
 #include <boost/mpl/vector.hpp>
@@ -37,9 +37,10 @@ struct TestBinaryExpression;
 #include <boost/mpl/placeholders.hpp>
 #include <boost/mpl/transform.hpp>
 #include <boost/mpl/at.hpp>
-
 namespace MPL = boost::mpl::placeholders;
 
+#include <new> // for placement new
+
 class ExpressionFactorBinaryTest;
 // Forward declare for testing
 
@@ -225,12 +226,20 @@ public:
     return keys;
   }
 
-  /// Return dimensions for each argument
+  /// Return dimensions for each argument, as a map
-  virtual std::map<Key, size_t> dimensions() const {
+  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;
+  }
+
   // Return size needed for memory buffer in traceExecution
   size_t traceSize() const {
     return traceSize_;
@@ -298,7 +307,7 @@ public:
   }
 
   /// Return dimensions for each argument
-  virtual std::map<Key, size_t> dimensions() const {
+  virtual std::map<Key, size_t> dims() const {
     std::map<Key, size_t> map;
     map[key_] = T::dimension;
     return map;
@@ -416,9 +425,9 @@ struct GenerateFunctionalNode: Argument<T, A, Base::N + 1>, Base {
   }
 
   /// Return dimensions for each argument
-  virtual std::map<Key, size_t> dimensions() const {
+  virtual std::map<Key, size_t> dims() const {
-    std::map<Key, size_t> map = Base::dimensions();
+    std::map<Key, size_t> map = Base::dims();
-    std::map<Key, size_t> myMap = This::expression->dimensions();
+    std::map<Key, size_t> myMap = This::expression->dims();
     map.insert(myMap.begin(), myMap.end());
     return map;
   }

gtsam_unstable/nonlinear/Expression.h

@@ -22,8 +22,6 @@
 #include "Expression-inl.h"
 #include <gtsam/inference/Symbol.h>
 #include <boost/bind.hpp>
-#include <boost/range/adaptor/map.hpp>
-#include <boost/range/algorithm.hpp>
 
 namespace gtsam {
 
@@ -36,43 +34,51 @@ class Expression {
 private:
 
   // Paul's trick shared pointer, polymorphic root of entire expression tree
-  boost::shared_ptr<ExpressionNode<T> > root_;
+  const boost::shared_ptr<ExpressionNode<T> > root_;
+
+  // Fixed dimensions: an Expression is assumed unmutable
+  const std::vector<size_t> dimensions_;
 
 public:
 
   // Construct a constant expression
   Expression(const T& value) :
-      root_(new ConstantExpression<T>(value)) {
+      root_(new ConstantExpression<T>(value)), //
+      dimensions_(root_->dimensions()) {
   }
 
   // Construct a leaf expression, with Key
   Expression(const Key& key) :
-      root_(new LeafExpression<T>(key)) {
+      root_(new LeafExpression<T>(key)), //
+      dimensions_(root_->dimensions()) {
   }
 
   // Construct a leaf expression, with Symbol
   Expression(const Symbol& symbol) :
-      root_(new LeafExpression<T>(symbol)) {
+      root_(new LeafExpression<T>(symbol)), //
+      dimensions_(root_->dimensions()) {
   }
 
   // Construct a leaf expression, creating Symbol
   Expression(unsigned char c, size_t j) :
-      root_(new LeafExpression<T>(Symbol(c, j))) {
+      root_(new LeafExpression<T>(Symbol(c, j))), //
+      dimensions_(root_->dimensions()) {
   }
 
   /// Construct a nullary method expression
   template<typename A>
   Expression(const Expression<A>& expression,
-      T (A::*method)(typename Optional<T, A>::type) const) {
+      T (A::*method)(typename Optional<T, A>::type) const) :
-    root_.reset(
+      root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)), //
-        new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression));
+      dimensions_(root_->dimensions()) {
   }
 
   /// Construct a unary function expression
   template<typename A>
   Expression(typename UnaryExpression<T, A>::Function function,
-      const Expression<A>& expression) {
+      const Expression<A>& expression) :
-    root_.reset(new UnaryExpression<T, A>(function, expression));
+      root_(new UnaryExpression<T, A>(function, expression)), //
+      dimensions_(root_->dimensions()) {
   }
 
   /// Construct a unary method expression
@@ -80,28 +86,31 @@ public:
   Expression(const Expression<A1>& expression1,
       T (A1::*method)(const A2&, typename Optional<T, A1>::type,
           typename Optional<T, A2>::type) const,
-      const Expression<A2>& expression2) {
+      const Expression<A2>& expression2) :
-    root_.reset(
+      root_(
           new BinaryExpression<T, A1, A2>(boost::bind(method, _1, _2, _3, _4),
-            expression1, expression2));
+              expression1, expression2)), //
+      dimensions_(root_->dimensions()) {
   }
 
   /// Construct a binary function expression
   template<typename A1, typename A2>
   Expression(typename BinaryExpression<T, A1, A2>::Function function,
-      const Expression<A1>& expression1, const Expression<A2>& expression2) {
+      const Expression<A1>& expression1, const Expression<A2>& expression2) :
-    root_.reset(
+      root_(
-        new BinaryExpression<T, A1, A2>(function, expression1, expression2));
+          new BinaryExpression<T, A1, A2>(function, expression1, expression2)), //
+      dimensions_(root_->dimensions()) {
   }
 
   /// Construct a ternary function expression
   template<typename A1, typename A2, typename A3>
   Expression(typename TernaryExpression<T, A1, A2, A3>::Function function,
       const Expression<A1>& expression1, const Expression<A2>& expression2,
-      const Expression<A3>& expression3) {
+      const Expression<A3>& expression3) :
-    root_.reset(
+      root_(
-        new TernaryExpression<T, A1, A2, A3>(function, expression1, expression2,
+          new TernaryExpression<T, A1, A2, A3>(function, expression1,
-            expression3));
+              expression2, expression3)), //
+      dimensions_(root_->dimensions()) {
   }
 
   /// Return keys that play in this expression
@@ -110,11 +119,8 @@ public:
   }
 
   /// Return dimensions for each argument, must be same order as keys
-  std::vector<size_t> dimensions() const {
+  const std::vector<size_t>& dimensions() const {
-    std::map<Key,size_t> map = root_->dimensions();
+    return dimensions_;
-    std::vector<size_t> dims(map.size());
-    boost::copy(map | boost::adaptors::map_values, dims.begin());
-    return dims;
   }
 
   // Return size needed for memory buffer in traceExecution