From 372ae8d09275c0c01b1b36d9a58720e6a80da8fb Mon Sep 17 00:00:00 2001
From: Frank Dellaert <dellaert@gmail.com>
Date: Thu, 25 Apr 2019 00:46:20 -0400
Subject: [PATCH] Fixed up conversion from Hessian -> Jacobian to accommodate
 QPSolver (same solution as Ivan, now tested that it does not interfere with
 normal workings)

---
 gtsam/linear/JacobianFactor.cpp           | 28 ++++++++++++-----------
 gtsam/linear/tests/testJacobianFactor.cpp | 21 +++++++++++++++++
 2 files changed, 36 insertions(+), 13 deletions(-)

diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp
index cba6980fa..06f6b3bed 100644
--- a/gtsam/linear/JacobianFactor.cpp
+++ b/gtsam/linear/JacobianFactor.cpp
@@ -102,10 +102,9 @@ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
 }
 
 /* ************************************************************************* */
-JacobianFactor::JacobianFactor(const HessianFactor& factor) :
-    Base(factor), Ab_(
-        VerticalBlockMatrix::LikeActiveViewOf(factor.info(),
-            factor.rows())) {
+JacobianFactor::JacobianFactor(const HessianFactor& factor)
+    : Base(factor),
+      Ab_(VerticalBlockMatrix::LikeActiveViewOf(factor.info(), factor.rows())) {
   // Copy Hessian into our matrix and then do in-place Cholesky
   Ab_.full() = factor.info().selfadjointView();
 
@@ -114,16 +113,19 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) :
   bool success;
   boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix());
 
-  // Check for indefinite system
-  if (!success and maxrank < factor.rows() - 1)
+  // Check that Cholesky succeeded OR it managed to factor the full Hessian.
+  // THe latter case occurs with non-positive definite matrices arising from QP.
+  if (success || maxrank == factor.rows() - 1) {
+    // Zero out lower triangle
+    Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
+        Matrix::Zero(maxrank, Ab_.matrix().cols());
+    // FIXME: replace with triangular system
+    Ab_.rowEnd() = maxrank;
+    model_ = SharedDiagonal();  // is equivalent to Unit::Create(maxrank)
+  } else {
+    // indefinite system
     throw IndeterminantLinearSystemException(factor.keys().front());
-
-  // Zero out lower triangle
-  Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
-      Matrix::Zero(maxrank, Ab_.matrix().cols());
-  // FIXME: replace with triangular system
-  Ab_.rowEnd() = maxrank;
-  model_ = SharedDiagonal(); // should be same as Unit::Create(maxrank);
+  }
 }
 
 /* ************************************************************************* */
diff --git a/gtsam/linear/tests/testJacobianFactor.cpp b/gtsam/linear/tests/testJacobianFactor.cpp
index f6ab4be73..98eaf740c 100644
--- a/gtsam/linear/tests/testJacobianFactor.cpp
+++ b/gtsam/linear/tests/testJacobianFactor.cpp
@@ -162,6 +162,27 @@ TEST(JabobianFactor, Hessian_conversion) {
   EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3));
 }
 
+/* ************************************************************************* */
+TEST(JabobianFactor, Hessian_conversion2) {
+  JacobianFactor jf(0, (Matrix(3,3) <<
+      1, 2, 3,
+      0, 0, 3, 
+      2, 1, 1).finished(),
+    Vector3(1, 2, 2));
+  HessianFactor hessian(jf);
+  EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9));
+}
+
+/* ************************************************************************* */
+TEST(JabobianFactor, Hessian_conversion3) {
+  JacobianFactor jf(0, (Matrix(2,4) <<
+      1, 2, 3, 0,
+      0, 3, 2, 1).finished(),
+    Vector2(1, 2));
+  HessianFactor hessian(jf);
+  EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9));
+}
+
 /* ************************************************************************* */
 namespace simple_graph {