From 4b95a2799b25b69c2544f222b9ecd53a7e401e47 Mon Sep 17 00:00:00 2001
From: Richard Roberts <richard.jw.roberts@gmail.com>
Date: Wed, 18 Jan 2012 20:44:27 +0000
Subject: [PATCH] Bug fix in converting HessianFactor to JacobianFactor - had
 to zero out lower triangle of eliminated matrix.  Additional unit tests to
 catch this.

---
 gtsam/linear/JacobianFactor.cpp               |  3 +++
 .../linear/tests/testGaussianJunctionTree.cpp | 26 ++++++++++++-------
 gtsam/linear/tests/testJacobianFactor.cpp     | 22 ++++++++++++++++
 3 files changed, 41 insertions(+), 10 deletions(-)

diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index 0d9367f49..27065b9a7 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -164,6 +164,9 @@ namespace gtsam {
     keys_ = factor.keys_;
     Ab_.assignNoalias(factor.info_);
     size_t maxrank = choleskyCareful(matrix_).first;
+    // Zero out lower triangle
+    matrix_.topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
+        Matrix::Zero(maxrank, matrix_.cols());
     // FIXME: replace with triangular system
     Ab_.rowEnd() = maxrank;
     model_ = noiseModel::Unit::Create(maxrank);
diff --git a/gtsam/linear/tests/testGaussianJunctionTree.cpp b/gtsam/linear/tests/testGaussianJunctionTree.cpp
index 7a5358a4a..2462f5255 100644
--- a/gtsam/linear/tests/testGaussianJunctionTree.cpp
+++ b/gtsam/linear/tests/testGaussianJunctionTree.cpp
@@ -236,12 +236,15 @@ TEST(GaussianJunctionTree, complicatedMarginal) {
 
   // Marginal on 5
   Matrix expectedCov = (Matrix(1,1) << 236.5166).finished();
-  JacobianFactor::shared_ptr actualJacobian = boost::dynamic_pointer_cast<JacobianFactor>(
+  JacobianFactor::shared_ptr actualJacobianLDL = boost::dynamic_pointer_cast<JacobianFactor>(
       bt.marginalFactor(5, EliminateLDL));
-  LONGS_EQUAL(1, actualJacobian->rows());
-  LONGS_EQUAL(1, actualJacobian->size());
-  LONGS_EQUAL(5, actualJacobian->keys()[0]);
-  Matrix actualA = actualJacobian->getA(actualJacobian->begin());
+  JacobianFactor::shared_ptr actualJacobianQR = boost::dynamic_pointer_cast<JacobianFactor>(
+      bt.marginalFactor(5, EliminateQR));
+  CHECK(assert_equal(*actualJacobianLDL, *actualJacobianQR)); // Check that LDL and QR obtained marginals are the same
+  LONGS_EQUAL(1, actualJacobianLDL->rows());
+  LONGS_EQUAL(1, actualJacobianLDL->size());
+  LONGS_EQUAL(5, actualJacobianLDL->keys()[0]);
+  Matrix actualA = actualJacobianLDL->getA(actualJacobianLDL->begin());
   Matrix actualCov = inverse(actualA.transpose() * actualA);
   EXPECT(assert_equal(expectedCov, actualCov, 1e-1));
 
@@ -252,12 +255,15 @@ TEST(GaussianJunctionTree, complicatedMarginal) {
   expectedCov = (Matrix(2,2) <<
       1015.8,    2886.2,
       2886.2,    8471.2).finished();
-  actualJacobian = boost::dynamic_pointer_cast<JacobianFactor>(
+  actualJacobianLDL = boost::dynamic_pointer_cast<JacobianFactor>(
       bt.marginalFactor(6, EliminateLDL));
-  LONGS_EQUAL(2, actualJacobian->rows());
-  LONGS_EQUAL(1, actualJacobian->size());
-  LONGS_EQUAL(6, actualJacobian->keys()[0]);
-  actualA = actualJacobian->getA(actualJacobian->begin());
+  actualJacobianQR = boost::dynamic_pointer_cast<JacobianFactor>(
+      bt.marginalFactor(6, EliminateQR));
+  CHECK(assert_equal(*actualJacobianLDL, *actualJacobianQR)); // Check that LDL and QR obtained marginals are the same
+  LONGS_EQUAL(2, actualJacobianLDL->rows());
+  LONGS_EQUAL(1, actualJacobianLDL->size());
+  LONGS_EQUAL(6, actualJacobianLDL->keys()[0]);
+  actualA = actualJacobianLDL->getA(actualJacobianLDL->begin());
   actualCov = inverse(actualA.transpose() * actualA);
   EXPECT(assert_equal(expectedCov, actualCov, 1e1));
 
diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp
index 8b287a491..c78618ff7 100644
--- a/gtsam/linear/tests/testJacobianFactor.cpp
+++ b/gtsam/linear/tests/testJacobianFactor.cpp
@@ -20,6 +20,7 @@
 #include <CppUnitLite/TestHarness.h>
 
 #include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/HessianFactor.h>
 #include <gtsam/linear/GaussianConditional.h>
 
 using namespace std;
@@ -70,6 +71,27 @@ TEST(JacobianFactor, constructor2)
   EXPECT(assert_equal(b, actualb));
 }
 
+/* ************************************************************************* */
+TEST(JabobianFactor, Hessian_conversion) {
+  HessianFactor hessian(0, (Matrix(4,4) <<
+        1.57,        2.695,         -1.1,        -2.35,
+       2.695,      11.3125,        -0.65,      -10.225,
+        -1.1,        -0.65,            1,          0.5,
+       -2.35,      -10.225,          0.5,         9.25).finished(),
+      (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
+      73.1725);
+
+  JacobianFactor expected(0, (Matrix(2,4) <<
+      1.2530,   2.1508,   -0.8779,  -1.8755,
+           0,   2.5858,    0.4789,  -2.3943).finished(),
+      (Vector(2) << -6.2929, -5.7941).finished(),
+      sharedUnit(2));
+
+  JacobianFactor actual(hessian);
+
+  EXPECT(assert_equal(expected, actual, 1e-3));
+}
+
 /* ************************************************************************* */
 TEST(JacobianFactor, error) {
   Vector b = Vector_(3, 1., 2., 3.);