Reapply hacks in EliminatePreferCholesky to deal with negative definite hessians obtained from multiplying dual variables with the hessians of nonlinear constraints needed for SQP.

gtsam/linear/GaussianFactorGraph.cpp

@@ -384,6 +384,29 @@ namespace gtsam {
     return false;
   }
 
+  /* ************************************************************************* */
+  boost::tuple<GaussianFactorGraph, GaussianFactorGraph, GaussianFactorGraph> GaussianFactorGraph::splitConstraints() const {
+    typedef HessianFactor H;
+    typedef JacobianFactor J;
+
+    GaussianFactorGraph hessians, jacobians, constraints;
+    BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, *this) {
+      H::shared_ptr hessian(boost::dynamic_pointer_cast<H>(factor));
+      if (hessian)
+        hessians.push_back(factor);
+      else {
+        J::shared_ptr jacobian(boost::dynamic_pointer_cast<J>(factor));
+        if (jacobian && jacobian->get_model() && jacobian->get_model()->isConstrained()) {
+          constraints.push_back(jacobian);
+        }
+        else {
+          jacobians.push_back(factor);
+        }
+      }
+    }
+    return boost::make_tuple(hessians, jacobians, constraints);
+  }
+  
   /* ************************************************************************* */
   // x += alpha*A'*e
   void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e,

gtsam/linear/GaussianFactorGraph.h

@@ -316,6 +316,12 @@ namespace gtsam {
     /** In-place version e <- A*x that takes an iterator. */
     void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
 
+    /**
+     * Split constraints and unconstrained factors into two different graphs
+     * @return a pair of <unconstrained, constrained> graphs
+     */
+    boost::tuple<GaussianFactorGraph, GaussianFactorGraph, GaussianFactorGraph> splitConstraints() const;
+
     /// @}
 
   private:

gtsam/linear/HessianFactor.cpp

@@ -616,10 +616,110 @@ 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))
+
-    return EliminateQR(factors, keys);
+  /* Currently, when eliminating a constrained variable, EliminatePreferCholesky
-  else
+   * 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.
+   */
+  GaussianFactorGraph hessians, jacobians, constraints;
+//  factors.print("factors: ");
+  boost::tie(hessians, jacobians, constraints) = factors.splitConstraints();
+//  keys.print("Frontal variables to eliminate: ");
+//  hessians.print("Hessians: ");
+//  jacobians.print("Jacobians: ");
+//  constraints.print("Constraints: ");
+
+  bool hasHessians = hessians.size() > 0;
+
+  // Add all jacobians to gather as much info as we can
+  hessians.push_back(jacobians);
+
+  if (constraints.size()>0) {
+//    // Build joint factor
+//    HessianFactor::shared_ptr jointFactor;
+//    try {
+//      jointFactor = boost::make_shared<HessianFactor>(hessians, 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);
+
+    // If there are hessian factors, turn them into conditional
+    // by doing partial elimination, then use those jacobians when eliminating the constraints
+    GaussianFactor::shared_ptr unconstrainedNewFactor;
+    if (hessians.size() > 0) {
+      bool hasSeparator = false;
+      GaussianFactorGraph::Keys unconstrainedKeys = hessians.keys();
+      BOOST_FOREACH(Key key, unconstrainedKeys) {
+        if (find(keys.begin(), keys.end(), key) == keys.end()) {
+          hasSeparator = true;
+          break;
+        }
+      }
+
+      if (hasSeparator) {
+        // find frontal keys in the unconstrained factor to eliminate
+        Ordering subkeys;
+        BOOST_FOREACH(Key key, keys) {
+          if (unconstrainedKeys.exists(key))
+            subkeys.push_back(key);
+        }
+        GaussianConditional::shared_ptr unconstrainedConditional;
+        boost::tie(unconstrainedConditional, unconstrainedNewFactor)
+            = EliminateCholesky(hessians, subkeys);
+        constraints.push_back(unconstrainedConditional);
+      }
+      else {
+        if (hasHessians) {
+          HessianFactor::shared_ptr jointFactor = boost::make_shared<
+              HessianFactor>(hessians, Scatter(factors, keys));
+          constraints.push_back(jointFactor);
+        }
+        else {
+          constraints.push_back(hessians);
+        }
+      }
+    }
+
+    // Now eliminate the constraints
+    GaussianConditional::shared_ptr constrainedConditional;
+    GaussianFactor::shared_ptr constrainedNewFactor;
+    boost::tie(constrainedConditional, constrainedNewFactor) = EliminateQR(
+        constraints, keys);
+//    constraints.print("constraints: ");
+//    constrainedConditional->print("constrainedConditional: ");
+//    constrainedNewFactor->print("constrainedNewFactor: ");
+
+    if (unconstrainedNewFactor) {
+      GaussianFactorGraph newFactors;
+      newFactors.push_back(unconstrainedNewFactor);
+      newFactors.push_back(constrainedNewFactor);
+//      newFactors.print("newFactors: ");
+      HessianFactor::shared_ptr newFactor(new HessianFactor(newFactors));
+      return make_pair(constrainedConditional, newFactor);
+    } else {
+      return make_pair(constrainedConditional, constrainedNewFactor);
+    }
+  }
+  else {
     return EliminateCholesky(factors, keys);
+  }
 }
 
 } // gtsam