From b1949966e94ad24f99458abc6a2cab53071a5953 Mon Sep 17 00:00:00 2001 From: Ivan Jimenez Date: Tue, 26 Jan 2016 09:34:05 -0500 Subject: [PATCH] [REFACTOR] Extract LPSolver.cpp from LPSolver.h --- gtsam_unstable/linear/ActiveSetSolver.h | 1 + gtsam_unstable/linear/LP.h | 3 + gtsam_unstable/linear/LPSolver.cpp | 203 ++++++++++++++++++++++++ gtsam_unstable/linear/LPSolver.h | 175 ++------------------ gtsam_unstable/linear/LPState.h | 3 + 5 files changed, 220 insertions(+), 165 deletions(-) create mode 100644 gtsam_unstable/linear/LPSolver.cpp diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h index 72a2f8c76..288488331 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.h +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -6,6 +6,7 @@ */ #pragma once +#include #include namespace gtsam { diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h index 71826d972..4e5866695 100644 --- a/gtsam_unstable/linear/LP.h +++ b/gtsam_unstable/linear/LP.h @@ -7,6 +7,9 @@ #pragma once +#include +#include + #include namespace gtsam { diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp new file mode 100644 index 000000000..4fd2d21a5 --- /dev/null +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -0,0 +1,203 @@ +/** + * @file LPSolver.cpp + * @brief + * @author Ivan Dario Jimenez + * @date 1/26/16 + */ + +#include +#include +#include + + +namespace gtsam { +LPSolver::LPSolver(const LP &lp) : + lp_(lp) { + // Push back factors that are the same in every iteration to the base graph. + // Those include the equality constraints and zero priors for keys that are not + // in the cost + baseGraph_.push_back(lp_.equalities); + + // Collect key-dim map of all variables in the constraints to create their zero priors later + keysDim_ = collectKeysDim(lp_.equalities); + KeyDimMap keysDim2 = collectKeysDim(lp_.inequalities); + keysDim_.insert(keysDim2.begin(), keysDim2.end()); + + // Create and push zero priors of constrained variables that do not exist in the cost function + baseGraph_.push_back(*createZeroPriors(lp_.cost.keys(), keysDim_)); + + // Variable index + equalityVariableIndex_ = VariableIndex(lp_.equalities); + inequalityVariableIndex_ = VariableIndex(lp_.inequalities); + constrainedKeys_ = lp_.equalities.keys(); + constrainedKeys_.merge(lp_.inequalities.keys()); +} + +GaussianFactorGraph::shared_ptr LPSolver::createZeroPriors( + const KeyVector &costKeys, const KeyDimMap &keysDim) const { + GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph()); + BOOST_FOREACH(Key key, keysDim | boost::adaptors::map_keys) { + if (find(costKeys.begin(), costKeys.end(), key) == costKeys.end()) { + size_t dim = keysDim.at(key); + graph->push_back(JacobianFactor(key, eye(dim), zero(dim))); + } + } + return graph; +} + +LPState LPSolver::iterate(const LPState &state) const { + // Solve with the current working set + // LP: project the objective neggradient to the constraint's null space + // to find the direction to move + VectorValues newValues = solveWithCurrentWorkingSet(state.values, + state.workingSet); + // If we CAN'T move further + // LP: projection on the constraints' nullspace is zero: we are at a vertex + if (newValues.equals(state.values, 1e-7)) { + // Find and remove the bad ineq constraint by computing its lambda + // Compute lambda from the dual graph + // LP: project the objective's gradient onto each constraint gradient to obtain the dual scaling factors + // is it true?? + GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, + newValues); + VectorValues duals = dualGraph->optimize(); + // LP: see which ineq constraint has wrong pulling direction, i.e., dual < 0 + int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); + // If all inequality constraints are satisfied: We have the solution!! + if (leavingFactor < 0) { + // TODO If we still have infeasible equality constraints: the problem is over-constrained. No solution! + // ... + return LPState(newValues, duals, state.workingSet, true, + state.iterations + 1); + } else { + // Inactivate the leaving constraint + // LP: remove the bad ineq constraint out of the working set + InequalityFactorGraph newWorkingSet = state.workingSet; + newWorkingSet.at(leavingFactor)->inactivate(); + return LPState(newValues, duals, newWorkingSet, false, + state.iterations + 1); + } + } else { + // If we CAN make some progress, i.e. p_k != 0 + // Adapt stepsize if some inactive constraints complain about this move + // LP: projection on nullspace is NOT zero: + // find and put a blocking inactive constraint to the working set, + // otherwise the problem is unbounded!!! + double alpha; + int factorIx; + VectorValues p = newValues - state.values; + boost::tie(alpha, factorIx) = // using 16.41 + computeStepSize(state.workingSet, state.values, p); + // also add to the working set the one that complains the most + InequalityFactorGraph newWorkingSet = state.workingSet; + if (factorIx >= 0) + newWorkingSet.at(factorIx)->activate(); + // step! + newValues = state.values + alpha * p; + return LPState(newValues, state.duals, newWorkingSet, false, + state.iterations + 1); + } +} + +GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors( + const LinearCost &cost, const VectorValues &xk) const { + GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph()); + KeyVector keys = cost.keys(); + + for (LinearCost::const_iterator it = cost.begin(); it != cost.end(); ++it) { + size_t dim = cost.getDim(it); + Vector b = xk.at(*it) - cost.getA(it).transpose(); // b = xk-g + graph->push_back(JacobianFactor(*it, eye(dim), b)); + } + + return graph; +} + +VectorValues LPSolver::solveWithCurrentWorkingSet( + const VectorValues &xk, + const InequalityFactorGraph &workingSet) const { + GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2 + workingGraph.push_back(*createLeastSquareFactors(lp_.cost, xk)); + + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) { + if (factor->active()) workingGraph.push_back(factor); + } + return workingGraph.optimize(); +} + +boost::shared_ptr LPSolver::createDualFactor( + Key key, + const InequalityFactorGraph &workingSet, + const VectorValues &delta) const { + + // Transpose the A matrix of constrained factors to have the jacobian of the dual key + TermsContainer Aterms = collectDualJacobians < LinearEquality + > (key, lp_.equalities, equalityVariableIndex_); + TermsContainer AtermsInequalities = collectDualJacobians < LinearInequality + > (key, workingSet, inequalityVariableIndex_); + Aterms.insert(Aterms.end(), AtermsInequalities.begin(), + AtermsInequalities.end()); + + // Collect the gradients of unconstrained cost factors to the b vector + if (Aterms.size() > 0) { + Vector b = zero(delta.at(key).size()); + Factor::const_iterator it = lp_.cost.find(key); + if (it != lp_.cost.end()) + b = lp_.cost.getA(it).transpose(); + return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables + } else { + return boost::make_shared(); + } +} + +InequalityFactorGraph LPSolver::identifyActiveConstraints( + const InequalityFactorGraph &inequalities, + const VectorValues &initialValues, + const VectorValues &duals) const { + InequalityFactorGraph workingSet; + BOOST_FOREACH(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 { + { + + // Initialize workingSet from the feasible initialValues + InequalityFactorGraph workingSet = identifyActiveConstraints( + lp_.inequalities, initialValues, duals); + LPState state(initialValues, duals, workingSet, false, 0); + + /// main loop of the solver + while (!state.converged) { + state = iterate(state); + } + + return make_pair(state.values, state.duals); + } +} + +boost::tuples::tuple LPSolver::computeStepSize( + const InequalityFactorGraph &workingSet, + const VectorValues &xk, + const VectorValues &p) const { + return ActiveSetSolver::computeStepSize(workingSet, xk, p, + std::numeric_limits::infinity()); +} +} diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index ef07ea301..2d6a2433a 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -14,6 +14,7 @@ #include namespace gtsam { + typedef std::map KeyDimMap; class LPSolver: public ActiveSetSolver { @@ -22,27 +23,7 @@ class LPSolver: public ActiveSetSolver { public: /// Constructor - LPSolver(const LP& lp) : - lp_(lp) { - // Push back factors that are the same in every iteration to the base graph. - // Those include the equality constraints and zero priors for keys that are not - // in the cost - baseGraph_.push_back(lp_.equalities); - - // Collect key-dim map of all variables in the constraints to create their zero priors later - keysDim_ = collectKeysDim(lp_.equalities); - KeyDimMap keysDim2 = collectKeysDim(lp_.inequalities); - keysDim_.insert(keysDim2.begin(), keysDim2.end()); - - // Create and push zero priors of constrained variables that do not exist in the cost function - baseGraph_.push_back(*createZeroPriors(lp_.cost.keys(), keysDim_)); - - // Variable index - equalityVariableIndex_ = VariableIndex(lp_.equalities); - inequalityVariableIndex_ = VariableIndex(lp_.inequalities); - constrainedKeys_ = lp_.equalities.keys(); - constrainedKeys_.merge(lp_.inequalities.keys()); - } + LPSolver(const LP& lp); const LP& lp() const { return lp_; @@ -68,71 +49,10 @@ public: * Create a zero prior for any keys in the graph that don't exist in the cost */ GaussianFactorGraph::shared_ptr createZeroPriors(const KeyVector& costKeys, - const KeyDimMap& keysDim) const { - GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph()); - BOOST_FOREACH(Key key, keysDim | boost::adaptors::map_keys) { - if (find(costKeys.begin(), costKeys.end(), key) == costKeys.end()) { - size_t dim = keysDim.at(key); - graph->push_back(JacobianFactor(key, eye(dim), zero(dim))); - } - } - return graph; - } + const KeyDimMap& keysDim) const; //****************************************************************************** - LPState iterate(const LPState& state) const { - // Solve with the current working set - // LP: project the objective neggradient to the constraint's null space - // to find the direction to move - VectorValues newValues = solveWithCurrentWorkingSet(state.values, - state.workingSet); - // If we CAN'T move further - // LP: projection on the constraints' nullspace is zero: we are at a vertex - if (newValues.equals(state.values, 1e-7)) { - // Find and remove the bad ineq constraint by computing its lambda - // Compute lambda from the dual graph - // LP: project the objective's gradient onto each constraint gradient to obtain the dual scaling factors - // is it true?? - GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph( - state.workingSet, newValues); - VectorValues duals = dualGraph->optimize(); - // LP: see which ineq constraint has wrong pulling direction, i.e., dual < 0 - int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); - // If all inequality constraints are satisfied: We have the solution!! - if (leavingFactor < 0) { - // TODO If we still have infeasible equality constraints: the problem is over-constrained. No solution! - // ... - return LPState(newValues, duals, state.workingSet, true, - state.iterations + 1); - } else { - // Inactivate the leaving constraint - // LP: remove the bad ineq constraint out of the working set - InequalityFactorGraph newWorkingSet = state.workingSet; - newWorkingSet.at(leavingFactor)->inactivate(); - return LPState(newValues, duals, newWorkingSet, false, - state.iterations + 1); - } - } else { - // If we CAN make some progress, i.e. p_k != 0 - // Adapt stepsize if some inactive constraints complain about this move - // LP: projection on nullspace is NOT zero: - // find and put a blocking inactive constraint to the working set, - // otherwise the problem is unbounded!!! - double alpha; - int factorIx; - VectorValues p = newValues - state.values; - boost::tie(alpha, factorIx) = // using 16.41 - computeStepSize(state.workingSet, state.values, p); - // also add to the working set the one that complains the most - InequalityFactorGraph newWorkingSet = state.workingSet; - if (factorIx >= 0) - newWorkingSet.at(factorIx)->activate(); - // step! - newValues = state.values + alpha * p; - return LPState(newValues, state.duals, newWorkingSet, false, - state.iterations + 1); - } - } + LPState iterate(const LPState& state) const; //****************************************************************************** /** @@ -148,107 +68,32 @@ public: * is the projection of the gradient onto the constraints' subspace */ GaussianFactorGraph::shared_ptr createLeastSquareFactors( - const LinearCost& cost, const VectorValues& xk) const { - GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph()); - KeyVector keys = cost.keys(); - - for (LinearCost::const_iterator it = cost.begin(); it != cost.end(); ++it) { - size_t dim = cost.getDim(it); - Vector b = xk.at(*it) - cost.getA(it).transpose(); // b = xk-g - graph->push_back(JacobianFactor(*it, eye(dim), b)); - } - - return graph; - } + const LinearCost& cost, const VectorValues& xk) const; /// Find solution with the current working set VectorValues solveWithCurrentWorkingSet(const VectorValues& xk, - const InequalityFactorGraph& workingSet) const { - GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2 - workingGraph.push_back(*createLeastSquareFactors(lp_.cost, xk)); - - BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) { - if (factor->active()) workingGraph.push_back(factor); - } - return workingGraph.optimize(); - } + const InequalityFactorGraph& workingSet) const; //****************************************************************************** JacobianFactor::shared_ptr createDualFactor(Key key, - const InequalityFactorGraph& workingSet, - const VectorValues& delta) const { - - // Transpose the A matrix of constrained factors to have the jacobian of the dual key - TermsContainer Aterms = collectDualJacobians < LinearEquality - > (key, lp_.equalities, equalityVariableIndex_); - TermsContainer AtermsInequalities = collectDualJacobians < LinearInequality - > (key, workingSet, inequalityVariableIndex_); - Aterms.insert(Aterms.end(), AtermsInequalities.begin(), - AtermsInequalities.end()); - - // Collect the gradients of unconstrained cost factors to the b vector - if (Aterms.size() > 0) { - Vector b = zero(delta.at(key).size()); - Factor::const_iterator it = lp_.cost.find(key); - if (it != lp_.cost.end()) - b = lp_.cost.getA(it).transpose(); - return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables - } else { - return boost::make_shared(); - } - } + const InequalityFactorGraph& workingSet, const VectorValues& delta) const; //****************************************************************************** boost::tuple computeStepSize( const InequalityFactorGraph& workingSet, const VectorValues& xk, - const VectorValues& p) const { - return ActiveSetSolver::computeStepSize(workingSet, xk, p, - std::numeric_limits::infinity()); - } + const VectorValues& p) const; //****************************************************************************** InequalityFactorGraph identifyActiveConstraints( const InequalityFactorGraph& inequalities, - const VectorValues& initialValues, const VectorValues& duals) const { - InequalityFactorGraph workingSet; - BOOST_FOREACH(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; - } + 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 */ pair optimize(const VectorValues& initialValues, - const VectorValues& duals = VectorValues()) const { - - // Initialize workingSet from the feasible initialValues - InequalityFactorGraph workingSet = identifyActiveConstraints( - lp_.inequalities, initialValues, duals); - LPState state(initialValues, duals, workingSet, false, 0); - - /// main loop of the solver - while (!state.converged) { - state = iterate(state); - } - - return make_pair(state.values, state.duals); - } + const VectorValues& duals = VectorValues()) const; //****************************************************************************** /** diff --git a/gtsam_unstable/linear/LPState.h b/gtsam_unstable/linear/LPState.h index b5d7df091..8b52170e4 100644 --- a/gtsam_unstable/linear/LPState.h +++ b/gtsam_unstable/linear/LPState.h @@ -5,6 +5,9 @@ * @date 1/24/16 */ +#include +#include "InequalityFactorGraph.h" + namespace gtsam { struct LPState {