more comments and debug info
parent
47ed9f3687
commit
c6d541741b
|
@ -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;
|
if (debug) cout << "s, ajTp, b[s]: " << s << " " << ajTp << " " << b[s] << endl;
|
||||||
|
|
||||||
// Check if aj'*p >0. Don't care if it's not.
|
// 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;
|
if (ajTp<=0) continue;
|
||||||
|
|
||||||
// Compute aj'*xk
|
// Compute aj'*xk
|
||||||
|
@ -335,6 +333,7 @@ bool QPSolver::iterateInPlace(GaussianFactorGraph& workingGraph, VectorValues& c
|
||||||
|
|
||||||
int factorIx, sigmaIx;
|
int factorIx, sigmaIx;
|
||||||
boost::tie(factorIx, sigmaIx) = findWorstViolatedActiveIneq(lambdas);
|
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
|
// Try to disactivate the weakest violated ineq constraints
|
||||||
// if not successful, i.e. all ineq constraints are satisfied: We have the solution!!
|
// 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);
|
updateWorkingSetInplace(workingGraph, factorIx, sigmaIx, 0.0);
|
||||||
// step!
|
// step!
|
||||||
currentSolution = currentSolution + alpha * p;
|
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;
|
return false;
|
||||||
|
@ -461,6 +468,7 @@ std::pair<GaussianFactorGraph::shared_ptr, VectorValues> QPSolver::constraintsLP
|
||||||
|
|
||||||
/* ************************************************************************* */
|
/* ************************************************************************* */
|
||||||
std::pair<bool, VectorValues> QPSolver::findFeasibleInitialValues() const {
|
std::pair<bool, VectorValues> QPSolver::findFeasibleInitialValues() const {
|
||||||
|
static const bool debug = false;
|
||||||
// Initial values with slack variables for the LP subproblem, Nocedal06book, pg.473
|
// Initial values with slack variables for the LP subproblem, Nocedal06book, pg.473
|
||||||
VectorValues initials;
|
VectorValues initials;
|
||||||
size_t firstSlackKey;
|
size_t firstSlackKey;
|
||||||
|
@ -478,6 +486,11 @@ std::pair<bool, VectorValues> QPSolver::findFeasibleInitialValues() const {
|
||||||
LPSolver lpSolver(objectiveLP, constraints, slackLowerBounds);
|
LPSolver lpSolver(objectiveLP, constraints, slackLowerBounds);
|
||||||
VectorValues solution = lpSolver.solve();
|
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
|
// Remove slack variables from solution
|
||||||
double slackSum = 0.0;
|
double slackSum = 0.0;
|
||||||
for (Key key = firstSlackKey; key < firstSlackKey+constraintIndices_.size(); ++key) {
|
for (Key key = firstSlackKey; key < firstSlackKey+constraintIndices_.size(); ++key) {
|
||||||
|
|
Loading…
Reference in New Issue