From e2a28593ab8147c29f89ba237602d734e65313c0 Mon Sep 17 00:00:00 2001 From: = Date: Thu, 16 Jun 2016 09:39:13 -0400 Subject: [PATCH] [REFACTOR] Move identifyActiveConstraints from QP and LP to the ActiveSetSolver. --- gtsam_unstable/linear/ActiveSetSolver.cpp | 30 +++++++++++++++++++++++ gtsam_unstable/linear/ActiveSetSolver.h | 12 ++++++++- gtsam_unstable/linear/LPSolver.cpp | 23 ----------------- gtsam_unstable/linear/LPSolver.h | 14 +++-------- gtsam_unstable/linear/QPSolver.cpp | 25 ------------------- gtsam_unstable/linear/QPSolver.h | 9 +++---- 6 files changed, 48 insertions(+), 65 deletions(-) diff --git a/gtsam_unstable/linear/ActiveSetSolver.cpp b/gtsam_unstable/linear/ActiveSetSolver.cpp index 6736de31a..29d5f96a8 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.cpp +++ b/gtsam_unstable/linear/ActiveSetSolver.cpp @@ -7,6 +7,7 @@ */ #include +#include "InfeasibleInitialValues.h" namespace gtsam { @@ -129,4 +130,33 @@ boost::tuple ActiveSetSolver::computeStepSize( return boost::make_tuple(minAlpha, closestFactorIx); } +//****************************************************************************** +InequalityFactorGraph ActiveSetSolver::identifyActiveConstraints( + const InequalityFactorGraph& inequalities, + 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)); + if (useWarmStart && duals.size() > 0) { + if (duals.exists(workingFactor->dualKey())) + workingFactor->activate(); + else + workingFactor->inactivate(); + } else { + 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); + } + return workingSet; +} + + } diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h index 4519ad5a8..6594ac3b6 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.h +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -64,12 +64,22 @@ public: */ int identifyLeavingConstraint(const InequalityFactorGraph& workingSet, const VectorValues& lambdas) const; - + /** * Builds a dual graph from the current working set. */ GaussianFactorGraph::shared_ptr buildDualGraph( const InequalityFactorGraph& workingSet, const VectorValues& delta) const; + /* + * Given an initial value this function determine which constraints are active + * which can be used to initialize the working set. + * A constraint Ax <= b is active if we have an x' s.t. Ax' = b + */ + /// Identify active constraints based on initial values. + InequalityFactorGraph identifyActiveConstraints( + const InequalityFactorGraph& inequalities, + const VectorValues& initialValues, const VectorValues& duals = + VectorValues(), bool useWarmStart = true) const; protected: /** diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index 042b62ea1..30e6c3ce6 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -145,29 +145,6 @@ boost::shared_ptr LPSolver::createDualFactor( } } -//****************************************************************************** -InequalityFactorGraph LPSolver::identifyActiveConstraints( - const InequalityFactorGraph &inequalities, - const VectorValues &initialValues, const VectorValues &duals) 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(); - } else { - workingFactor->inactivate(); - } - workingSet.push_back(workingFactor); - } - return workingSet; -} - //****************************************************************************** std::pair LPSolver::optimize( const VectorValues &initialValues, const VectorValues &duals) const { diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index bf081c05e..b6e463164 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -34,7 +34,7 @@ public: * LP problems. At the end of this iteration the problem should either be found * to be unfeasible, solved or the current state changed to reflect a new * working set. - */ + */ //SAME LPState iterate(const LPState &state) const; /** @@ -53,6 +53,7 @@ public: const LinearCost &cost, const VectorValues &xk) const; /// Find solution with the current working set + //SAME VectorValues solveWithCurrentWorkingSet(const VectorValues &xk, const InequalityFactorGraph &workingSet) const; @@ -62,23 +63,16 @@ public: * for the following problem: f' = - lambda * g' where f is the objection * function g are dual factors and lambda is the lagrangian multiplier. */ + //SAME JacobianFactor::shared_ptr createDualFactor(Key key, const InequalityFactorGraph &workingSet, const VectorValues &delta) const; /// TODO(comment) + //SAME boost::tuple computeStepSize( const InequalityFactorGraph &workingSet, const VectorValues &xk, const VectorValues &p) const; - /* - * Given an initial value this function determine which constraints are active - * which can be used to initialize the working set. - * A constraint Ax <= b is active if we have an x' s.t. Ax' = b - */ - InequalityFactorGraph identifyActiveConstraints( - const InequalityFactorGraph &inequalities, - const VectorValues &initialValues, const VectorValues &duals) const; - /** Optimize with the provided feasible initial values * TODO: throw exception if the initial values is not feasible wrt inequality constraints * TODO: comment duals diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index a8ef028e8..f80bdc14a 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -124,31 +124,6 @@ QPState QPSolver::iterate(const QPState& state) const { } } -//****************************************************************************** -InequalityFactorGraph QPSolver::identifyActiveConstraints( - const InequalityFactorGraph& inequalities, - 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)); - if (useWarmStart && duals.size() > 0) { - if (duals.exists(workingFactor->dualKey())) workingFactor->activate(); - else workingFactor->inactivate(); - } else { - 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); - } - return workingSet; -} - //****************************************************************************** pair QPSolver::optimize( const VectorValues& initialValues, const VectorValues& duals, diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index bd1bb2a17..64ced2630 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -44,10 +44,12 @@ public: QPSolver(const QP& qp); /// Find solution with the current working set + //SAME VectorValues solveWithCurrentWorkingSet( const InequalityFactorGraph& workingSet) const; /// Create a dual factor + //SAME JacobianFactor::shared_ptr createDualFactor(Key key, const InequalityFactorGraph& workingSet, const VectorValues& delta) const; @@ -66,6 +68,7 @@ public: * * We want the minimum of all those alphas among all inactive inequality. */ + //SAME boost::tuple computeStepSize( const InequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const; @@ -73,12 +76,6 @@ public: /// Iterate 1 step, return a new state with a new workingSet and values QPState iterate(const QPState& state) const; - /// Identify active constraints based on initial values. - InequalityFactorGraph identifyActiveConstraints( - const InequalityFactorGraph& inequalities, - const VectorValues& initialValues, const VectorValues& duals = - VectorValues(), bool useWarmStart = true) const; - /** * Optimize with provided initial values * For this version, it is the responsibility of the caller to provide