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

LP now also has warm start

release/4.3a0
Duy-Nguyen Ta 2016-06-16 18:07:01 -04:00
parent f0a3e7a799
commit 85b8fb5626
2 changed files with 18 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam_unstable/linear/LPSolver.cpp
View File

@ -15,7 +15,6 @@ namespace gtsam {
//****************************************************************************** //******************************************************************************
LPSolver::LPSolver(const LP &lp) : LPSolver::LPSolver(const LP &lp) :
lp_(lp) { lp_(lp) {
// Variable index
equalityVariableIndex_ = VariableIndex(lp_.equalities); equalityVariableIndex_ = VariableIndex(lp_.equalities);
inequalityVariableIndex_ = VariableIndex(lp_.inequalities); inequalityVariableIndex_ = VariableIndex(lp_.inequalities);
constrainedKeys_ = lp_.equalities.keys(); constrainedKeys_ = lp_.equalities.keys();
@ -149,20 +148,22 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(
//****************************************************************************** //******************************************************************************
InequalityFactorGraph LPSolver::identifyActiveConstraints( InequalityFactorGraph LPSolver::identifyActiveConstraints(
const InequalityFactorGraph &inequalities, const InequalityFactorGraph &inequalities,
const VectorValues &initialValues, const VectorValues &duals) const { const VectorValues &initialValues, const VectorValues &duals,
bool useWarmStart) const {
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));
double error = workingFactor->error(initialValues); if (useWarmStart && duals.size() > 0) {
// TODO: find a feasible initial point for LPSolver. if (duals.exists(workingFactor->dualKey())) workingFactor->activate();
// For now, we just throw an exception else workingFactor->inactivate();
if (error > 0)
throw InfeasibleInitialValues();
if (fabs(error) < 1e-7) {
workingFactor->activate();
} else { } else {
workingFactor->inactivate(); double error = workingFactor->error(initialValues);
// Safety guard. This should not happen unless users provide a bad init
if (error > 0) throw InfeasibleInitialValues();
if (fabs(error) < 1e-7)
workingFactor->activate();
else
workingFactor->inactivate();
} }
workingSet.push_back(workingFactor); workingSet.push_back(workingFactor);
} }
@ -171,11 +172,12 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints(
//****************************************************************************** //******************************************************************************
std::pair<VectorValues, VectorValues> LPSolver::optimize( std::pair<VectorValues, VectorValues> LPSolver::optimize(
const VectorValues &initialValues, const VectorValues &duals) const { const VectorValues &initialValues, const VectorValues &duals,
bool useWarmStart) const {
{ {
// Initialize workingSet from the feasible initialValues // Initialize workingSet from the feasible initialValues
InequalityFactorGraph workingSet = identifyActiveConstraints( InequalityFactorGraph workingSet = identifyActiveConstraints(
lp_.inequalities, initialValues, duals); lp_.inequalities, initialValues, duals, useWarmStart);
LPState state(initialValues, duals, workingSet, false, 0); LPState state(initialValues, duals, workingSet, false, 0);
/// main loop of the solver /// main loop of the solver

5
gtsam_unstable/linear/LPSolver.h
View File

@ -75,14 +75,15 @@ public:
*/ */
InequalityFactorGraph identifyActiveConstraints( InequalityFactorGraph identifyActiveConstraints(
const InequalityFactorGraph &inequalities, const InequalityFactorGraph &inequalities,
const VectorValues &initialValues, const VectorValues &duals) const; const VectorValues &initialValues, const VectorValues &duals,
bool useWarmStart = false) const;
/** Optimize with the provided feasible initial values /** Optimize with the provided feasible initial values
* TODO: throw exception if the initial values is not feasible wrt inequality constraints * TODO: throw exception if the initial values is not feasible wrt inequality constraints
* TODO: comment duals * TODO: comment duals
*/ */
pair<VectorValues, VectorValues> optimize(const VectorValues &initialValues, pair<VectorValues, VectorValues> optimize(const VectorValues &initialValues,
const VectorValues &duals = VectorValues()) const; const VectorValues &duals = VectorValues(), bool useWarmStart = false) const;
/** /**
* Optimize without initial values. * Optimize without initial values.