diff --git a/gtsam_unstable/nonlinear/Expression.h b/gtsam_unstable/nonlinear/Expression.h index 5eca4bf84..78c4f0707 100644 --- a/gtsam_unstable/nonlinear/Expression.h +++ b/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 > root_; - - // Fixed dimensions: an Expression is assumed unmutable - const std::vector dimensions_; + boost::shared_ptr > root_; public: // Construct a constant expression Expression(const T& value) : - root_(new ConstantExpression(value)), // - dimensions_(root_->dimensions()) { + root_(new ConstantExpression(value)) { } // Construct a leaf expression, with Key Expression(const Key& key) : - root_(new LeafExpression(key)), // - dimensions_(root_->dimensions()) { + root_(new LeafExpression(key)) { } // Construct a leaf expression, with Symbol Expression(const Symbol& symbol) : - root_(new LeafExpression(symbol)), // - dimensions_(root_->dimensions()) { + root_(new LeafExpression(symbol)) { } // Construct a leaf expression, creating Symbol Expression(unsigned char c, size_t j) : - root_(new LeafExpression(Symbol(c, j))), // - dimensions_(root_->dimensions()) { + root_(new LeafExpression(Symbol(c, j))) { } /// Construct a nullary method expression template Expression(const Expression& expression, T (A::*method)(typename Optional::type) const) : - root_(new UnaryExpression(boost::bind(method, _1, _2), expression)), // - dimensions_(root_->dimensions()) { + root_(new UnaryExpression(boost::bind(method, _1, _2), expression)) { } /// Construct a unary function expression template Expression(typename UnaryExpression::Function function, const Expression& expression) : - root_(new UnaryExpression(function, expression)), // - dimensions_(root_->dimensions()) { + root_(new UnaryExpression(function, expression)) { } /// Construct a unary method expression @@ -89,8 +80,7 @@ public: const Expression& expression2) : root_( new BinaryExpression(boost::bind(method, _1, _2, _3, _4), - expression1, expression2)), // - dimensions_(root_->dimensions()) { + expression1, expression2)) { } /// Construct a binary function expression @@ -98,8 +88,7 @@ public: Expression(typename BinaryExpression::Function function, const Expression& expression1, const Expression& expression2) : root_( - new BinaryExpression(function, expression1, expression2)), // - dimensions_(root_->dimensions()) { + new BinaryExpression(function, expression1, expression2)) { } /// Construct a ternary function expression @@ -109,8 +98,7 @@ public: const Expression& expression3) : root_( new TernaryExpression(function, expression1, - expression2, expression3)), // - dimensions_(root_->dimensions()) { + expression2, expression3)) { } /// 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& dimensions() const { - return dimensions_; + std::vector dimensions() const { + return root_->dimensions(); } // Return size needed for memory buffer in traceExecution diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index b6bfba27f..013623bf5 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -30,8 +30,10 @@ namespace gtsam { template class ExpressionFactor: public NoiseModelFactor { - const T measurement_; - const Expression expression_; + T measurement_; ///< the measurement to be compared with the expression + Expression expression_; ///< the expression that is AD enabled + std::vector 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 dims = expression_.dimensions(); - // Create and zero out blocks to be passed to expression_ JacobianMap blocks; - for(DenseIndex i=0;iat(i); - Hi.resize(T::dimension, dims[i]); + Hi.resize(T::dimension, dimensions_[i]); Hi.setZero(); // zero out - Eigen::Block block = Hi.block(0,0,T::dimension, dims[i]); + Eigen::Block 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 linearize(const Values& x) const { - // Construct VerticalBlockMatrix and views into it - - // Get dimensions of Jacobian matrices - std::vector 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*m1]; - Eigen::Map > matrix(memory,T::dimension,m1); + double memory[T::dimension * augmentedCols_]; + Eigen::Map > // + 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