From 57716646227b619272f7fa00f11a488489c0ba91 Mon Sep 17 00:00:00 2001
From: dellaert <dellaert@cc.gatech.edu>
Date: Sun, 14 Jun 2015 16:01:54 -0700
Subject: [PATCH] Starting to diagnose issue with lower-left entry of Hessian

---
 gtsam/linear/HessianFactor.cpp           | 15 ++++-----------
 gtsam/linear/JacobianFactor.cpp          |  8 ++++----
 gtsam/linear/tests/testHessianFactor.cpp | 14 ++++++++------
 3 files changed, 16 insertions(+), 21 deletions(-)

diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index bbc684a10..21f4b117f 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -268,18 +268,11 @@ void HessianFactor::print(const std::string& s,
 
 /* ************************************************************************* */
 bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
-  if (!dynamic_cast<const HessianFactor*>(&lf))
+  const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&lf);
+  if (!rhs || !Factor::equals(lf, tol))
     return false;
-  else {
-    if (!Factor::equals(lf, tol))
-      return false;
-    Matrix thisMatrix = info_.full().selfadjointView();
-    thisMatrix(thisMatrix.rows() - 1, thisMatrix.cols() - 1) = 0.0;
-    Matrix rhsMatrix =
-        static_cast<const HessianFactor&>(lf).info_.full().selfadjointView();
-    rhsMatrix(rhsMatrix.rows() - 1, rhsMatrix.cols() - 1) = 0.0;
-    return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
-  }
+  return equal_with_abs_tol(augmentedInformation(), rhs->augmentedInformation(),
+      tol);
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 8214294b2..b38aa89e7 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -66,10 +66,10 @@ JacobianFactor::JacobianFactor() :
 /* ************************************************************************* */
 JacobianFactor::JacobianFactor(const GaussianFactor& gf) {
   // Copy the matrix data depending on what type of factor we're copying from
-  if (const JacobianFactor* rhs = dynamic_cast<const JacobianFactor*>(&gf))
-    *this = JacobianFactor(*rhs);
-  else if (const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&gf))
-    *this = JacobianFactor(*rhs);
+  if (const JacobianFactor* asJacobian = dynamic_cast<const JacobianFactor*>(&gf))
+    *this = JacobianFactor(*asJacobian);
+  else if (const HessianFactor* asHessian = dynamic_cast<const HessianFactor*>(&gf))
+    *this = JacobianFactor(*asHessian);
   else
     throw std::invalid_argument(
         "In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");
diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp
index 96e61aa33..afd611112 100644
--- a/gtsam/linear/tests/testHessianFactor.cpp
+++ b/gtsam/linear/tests/testHessianFactor.cpp
@@ -286,8 +286,8 @@ TEST(HessianFactor, CombineAndEliminate)
       1.0, 0.0, 0.0,
       0.0, 1.0, 0.0,
       0.0, 0.0, 1.0).finished();
-  Vector3 b0(1.5, 1.5, 1.5);
-  Vector3 s0(1.6, 1.6, 1.6);
+  Vector3 b0(1,0,0);//(1.5, 1.5, 1.5);
+  Vector3 s0=Vector3::Ones();//(1.6, 1.6, 1.6);
 
   Matrix A10 = (Matrix(3,3) <<
       2.0, 0.0, 0.0,
@@ -297,15 +297,15 @@ TEST(HessianFactor, CombineAndEliminate)
       -2.0, 0.0, 0.0,
       0.0, -2.0, 0.0,
       0.0, 0.0, -2.0).finished();
-  Vector3 b1(2.5, 2.5, 2.5);
-  Vector3 s1(2.6, 2.6, 2.6);
+  Vector3 b1 = Vector3::Zero();//(2.5, 2.5, 2.5);
+  Vector3 s1=Vector3::Ones();//(2.6, 2.6, 2.6);
 
   Matrix A21 = (Matrix(3,3) <<
       3.0, 0.0, 0.0,
       0.0, 3.0, 0.0,
       0.0, 0.0, 3.0).finished();
-  Vector3 b2(3.5, 3.5, 3.5);
-  Vector3 s2(3.6, 3.6, 3.6);
+  Vector3 b2 = Vector3::Zero();//(3.5, 3.5, 3.5);
+  Vector3 s2=Vector3::Ones();//(3.6, 3.6, 3.6);
 
   GaussianFactorGraph gfg;
   gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@@ -334,6 +334,8 @@ TEST(HessianFactor, CombineAndEliminate)
   boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0)));
 
   EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
+  VectorValues vv; vv.insert(1, Vector3(1,2,3));
+  EXPECT_DOUBLES_EQUAL(expectedRemainingFactor->error(vv), actualCholeskyFactor->error(vv), 1e-9);
   EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6));
 }