diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 304686b0e..f98287809 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -215,6 +215,7 @@ bool GaussianFactorGraph::getDiagonalOfHessian(VectorValues &values) const {
 		BOOST_FOREACH( const Index& idx, factor->keys_) {
 			Vector v = columnNormSquare(factor->Ab_(i)) ;
 			values[idx] += v;
+			++i ;
 		}
 	}
 	return true ;