From c6d541741bc2cbed73c917f6a28aa4dd4d07bc3a Mon Sep 17 00:00:00 2001
From: thduynguyen <thduynguyen@gmail.com>
Date: Tue, 5 Aug 2014 21:55:50 -0400
Subject: [PATCH] more comments and debug info

---
 gtsam/linear/QPSolver.cpp | 17 +++++++++++++++--
 1 file changed, 15 insertions(+), 2 deletions(-)

diff --git a/gtsam/linear/QPSolver.cpp b/gtsam/linear/QPSolver.cpp
index 3ac56f929..d1f2640ae 100644
--- a/gtsam/linear/QPSolver.cpp
+++ b/gtsam/linear/QPSolver.cpp
@@ -287,8 +287,6 @@ boost::tuple<double, int, int> QPSolver::computeStepSize(const GaussianFactorGra
         if (debug) cout << "s, ajTp, b[s]: " << s << " " << ajTp << " " << b[s] << endl;
 
         // Check if  aj'*p >0. Don't care if it's not.
-        //        if (ajTp - b[s]>0)
-        //          throw std::runtime_error("Infeasible point detected. Please choose a feasible initial values!");
         if (ajTp<=0) continue;
 
         // Compute aj'*xk
@@ -335,6 +333,7 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c
 
     int factorIx, sigmaIx;
     boost::tie(factorIx, sigmaIx) = findWorstViolatedActiveIneq(lambdas);
+    if (debug) cout << "violated active ineq - factorIx, sigmaIx: " << factorIx << " " << sigmaIx << endl;
 
     // Try to disactivate the weakest violated ineq constraints
     // if not successful, i.e. all ineq constraints are satisfied: We have the solution!!
@@ -354,6 +353,14 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c
     updateWorkingSetInplace(workingGraph, factorIx, sigmaIx, 0.0);
     // step!
     currentSolution = currentSolution + alpha * p;
+//    if (alpha <1e-5) {
+//      if (debug) cout << "Building dual graph..." << endl;
+//      GaussianFactorGraph dualGraph = buildDualGraph(workingGraph, newSolution);
+//      if (debug) dualGraph.print("Dual graph: ");
+//      lambdas = dualGraph.optimize();
+//      if (debug) lambdas.print("lambdas :");
+//      return true; // TODO: HACK HACK!!!
+//    }
   }
 
   return false;
@@ -461,6 +468,7 @@ std::pair<GaussianFactorGraph::shared_ptr, VectorValues> QPSolver::constraintsLP
 
 /* ************************************************************************* */
 std::pair<bool, VectorValues> QPSolver::findFeasibleInitialValues() const {
+  static const bool debug = false;
   // Initial values with slack variables for the LP subproblem, Nocedal06book, pg.473
   VectorValues initials;
   size_t firstSlackKey;
@@ -478,6 +486,11 @@ std::pair<bool, VectorValues> QPSolver::findFeasibleInitialValues() const {
   LPSolver lpSolver(objectiveLP, constraints, slackLowerBounds);
   VectorValues solution = lpSolver.solve();
 
+  if (debug) initials.print("Initials LP: ");
+  if (debug) objectiveLP.print("Objective LP: ");
+  if (debug) constraints->print("Constraints LP: ");
+  if (debug) solution.print("LP solution: ");
+
   // Remove slack variables from solution
   double slackSum = 0.0;
   for (Key key = firstSlackKey; key < firstSlackKey+constraintIndices_.size(); ++key) {