From 0576aac69bbb46c8e11f321b54aa04dd76cfa4ad Mon Sep 17 00:00:00 2001
From: thduynguyen <thduynguyen@gmail.com>
Date: Fri, 12 Dec 2014 22:08:09 -0500
Subject: [PATCH] remove support for special EliminatePreferCholesky to deal
 with Indeterminant exception arising from multiplied Hessian terms of
 nonlinear equality constraints.

---
 gtsam/linear/GaussianFactorGraph.cpp |  23 ------
 gtsam/linear/HessianFactor.cpp       | 106 +--------------------------
 2 files changed, 3 insertions(+), 126 deletions(-)

diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp
index 674cc7bf8..2879e0a4c 100644
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/gtsam/linear/GaussianFactorGraph.cpp
@@ -393,29 +393,6 @@ 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,
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index 167557162..6d7f3c2e1 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -682,110 +682,10 @@ 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.
-
-  /* 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.
-   */
-  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 {
+  if (hasConstraints(factors))
+    return EliminateQR(factors, keys);
+  else
     return EliminateCholesky(factors, keys);
-  }
 }
 
 } // gtsam