Support non positive definite Hessian factors while doing EliminatePreferCholesky with some constrained factors.

Currently, when eliminating a constrained variable, EliminatePreferCholesky converts every other factors to JacobianFactor before doing the special QR factorization for constrained variables. Unfortunately, after a constrained nonlinear graph is linearized, new hessian factors from constraints, multiplied with the dual variable  (-lambda*\hessian{c} terms in the Lagrangian objective function), might become negative definite, thus cannot be converted to JacobianFactors.

Following EliminateCholesky, this version of EliminatePreferCholesky for constrained var gathers all unconstrained factors into a big joint HessianFactor before converting it into a JacobianFactor to be eliminiated by QR together with the other constrained factors.

Of course, this might not solve the non-positive-definite problem entirely, because (1) the original hessian factors might be non-positive definite and (2) large strange value of lambdas might cause the joint factor non-positive definite [is this true?]. But at least, this will help in typical cases.

.gitignore

@@ -1,3 +1,5 @@
 /build*
 *.pyc
 *.DS_Store
+/debug/
+*.txt.user

gtsam/linear/GaussianFactorGraph.cpp

@@ -370,6 +370,22 @@ namespace gtsam {
     return false;
   }
 
+  /* ************************************************************************* */
+  std::pair<GaussianFactorGraph, GaussianFactorGraph> GaussianFactorGraph::splitConstraints() const {
+    typedef JacobianFactor J;
+    GaussianFactorGraph unconstraints, constraints;
+    BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *this) {
+      J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
+      if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) {
+        constraints.push_back(jacobian);
+      }
+      else {
+        unconstraints.push_back(factor);
+      }
+    }
+    return make_pair(unconstraints, constraints);
+  }
+
   /* ************************************************************************* */
   // x += alpha*A'*e
   void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,

gtsam/linear/GaussianFactorGraph.h

@@ -313,6 +313,13 @@ namespace gtsam {
 
     /// @}
 
+    /**
+     * Split constraints and unconstrained factors into two different graphs
+     * @return a pair of <unconstrained, constrained> graphs
+     */
+    std::pair<GaussianFactorGraph, GaussianFactorGraph> splitConstraints() const;
+
+
   private:
     /** Serialization function */
     friend class boost::serialization::access;

gtsam/linear/HessianFactor.cpp

@@ -641,8 +641,21 @@ EliminatePreferCholesky(const GaussianFactorGraph& factors, const Ordering& keys
   // all factors to JacobianFactors.  Otherwise, we can convert all factors
   // to HessianFactors.  This is because QR can handle constrained noise
   // models but Cholesky cannot.
-  if (hasConstraints(factors))
+  GaussianFactorGraph unconstraints, constraints;
-    return EliminateQR(factors, keys);
+  boost::tie(unconstraints, constraints) = factors.splitConstraints();
+  if (constraints.size()>0) {
+    // Build joint factor
+    HessianFactor::shared_ptr jointFactor;
+    try {
+      jointFactor = boost::make_shared<HessianFactor>(unconstraints, Scatter(factors, keys));
+    } catch(std::invalid_argument&) {
+      throw InvalidDenseElimination(
+          "EliminateCholesky was called with a request to eliminate variables that are not\n"
+          "involved in the provided factors.");
+    }
+    constraints.push_back(jointFactor);
+    return EliminateQR(constraints, keys);
+  }
   else
     return EliminateCholesky(factors, keys);
 }

gtsam/linear/JacobianFactor.cpp

@@ -115,6 +115,9 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) :
   bool success;
   boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix());
 
+  factor.print("HessianFactor to convert: ");
+  cout << "Maxrank: " << maxrank << ", success: " << int(success) << endl;
+
   // Check for indefinite system
   if (!success)
     throw IndeterminantLinearSystemException(factor.keys().front());

gtsam/linear/QPSolver.cpp

@@ -360,17 +360,15 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c
 }
 
 /* ************************************************************************* */
