diff --git a/gtsam/base/VerticalBlockMatrix.h b/gtsam/base/VerticalBlockMatrix.h index 029f55c58..c09cc7577 100644 --- a/gtsam/base/VerticalBlockMatrix.h +++ b/gtsam/base/VerticalBlockMatrix.h @@ -74,8 +74,8 @@ namespace gtsam { } /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ - template - VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) : + template + VerticalBlockMatrix(const CONTAINER& dimensions, const Eigen::MatrixBase& matrix, bool appendOneDimension = false) : matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) { fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index a2e9fb273..b6bfba27f 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,26 @@ public: } } - // Internal function to allocate a VerticalBlockMatrix and - // create Eigen::Block views into it - VerticalBlockMatrix prepareBlocks(JacobianMap& blocks) const { + 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); + matrix.setZero(); // zero out + // Construct block matrix, is of right size but un-initialized - VerticalBlockMatrix Ab(dims, T::dimension, true); - Ab.matrix().setZero(); // zero out + VerticalBlockMatrix Ab(dims, matrix, true); // Create blocks to be passed to expression_ + JacobianMap blocks; for(DenseIndex i=0;i linearize(const Values& x) const { - - // Construct VerticalBlockMatrix and views into it - JacobianMap blocks; - VerticalBlockMatrix Ab = prepareBlocks(blocks); - // Evaluate error to get Jacobians and RHS vector b T value = expression_.value(x, blocks); Ab(size()).col(0) = -measurement_.localCoordinates(value);