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

simplify useWarmStart logic

release/4.3a0
Duy-Nguyen Ta 2016-06-15 17:32:29 -04:00
parent 3c85e2d625
commit 7492a708d2
1 changed files with 9 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam_unstable/linear/QPSolver.cpp
View File

@ -139,26 +139,18 @@ InequalityFactorGraph QPSolver::identifyActiveConstraints(
InequalityFactorGraph workingSet; InequalityFactorGraph workingSet;
for (const LinearInequality::shared_ptr& factor : inequalities) { for (const LinearInequality::shared_ptr& factor : inequalities) {
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
if (useWarmStart == true && duals.exists(workingFactor->dualKey())) { if (useWarmStart && duals.size() > 0) {
workingFactor->activate(); if (duals.exists(workingFactor->dualKey())) workingFactor->activate();
} else { else workingFactor->inactivate();
if (useWarmStart == true && duals.size() > 0) {
workingFactor->inactivate();
} else { } else {
double error = workingFactor->error(initialValues); double error = workingFactor->error(initialValues);
// TODO: find a feasible initial point for QPSolver. // Safety guard. This should not happen unless users provide a bad init
// For now, we just throw an exception, since we don't have an LPSolver if (error > 0) throw InfeasibleInitialValues();
// to do this yet if (fabs(error) < 1e-7)
if (error > 0)
throw InfeasibleInitialValues();
if (fabs(error) < 1e-7) {
workingFactor->activate(); workingFactor->activate();
} else { else
workingFactor->inactivate(); workingFactor->inactivate();
} }
}
}
workingSet.push_back(workingFactor); workingSet.push_back(workingFactor);
} }
return workingSet; return workingSet;