Only ExpressionFactor needs dimensions! Also, add dimensions at construction -> speeds up linearize.

gtsam_unstable/nonlinear/Expression.h

@@ -34,51 +34,42 @@ class Expression {
 private:
 
   // Paul's trick shared pointer, polymorphic root of entire expression tree
-  const boost::shared_ptr<ExpressionNode<T> > root_;
+  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) :
-      root_(new UnaryExpression<T, A>(boost::bind(method, _1, _2), expression)), //
+      root_(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) :
-      root_(new UnaryExpression<T, A>(function, expression)), //
+      root_(new UnaryExpression<T, A>(function, expression)) {
-      dimensions_(root_->dimensions()) {
   }
 
   /// Construct a unary method expression
@@ -89,8 +80,7 @@ public:
       const Expression<A2>& expression2) :
       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
@@ -98,8 +88,7 @@ public:
   Expression(typename BinaryExpression<T, A1, A2>::Function function,
       const Expression<A1>& expression1, const Expression<A2>& expression2) :
       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
@@ -109,8 +98,7 @@ public:
       const Expression<A3>& expression3) :
       root_(
           new TernaryExpression<T, A1, A2, A3>(function, expression1,
-              expression2, expression3)), //
+              expression2, expression3)) {
-      dimensions_(root_->dimensions()) {
   }
 
   /// Return keys that play in this expression
@@ -119,8 +107,8 @@ public:
   }
 
   /// Return dimensions for each argument, must be same order as keys
-  const std::vector<size_t>& dimensions() const {
+  std::vector<size_t> dimensions() const {
-    return dimensions_;
+    return root_->dimensions();
   }
 
   // Return size needed for memory buffer in traceExecution

gtsam_unstable/nonlinear/ExpressionFactor.h

@@ -30,8 +30,10 @@ namespace gtsam {
 template<class T>
 class ExpressionFactor: public NoiseModelFactor {
 
-  const T measurement_;
+  T measurement_; ///< the measurement to be compared with the expression
-  const Expression<T> expression_;
+  Expression<T> expression_; ///< the expression that is AD enabled
+  std::vector<size_t> dimensions_; ///< dimensions of the Jacobian matrices
+  size_t augmentedCols_; ///< total number of columns + 1 (for RHS)
 
 public:
 
@@ -45,6 +47,19 @@ public:
     if (noiseModel->dim() != T::dimension)
       throw std::invalid_argument(
           "ExpressionFactor was created with a NoiseModel of incorrect dimension.");
+
+    // Get dimensions of Jacobian matrices
+    // An Expression is assumed unmutable, so we do this now
+    dimensions_ = expression_.dimensions();
+
+    // Add sizes to know how much memory to allocate on stack in linearize
+    augmentedCols_ = std::accumulate(dimensions_.begin(), dimensions_.end(), 1);
+
+#ifdef DEBUG_ExpressionFactor
+    BOOST_FOREACH(size_t d, dimensions_)
+    std::cout << d << " ";
+    std::cout << " -> " << T::dimension << "x" << augmentedCols_ << std::endl;
+#endif
   }
 
   /**
@@ -58,16 +73,14 @@ public:
       // H should be pre-allocated
       assert(H->size()==size());
 
-      // Get dimensions of Jacobian matrices
-      std::vector<size_t> dims = expression_.dimensions();
-
       // Create and zero out blocks to be passed to expression_
       JacobianMap blocks;
-      for(DenseIndex i=0;i<size();i++) {
+      for (DenseIndex i = 0; i < size(); i++) {
         Matrix& Hi = H->at(i);
-        Hi.resize(T::dimension, dims[i]);
+        Hi.resize(T::dimension, dimensions_[i]);
         Hi.setZero(); // zero out
-        Eigen::Block<Matrix> block = Hi.block(0,0,T::dimension, dims[i]);
+        Eigen::Block<Matrix> block = Hi.block(0, 0, T::dimension,
+            dimensions_[i]);
         blocks.insert(std::make_pair(keys_[i], block));
       }
 
@@ -81,23 +94,18 @@ public:
 
   virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
 
-    // Construct VerticalBlockMatrix and views into it
-
-    // 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);
+    double memory[T::dimension * augmentedCols_];
-    double memory[T::dimension*m1];
+    Eigen::Map<Eigen::Matrix<double, T::dimension, Eigen::Dynamic> > //
-    Eigen::Map<Eigen::Matrix<double,T::dimension,Eigen::Dynamic> > matrix(memory,T::dimension,m1);
+    matrix(memory, T::dimension, augmentedCols_);
     matrix.setZero(); // zero out
 
     // Construct block matrix, is of right size but un-initialized
-    VerticalBlockMatrix Ab(dims, matrix, true);
+    VerticalBlockMatrix Ab(dimensions_, matrix, true);
 
-    // Create blocks to be passed to expression_
+    // Create blocks into Ab_ to be passed to expression_
     JacobianMap blocks;
-    for(DenseIndex i=0;i<size();i++)
+    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);