-VectorValues QPSolver::optimize(const VectorValues& initials,
+std::pair<VectorValues, VectorValues> QPSolver::optimize(const VectorValues& initials) const {
-    boost::optional<VectorValues&> lambdas) const {
   GaussianFactorGraph workingGraph = graph_.clone();
   VectorValues currentSolution = initials;
-  VectorValues workingLambdas;
+  VectorValues lambdas;
   bool converged = false;
   while (!converged) {
-    converged = iterateInPlace(workingGraph, currentSolution, workingLambdas);
+    converged = iterateInPlace(workingGraph, currentSolution, lambdas);
   }
-  if (lambdas) *lambdas = workingLambdas;
+  return make_pair(currentSolution, lambdas);
-  return currentSolution;
 }
 
 /* ************************************************************************* */
@@ -500,14 +498,14 @@ std::pair<bool, VectorValues> QPSolver::findFeasibleInitialValues() const {
 }
 
 /* ************************************************************************* */
-VectorValues QPSolver::optimize(boost::optional<VectorValues&> lambdas) const {
+std::pair<VectorValues, VectorValues> QPSolver::optimize() const {
   bool isFeasible;
   VectorValues initials;
   boost::tie(isFeasible, initials) = findFeasibleInitialValues();
   if (!isFeasible) {
     throw std::runtime_error("LP subproblem is infeasible!");
   }
-  return optimize(initials, lambdas);
+  return optimize(initials);
 }
 
 } /* namespace gtsam */

gtsam/linear/QPSolver.h

@@ -125,16 +125,17 @@ public:
    * a feasible initial value, otherwise the solution will be wrong.
    * If you don't know a feasible initial value, use the other version
    * of optimize().
+   * @return a pair of <primal, dual> solutions
    */
-  VectorValues optimize(const VectorValues& initials,
+  std::pair<VectorValues, VectorValues> optimize(const VectorValues& initials) const;
-      boost::optional<VectorValues&> lambdas = boost::none) const;
 
   /** Optimize without an initial value.
    * This version of optimize will try to find a feasible initial value by solving
    * an LP program before solving this QP graph.
    * TODO: If no feasible initial point exists, it should throw an InfeasibilityException!
+   * @return a pair of <primal, dual> solutions
    */
-  VectorValues optimize(boost::optional<VectorValues&> lambdas = boost::none) const;
+  std::pair<VectorValues, VectorValues> optimize() const;
 
 
   /**

gtsam/linear/tests/testQPSolver.cpp

@@ -165,7 +165,8 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
   VectorValues initials;
   initials.insert(X(1), zero(1));
   initials.insert(X(2), zero(1));
-  VectorValues solution = solver.optimize(initials);
+  VectorValues solution;
+  boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials);
   VectorValues expectedSolution;
   expectedSolution.insert(X(1), (Vector(1)<< 1.5));
   expectedSolution.insert(X(2), (Vector(1)<< 0.5));
@@ -205,7 +206,8 @@ TEST(QPSolver, optimizeMatlabEx) {
   VectorValues initials;
   initials.insert(X(1), zero(1));
   initials.insert(X(2), zero(1));
-  VectorValues solution = solver.optimize(initials);
+  VectorValues solution;
+  boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials);
   VectorValues expectedSolution;
   expectedSolution.insert(X(1), (Vector(1)<< 2.0/3.0));
   expectedSolution.insert(X(2), (Vector(1)<< 4.0/3.0));
@@ -239,7 +241,8 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) {
   initials.insert(X(1), (Vector(1)<<2.0));
   initials.insert(X(2), zero(1));
 
-  VectorValues solution = solver.optimize(initials);
+  VectorValues solution;
+  boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials);
   VectorValues expectedSolution;
   expectedSolution.insert(X(1), (Vector(1)<< 1.4));
   expectedSolution.insert(X(2), (Vector(1)<< 1.7));
@@ -310,7 +313,8 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) {
   EXPECT(assert_equal(1.0*ones(1), initials.at(X(1))));
   EXPECT(assert_equal(0.0*ones(1), initials.at(X(2))));
 
-  VectorValues solution = solver.optimize();
+  VectorValues solution;
+  boost::tie(solution, boost::tuples::ignore) = solver.optimize();
   EXPECT(assert_equal(2.0*ones(1), solution.at(X(1))));
   EXPECT(assert_equal(0.5*ones(1), solution.at(X(2))));
 }
@@ -326,14 +330,36 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4_2) {
   expectedSolution.insert(X(1), (Vector(1)<< 1.4));
   expectedSolution.insert(X(2), (Vector(1)<< 1.7));
 
-  VectorValues solution = solver.optimize(initials);
+  VectorValues solution;
+  boost::tie(solution, boost::tuples::ignore) = solver.optimize(initials);
   // THIS should fail because of the bad infeasible initial point!!
 //  CHECK(assert_equal(expectedSolution, solution, 1e-7));
 
-  VectorValues solution2 = solver.optimize();
+  VectorValues solution2;
+  boost::tie(solution2, boost::tuples::ignore) = solver.optimize();
   CHECK(assert_equal(expectedSolution, solution2, 1e-7));
 }
 
+/* ************************************************************************* */
+
+TEST(QPSolver, failedSubproblem) {
+  GaussianFactorGraph graph;
+  graph.push_back(JacobianFactor(X(1), eye(2), zero(2)));
+  graph.push_back(HessianFactor(X(1), zeros(2,2), zero(2), 100.0));
+  graph.push_back(JacobianFactor(X(1), (Matrix(1,2)<<-1.0, 0.0), -ones(1),
+                                 noiseModel::Constrained::MixedSigmas(-ones(1))));
+
+  VectorValues expected;
+  expected.insert(X(1), (Vector(2)<< 1.0, 0.0));
+
+  QPSolver solver(graph);
+  VectorValues solution;
+  boost::tie(solution, boost::tuples::ignore) = solver.optimize();
+  graph.print("Graph: ");
+  solution.print("Solution: ");
+  CHECK(assert_equal(expected, solution, 1e-7));
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;