fixed bug in hessianDiagonal (raw access)

examples/Data/dubrovnik-3-7-18-pre.txt

@@ -0,0 +1,79 @@
+3 7 18
+
+0 0     -3.859900e+02 3.871200e+02
+1 0     -3.844000e+01 4.921200e+02
+2 0     -6.679200e+02 1.231100e+02
+0 1     3.838800e+02 -1.529999e+01
+1 1     5.597500e+02 -1.061500e+02
+0 2     5.915500e+02 1.364400e+02
+1 2     8.638600e+02 -2.346997e+01
+2 2     4.947200e+02 1.125200e+02
+0 3     5.925000e+02 1.257500e+02
+1 3     8.610800e+02 -3.521997e+01
+2 3     4.985400e+02 1.015600e+02
+0 4     3.487200e+02 5.583800e+02
+1 4     7.760300e+02 4.835300e+02
+2 4     7.780029e+00 3.263500e+02
+0 5     1.401001e+01 9.642001e+01
+1 5     2.071300e+02 1.183600e+02
+1 6     5.431801e+02 2.948100e+02
+2 6     -5.841998e+01 1.108300e+02
+
+-1.6943983532198115e-02
+1.1171804676513932e-02
+2.4643508831711991e-03
+7.3030995682610689e-01
+-2.6490818471043420e-01
+-1.7127892627337182e+00
+1.4300319432711681e+03
+-7.5572758535864072e-08
+3.2377569465570913e-14
+
+1.5049725341485708e-02
+-1.8504564785154357e-01
+-2.9278402790141456e-01
+-1.0590476152349551e+00
+-3.6017862414345798e-02
+-1.5720340175803784e+00
+1.4321374541298685e+03
+-7.3171919892612292e-08
+3.1759419019880947e-14
+
+-3.0793597986873011e-01
+3.2077907982952031e-01
+2.2253985096991455e-01
+8.5034483295909009e+00
+6.7499603629668741e+00
+-3.6383814384447088e+00
+1.5720470590375264e+03
+-1.5962623661947355e-08
+-1.6507904848058800e-14
+
+-1.2055995050700867e+01
+1.2838775976205760e+01
+-4.1099369264082803e+01
+
+6.4168905904672933e+00
+3.8897031177598462e-01
+-2.3586282709150449e+01
+
+1.3051100355717297e+01
+3.8387587111611952e+00
+-2.9777932175344951e+01
+
+1.3060946673472820e+01
+3.5910521225905803e+00
+-2.9759080795372942e+01
+
+1.4265764475421857e+01
+2.4096216156436530e+01
+-5.4823971067225500e+01
+
+-2.5292283211391348e-01
+2.2166082122808284e+00
+-2.1712127480255084e+01
+
+7.6465738085189585e+00
+1.4185331909846619e+01
+-5.2070299568846060e+01
+

gtsam/linear/HessianFactor.cpp

@@ -367,9 +367,10 @@ void HessianFactor::hessianDiagonal(double* d) const {
   typedef Eigen::Map<DVector> DMap;
 
   // Loop over all variables in the factor
-    for (DenseIndex j = 0; j < (DenseIndex)size(); ++j) {
+    for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
+      Key j = keys_[pos];
       // Get the diagonal block, and insert its diagonal
-      Matrix B = info_(j, j).selfadjointView();
+      Matrix B = info_(pos, pos).selfadjointView();
       DVector dj =  B.diagonal();
       DMap(d + 9 * j) += dj;
     }

gtsam/linear/tests/testHessianFactorUnordered.cpp

@@ -471,7 +471,7 @@ TEST(HessianFactor, hessianDiagonal)
   LONGS_EQUAL(2,actualBD.size());
   EXPECT(assert_equal(G11,actualBD[0]));
   EXPECT(assert_equal(G22,actualBD[1]));
-  }
+}
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}