diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 06d2ece7b..8a9adf6f6 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -313,8 +313,9 @@ VectorValues HessianFactor::hessianDiagonal() const { /* ************************************************************************* */ void HessianFactor::hessianDiagonalAdd(VectorValues &d) const { for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { - if(d.exists(keys_[j])) { - d.at(keys_[j]) += info_.diagonal(j); + auto item = d.find(keys_[j]); + if(item != d.end()) { + item->second += info_.diagonal(j); } else { d.emplace(keys_[j], info_.diagonal(j)); } diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 2e243c033..3f6eba5f3 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -560,8 +560,9 @@ void JacobianFactor::hessianDiagonalAdd(VectorValues& d) const { model_->whitenInPlace(column_k); dj(k) = dot(column_k, column_k); } - if(d.exists(j)) { - d.at(j) += dj; + auto item = d.find(j); + if(item != d.end()) { + item->second += dj; } else { d.emplace(j, dj); }