12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compute the least-square values of dual variables instead of forcing them to satisfy the stationarity condition exactly. This fixes an infinite-loop bug in QPSolver when a constraint was continuously added to and removed from the active set.

release/4.3a0
krunalchande 2014-12-23 16:09:18 -05:00 committed by thduynguyen
parent 276959e39a
commit bcdeddbda1
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam_unstable/linear/QPSolver.cpp
View File

@ -58,7 +58,7 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
b += factor->gradient(key, delta); b += factor->gradient(key, delta);
} }
} }
return boost::make_shared<JacobianFactor>(Aterms, b, noiseModel::Constrained::All(b.rows())); return boost::make_shared<JacobianFactor>(Aterms, b); // compute the least-square approximation of dual variables
} }
else { else {
return boost::make_shared<JacobianFactor>(); return boost::make_shared<JacobianFactor>();
@ -156,7 +156,7 @@ boost::tuple<double, int> QPSolver::computeStepSize(
//****************************************************************************** //******************************************************************************
QPState QPSolver::iterate(const QPState& state) const { QPState QPSolver::iterate(const QPState& state) const {
static bool debug = false; static bool debug = true;
// Solve with the current working set // Solve with the current working set
VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet); VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet);
@ -164,7 +164,7 @@ QPState QPSolver::iterate(const QPState& state) const {
newValues.print("New solution:"); newValues.print("New solution:");
// If we CAN'T move further // If we CAN'T move further
if (newValues.equals(state.values, 1e-5)) { if (newValues.equals(state.values, 1e-7)) {
// Compute lambda from the dual graph // Compute lambda from the dual graph
if (debug) if (debug)
cout << "Building dual graph..." << endl; cout << "Building dual graph..." << endl;