diff --git a/gtsam_unstable/nonlinear/ExpressionFactor.h b/gtsam_unstable/nonlinear/ExpressionFactor.h index 42d9ad598..bc0edbbb5 100644 --- a/gtsam_unstable/nonlinear/ExpressionFactor.h +++ b/gtsam_unstable/nonlinear/ExpressionFactor.h @@ -85,8 +85,8 @@ public: // 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); + size_t m1 = std::accumulate(dims.begin(),dims.end(),1); + Matrix matrix = Matrix::Identity(T::dimension,m1); // Construct block matrix, is of right size but un-initialized VerticalBlockMatrix Ab(dims, matrix, true);