From b031996bbc05675824963e3d164fae549821dc0a Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Wed, 18 Jan 2012 15:42:00 +0000
Subject: [PATCH] Fixed denseHessian bug - was only returning upper triangle,
 now returns full matrix

---
 gtsam/linear/GaussianFactorGraph.cpp          |  7 +++--
 gtsam/linear/HessianFactor.h                  | 10 +++++--
 .../linear/tests/testGaussianFactorGraph.cpp  | 26 +++++++++++++++++++
 3 files changed, 39 insertions(+), 4 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 4107d347b..d3124ae65 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -346,9 +346,12 @@ namespace gtsam {
     }
     dims.push_back(1);
 
-    // combine all factors
+    // combine all factors and get upper-triangular part of Hessian
     HessianFactor combined(*this, dims, scatter);
-    return combined.info();
+    Matrix result = combined.info();
+    // Fill in lower-triangular part of Hessian
+    result.triangularView<Eigen::StrictlyLower>() = result.transpose();
+    return result;
   }
 
 	/* ************************************************************************* */
diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h
index 22a107691..988b45a5c 100644
--- a/gtsam/linear/HessianFactor.h
+++ b/gtsam/linear/HessianFactor.h
@@ -226,7 +226,10 @@ namespace gtsam {
     /** Return the number of columns and rows of the Hessian matrix */
     size_t rows() const { return info_.rows(); }
 
-    /** Return a view of the block at (j1,j2) of the information matrix \f$ H \f$, no data is copied.
+    /** Return a view of the block at (j1,j2) of the <emph>upper-triangular part</emph> of the
+     * information matrix \f$ H \f$, no data is copied.  See HessianFactor class documentation
+     * above to explain that only the upper-triangular part of the information matrix is stored
+     * and returned by this function.
      * @param j1 Which block row to get, as an iterator pointing to the slot in this factor.  You can
      * use, for example, begin() + 2 to get the 3rd variable in this factor.
      * @param j2 Which block column to get, as an iterator pointing to the slot in this factor.  You can
@@ -235,7 +238,10 @@ namespace gtsam {
      */
     constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }
 
-    /** Return the full *augmented* information matrix, as described above */
+    /** Return the <emph>upper-triangular part</emph> of the full *augmented* information matrix,
+     * as described above.  See HessianFactor class documentation above to explain that only the
+     * upper-triangular part of the information matrix is stored and returned by this function.
+     */
     constBlock info() const { return info_.full(); }
 
     /** Return the constant term \f$ f \f$ as described above
diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp
index 31c0bd671..ade5e65ce 100644
--- a/gtsam/linear/tests/testGaussianFactorGraph.cpp
+++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp
@@ -610,6 +610,32 @@ TEST(GaussianFactorGraph, sparseJacobian) {
   EXPECT(assert_equal(expected, actual));
 }
 
+/* ************************************************************************* */
+TEST(GaussianFactorGraph, denseHessian) {
+  // Create factor graph:
+  // x1 x2 x3 x4 x5  b
+  //  1  2  3  0  0  4
+  //  5  6  7  0  0  8
+  //  9 10  0 11 12 13
+  //  0  0  0 14 15 16
+
+  GaussianFactorGraph gfg;
+  SharedDiagonal model = sharedUnit(2);
+  gfg.add(0, Matrix_(2,3, 1., 2., 3., 5., 6., 7.), Vector_(2, 4., 8.), model);
+  gfg.add(0, Matrix_(2,3, 9.,10., 0., 0., 0., 0.), 1, Matrix_(2,2, 11., 12., 14., 15.), Vector_(2, 13.,16.), model);
+
+  Matrix jacobian(4,6);
+  jacobian <<
+      1, 2, 3, 0, 0, 4,
+      5, 6, 7, 0, 0, 8,
+      9,10, 0,11,12,13,
+      0, 0, 0,14,15,16;
+
+  Matrix expectedHessian = jacobian.transpose() * jacobian;
+  Matrix actualHessian = gfg.denseHessian();
+  EXPECT(assert_equal(expectedHessian, actualHessian));
+}
+
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
 /* ************************************************************************* */