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) :
lp_(lp) {
// Variable index
equalityVariableIndex_ = VariableIndex(lp_.equalities);
inequalityVariableIndex_ = VariableIndex(lp_.inequalities);
constrainedKeys_ = lp_.equalities.keys();
@ -149,20 +148,22 @@ boost::shared_ptr<JacobianFactor> LPSolver::createDualFactor(
//******************************************************************************
InequalityFactorGraph LPSolver::identifyActiveConstraints(
const InequalityFactorGraph &inequalities,
const VectorValues &initialValues, const VectorValues &duals) const {
const VectorValues &initialValues, const VectorValues &duals,
bool useWarmStart) const {
InequalityFactorGraph workingSet;
for (const LinearInequality::shared_ptr &factor : inequalities) {
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
double error = workingFactor->error(initialValues);
// TODO: find a feasible initial point for LPSolver.
// For now, we just throw an exception
if (error > 0)
throw InfeasibleInitialValues();
if (fabs(error) < 1e-7) {
workingFactor->activate();
if (useWarmStart && duals.size() > 0) {
if (duals.exists(workingFactor->dualKey())) workingFactor->activate();
else workingFactor->inactivate();
} 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);
}
@ -171,11 +172,12 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints(
//******************************************************************************
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
InequalityFactorGraph workingSet = identifyActiveConstraints(
lp_.inequalities, initialValues, duals);
lp_.inequalities, initialValues, duals, useWarmStart);
LPState state(initialValues, duals, workingSet, false, 0);
/// main loop of the solver

5
gtsam_unstable/linear/LPSolver.h
View File

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