diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 6a980eac2..02948fc04 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -315,6 +315,7 @@ void HessianFactor::hessianDiagonalAdd(VectorValues &d) const { for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) { auto result = d.emplace(keys_[j], info_.diagonal(j)); if(!result.second) { + // if emplace fails, it returns an iterator to the existing element, which we add to: result.first->second += info_.diagonal(j); } } diff --git a/gtsam/linear/linearAlgorithms-inst.h b/gtsam/linear/linearAlgorithms-inst.h index e339347f1..811e07121 100644 --- a/gtsam/linear/linearAlgorithms-inst.h +++ b/gtsam/linear/linearAlgorithms-inst.h @@ -100,9 +100,9 @@ namespace gtsam for(GaussianConditional::const_iterator frontal = c.beginFrontals(); frontal != c.endFrontals(); ++frontal) { auto result = collectedResult.emplace(*frontal, solution.segment(vectorPosition, c.getDim(frontal))); if(!result.second) - throw std::invalid_argument( - "Requested to emplace variable '" + DefaultKeyFormatter(*frontal) - + "' already in this VectorValues."); + throw std::runtime_error( + "Internal error while optimizing clique. Trying to insert key '" + DefaultKeyFormatter(*frontal) + + "' that exists."); VectorValues::const_iterator r = result.first; myData.cliqueResults.emplace(r->first, r); diff --git a/gtsam/slam/RegularImplicitSchurFactor.h b/gtsam/slam/RegularImplicitSchurFactor.h index 4443d6a6d..8be1b7bb6 100644 --- a/gtsam/slam/RegularImplicitSchurFactor.h +++ b/gtsam/slam/RegularImplicitSchurFactor.h @@ -151,7 +151,7 @@ public: return d; } - /// Return the diagonal of the Hessian for this factor + /// Add the diagonal of the Hessian for this factor to existing VectorValues virtual void hessianDiagonalAdd(VectorValues &d) const override { // diag(Hessian) = diag(F' * (I - E * PointCov * E') * F); for (size_t k = 0; k < size(); ++k) { // for each camera