diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index bc0edbbb5..b6bfba27f 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -85,12 +85,15 @@ public: // 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); - Matrix matrix = Matrix::Identity(T::dimension,m1); + 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, matrix, true); - Ab.matrix().setZero(); // zero out // Create blocks to be passed to expression_ JacobianMap blocks;