diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 01ec7aa74..ef07ea301 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -81,17 +81,11 @@ public: //****************************************************************************** LPState iterate(const LPState& state) const { - static bool debug = false; - // Solve with the current working set // LP: project the objective neggradient to the constraint's null space // to find the direction to move VectorValues newValues = solveWithCurrentWorkingSet(state.values, state.workingSet); -// if (debug) state.workingSet.print("Working set:"); - if (debug) - (newValues - state.values).print("New direction:"); - // If we CAN'T move further // LP: projection on the constraints' nullspace is zero: we are at a vertex if (newValues.equals(state.values, 1e-7)) { @@ -99,21 +93,11 @@ public: // Compute lambda from the dual graph // LP: project the objective's gradient onto each constraint gradient to obtain the dual scaling factors // is it true?? - if (debug) - cout << "Building dual graph..." << endl; GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph( state.workingSet, newValues); - if (debug) - dualGraph->print("Dual graph: "); VectorValues duals = dualGraph->optimize(); - if (debug) - duals.print("Duals :"); - // LP: see which ineq constraint has wrong pulling direction, i.e., dual < 0 int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); - if (debug) - cout << "leavingFactor: " << leavingFactor << endl; - // If all inequality constraints are satisfied: We have the solution!! if (leavingFactor < 0) { // TODO If we still have infeasible equality constraints: the problem is over-constrained. No solution! @@ -139,19 +123,12 @@ public: VectorValues p = newValues - state.values; boost::tie(alpha, factorIx) = // using 16.41 computeStepSize(state.workingSet, state.values, p); - if (debug) - cout << "alpha, factorIx: " << alpha << " " << factorIx << " " << endl; - // also add to the working set the one that complains the most InequalityFactorGraph newWorkingSet = state.workingSet; if (factorIx >= 0) newWorkingSet.at(factorIx)->activate(); - // step! newValues = state.values + alpha * p; - if (debug) - newValues.print("New solution:"); - return LPState(newValues, state.duals, newWorkingSet, false, state.iterations + 1); } diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 6691f3deb..5b5abb018 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -101,32 +101,17 @@ return ActiveSetSolver::computeStepSize(workingSet, xk, p, 1); //****************************************************************************** QPState QPSolver::iterate(const QPState& state) const { -static bool debug = false; - // Algorithm 16.3 from Nocedal06book. // Solve with the current working set eqn 16.39, but instead of solving for p solve for x VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet); -if (debug) - newValues.print("New solution:"); - // If we CAN'T move further // if p_k = 0 is the original condition, modified by Duy to say that the state update is zero. if (newValues.equals(state.values, 1e-7)) { // Compute lambda from the dual graph - if (debug) - cout << "Building dual graph..." << endl; GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, newValues); - if (debug) - dualGraph->print("Dual graph: "); VectorValues duals = dualGraph->optimize(); - if (debug) - duals.print("Duals :"); - int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); - if (debug) - cout << "leavingFactor: " << leavingFactor << endl; - // If all inequality constraints are satisfied: We have the solution!! if (leavingFactor < 0) { return QPState(newValues, duals, state.workingSet, true, @@ -145,17 +130,12 @@ if (newValues.equals(state.values, 1e-7)) { VectorValues p = newValues - state.values; boost::tie(alpha, factorIx) = // using 16.41 computeStepSize(state.workingSet, state.values, p); - if (debug) - cout << "alpha, factorIx: " << alpha << " " << factorIx << " " << endl; - // also add to the working set the one that complains the most InequalityFactorGraph newWorkingSet = state.workingSet; if (factorIx >= 0) newWorkingSet.at(factorIx)->activate(); - // step! newValues = state.values + alpha * p; - return QPState(newValues, state.duals, newWorkingSet, false, state.iterations + 1); } diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 00ae8e6a7..0cede4a1d 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -32,6 +32,7 @@ namespace gtsam { * defined in the QP struct. * Note: This version of QPSolver only works with a feasible initial value. */ + //TODO: Remove Vector Values class QPSolver: public ActiveSetSolver { const QP& qp_; //!< factor graphs of the QP problem, can't be modified!