diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index a2e9fb273..42d9ad598 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -20,6 +20,7 @@ #include #include #include +#include namespace gtsam { @@ -78,30 +79,23 @@ public: } } - // Internal function to allocate a VerticalBlockMatrix and - // create Eigen::Block views into it - VerticalBlockMatrix prepareBlocks(JacobianMap& blocks) const { - - // Get dimensions of Jacobian matrices - std::vector dims = expression_.dimensions(); - - // Construct block matrix, is of right size but un-initialized - VerticalBlockMatrix Ab(dims, T::dimension, true); - Ab.matrix().setZero(); // zero out - - // Create blocks to be passed to expression_ - for(DenseIndex i=0;i linearize(const Values& x) const { // Construct VerticalBlockMatrix and views into it - JacobianMap blocks; - VerticalBlockMatrix Ab = prepareBlocks(blocks); + // Get dimensions of Jacobian matrices + std::vector dims = expression_.dimensions(); + size_t m = std::accumulate(dims.rend(),dims.rbegin(),0); + Matrix matrix(T::dimension,m); + + // Construct block matrix, is of right size but un-initialized + VerticalBlockMatrix Ab(dims, matrix, true); + Ab.matrix().setZero(); // zero out + + // Create blocks to be passed to expression_ + JacobianMap blocks; + for(DenseIndex i=0;i