diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 5f48be50b..814d4eede 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -325,17 +325,11 @@ double HessianFactor::error(const VectorValues& c) const { // error 0.5*(f - 2*x'*g + x'*G*x) const double f = constantTerm(); double xtg = 0, xGx = 0; - if (c.dim() == this->rows()) { - // If using the full vector values, this will reduce copying - xtg = c.vector().dot(linearTerm()); - xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * c.asVector(); - } else { - // extract the relevant subset of the VectorValues - // NOTE may not be as efficient - const Vector x = c.vector(this->keys()); - xtg = x.dot(linearTerm()); - xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; - } + // extract the relevant subset of the VectorValues + // NOTE may not be as efficient + const Vector x = c.vector(this->keys()); + xtg = x.dot(linearTerm()); + xGx = x.transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * x; return 0.5 * (f - 2.0 * xtg + xGx); }