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

@@ -102,10 +102,9 @@ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
 }
 
 /* ************************************************************************* */
-JacobianFactor::JacobianFactor(const HessianFactor& factor) :
+JacobianFactor::JacobianFactor(const HessianFactor& factor)
-    Base(factor), Ab_(
+    : Base(factor),
-        VerticalBlockMatrix::LikeActiveViewOf(factor.info(),
+      Ab_(VerticalBlockMatrix::LikeActiveViewOf(factor.info(), factor.rows())) {
-            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
+  // Check that Cholesky succeeded OR it managed to factor the full Hessian.
-  if (!success and maxrank < factor.rows() - 1)
+  // 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);
 }
 
 /* ************************************************************************* */

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 {