Address comments

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);
     }
   }

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);

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