diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h new file mode 100644 index 000000000..72a2f8c76 --- /dev/null +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -0,0 +1,152 @@ +/** + * @file ActiveSetSolver.h + * @brief Abstract class above for solving problems with the abstract set method. + * @author Ivan Dario Jimenez + * @date 1/25/16 + */ +#pragma once + +#include + +namespace gtsam { +class ActiveSetSolver { +protected: + typedef std::vector > TermsContainer; + KeySet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs + GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. + //!< used to initialize the working set factor graph, + //!< to which active inequalities will be added + VariableIndex costVariableIndex_, equalityVariableIndex_, + inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs + ActiveSetSolver() : + constrainedKeys_() { + } + /** + * Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1] + * + * @return a tuple of (alpha, factorIndex, sigmaIndex) where (factorIndex, sigmaIndex) + * is the constraint that has minimum alpha, or (-1,-1) if alpha = 1. + * This constraint will be added to the working set and become active + * in the next iteration + */ + boost::tuple computeStepSize( + const InequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p, const double& startAlpha) const { + double minAlpha = startAlpha; + int closestFactorIx = -1; + for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { + const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); + double b = factor->getb()[0]; + // only check inactive factors + if (!factor->active()) { + // Compute a'*p + double aTp = factor->dotProductRow(p); + + // Check if a'*p >0. Don't care if it's not. + if (aTp <= 0) + continue; + + // Compute a'*xk + double aTx = factor->dotProductRow(xk); + + // alpha = (b - a'*xk) / (a'*p) + double alpha = (b - aTx) / aTp; + // We want the minimum of all those max alphas + if (alpha < minAlpha) { + closestFactorIx = factorIx; + minAlpha = alpha; + } + } + } + return boost::make_tuple(minAlpha, closestFactorIx); + } +public: + /// Create a dual factor + virtual JacobianFactor::shared_ptr createDualFactor(Key key, + const InequalityFactorGraph& workingSet, + const VectorValues& delta) const = 0; + +//****************************************************************************** +/// Collect the Jacobian terms for a dual factor + template + TermsContainer collectDualJacobians(Key key, const FactorGraph &graph, + const VariableIndex &variableIndex) const { + TermsContainer Aterms; + if (variableIndex.find(key) != variableIndex.end()) { + BOOST_FOREACH(size_t factorIx, variableIndex[key]) { + typename FACTOR::shared_ptr factor = graph.at(factorIx); + if (!factor->active()) continue; + Matrix Ai = factor->getA(factor->find(key)).transpose(); + Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); + } + } + return Aterms; +} + + /** + * The goal of this function is to find currently active inequality constraints + * that violate the condition to be active. The one that violates the condition + * the most will be removed from the active set. See Nocedal06book, pg 469-471 + * + * Find the BAD active inequality that pulls x strongest to the wrong direction + * of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0) + * + * For active inequality constraints (those that are enforced as equality constraints + * in the current working set), we want lambda < 0. + * This is because: + * - From the Lagrangian L = f - lambda*c, we know that the constraint force + * is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay + * on the constraint surface, the constraint force has to balance out with + * other unconstrained forces that are pulling x towards the unconstrained + * minimum point. The other unconstrained forces are pulling x toward (-\grad f), + * hence the constraint force has to be exactly \grad f, so that the total + * force is 0. + * - We also know that at the constraint surface c(x)=0, \grad c points towards + (>= 0), + * while we are solving for - (<=0) constraint. + * - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction + * i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied. + * That means we want lambda < 0. + * - This is because when the constrained force pulls x towards the infeasible region (+), + * the unconstrained force is pulling x towards the opposite direction into + * the feasible region (again because the total force has to be 0 to make x stay still) + * So we can drop this constraint to have a lower error but feasible solution. + * + * In short, active inequality constraints with lambda > 0 are BAD, because they + * violate the condition to be active. + * + * And we want to remove the worst one with the largest lambda from the active set. + * + */ +int identifyLeavingConstraint(const InequalityFactorGraph& workingSet, + const VectorValues& lambdas) const { + int worstFactorIx = -1; + // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either + // inactive or a good inequality constraint, so we don't care! + double maxLambda = 0.0; + for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { + const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); + if (factor->active()) { + double lambda = lambdas.at(factor->dualKey())[0]; + if (lambda > maxLambda) { + worstFactorIx = factorIx; + maxLambda = lambda; + } + } + } + return worstFactorIx; +} + +//****************************************************************************** +GaussianFactorGraph::shared_ptr buildDualGraph( + const InequalityFactorGraph& workingSet, const VectorValues& delta) const { + GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); + BOOST_FOREACH(Key key, constrainedKeys_) { + // Each constrained key becomes a factor in the dual graph + JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, + delta); + if (!dualFactor->empty()) dualGraph->push_back(dualFactor); + } + return dualGraph; +} +}; +} diff --git a/gtsam_unstable/linear/InfeasibleInitialValues.h b/gtsam_unstable/linear/InfeasibleInitialValues.h index ff8606bf7..5689c90c5 100644 --- a/gtsam_unstable/linear/InfeasibleInitialValues.h +++ b/gtsam_unstable/linear/InfeasibleInitialValues.h @@ -5,13 +5,13 @@ * @author Duy-Nguyen Ta */ - - #pragma once namespace gtsam { /* ************************************************************************* */ -/** An exception indicating that the provided initial value is infeasible */ +/** An exception indicating that the provided initial value is infeasible + * Also used to indicatethat the noise model dimension passed into a + * JacobianFactor has a different dimensionality than the factor. */ class InfeasibleInitialValues: public ThreadsafeException< InfeasibleInitialValues> { public: diff --git a/gtsam_unstable/linear/LPInitSolverMatlab.h b/gtsam_unstable/linear/LPInitSolverMatlab.h index bfe1b18a1..d6f28e563 100644 --- a/gtsam_unstable/linear/LPInitSolverMatlab.h +++ b/gtsam_unstable/linear/LPInitSolverMatlab.h @@ -117,8 +117,8 @@ private: /// Collect all terms of a factor into a container. TODO: avoid memcpy? - TermsContainer collectTerms(const LinearInequality& factor) const { - TermsContainer terms; + std::vector > collectTerms(const LinearInequality& factor) const { + std::vector > terms; for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) { terms.push_back(make_pair(*it, factor.getA(it))); } @@ -126,11 +126,12 @@ private: } /// Turn Cx <= d into Cx - y <= d factors - InequalityFactorGraph addSlackVariableToInequalities(Key yKey, const InequalityFactorGraph& inequalities) const { + InequalityFactorGraph addSlackVariableToInequalities(Key yKey, + const InequalityFactorGraph& inequalities) const { InequalityFactorGraph slackInequalities; BOOST_FOREACH(const LinearInequality::shared_ptr& factor, lp_.inequalities) { - TermsContainer terms = collectTerms(*factor); // Cx - terms.push_back(make_pair(yKey, Matrix::Constant(1, 1, -1.0))); // -y + std::vector > terms = collectTerms(*factor); // Cx + terms.push_back(make_pair(yKey, Matrix::Constant(1, 1, -1.0)));// -y double d = factor->getb()[0]; slackInequalities.push_back(LinearInequality(terms, d, factor->dualKey())); } diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 76cd57a04..01ec7aa74 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -6,22 +6,22 @@ */ #pragma once + #include #include +#include +#include +#include namespace gtsam { typedef std::map KeyDimMap; -typedef std::vector > TermsContainer; -class LPSolver { +class LPSolver: public ActiveSetSolver { const LP& lp_; //!< the linear programming problem - GaussianFactorGraph baseGraph_; //!< unchanged factors needed in every iteration - VariableIndex costVariableIndex_, equalityVariableIndex_, - inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs - FastSet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs KeyDimMap keysDim_; //!< key-dim map of all variables in the constraints, used to create zero priors public: + /// Constructor LPSolver(const LP& lp) : lp_(lp) { // Push back factors that are the same in every iteration to the base graph. @@ -184,7 +184,7 @@ public: return graph; } - //****************************************************************************** + /// Find solution with the current working set VectorValues solveWithCurrentWorkingSet(const VectorValues& xk, const InequalityFactorGraph& workingSet) const { GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2 @@ -196,168 +196,88 @@ public: return workingGraph.optimize(); } - //****************************************************************************** - /// Collect the Jacobian terms for a dual factor - template - TermsContainer collectDualJacobians(Key key, const FactorGraph& graph, - const VariableIndex& variableIndex) const { - TermsContainer Aterms; - if (variableIndex.find(key) != variableIndex.end()) { - BOOST_FOREACH(size_t factorIx, variableIndex[key]) { - typename FACTOR::shared_ptr factor = graph.at(factorIx); - if (!factor->active()) continue; - Matrix Ai = factor->getA(factor->find(key)).transpose(); - Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); +//****************************************************************************** + 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(); } } - return Aterms; -} //****************************************************************************** -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(key, - lp_.equalities, equalityVariableIndex_); - TermsContainer AtermsInequalities = collectDualJacobians( - 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(); + boost::tuple computeStepSize( + const InequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p) const { + return ActiveSetSolver::computeStepSize(workingSet, xk, p, + std::numeric_limits::infinity()); } -} //****************************************************************************** -GaussianFactorGraph::shared_ptr buildDualGraph( - const InequalityFactorGraph& workingSet, const VectorValues& delta) const { - GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); - BOOST_FOREACH(Key key, constrainedKeys_) { - // Each constrained key becomes a factor in the dual graph - JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, - delta); - if (!dualFactor->empty()) dualGraph->push_back(dualFactor); - } - return dualGraph; -} + 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)); -//****************************************************************************** -int identifyLeavingConstraint(const InequalityFactorGraph& workingSet, - const VectorValues& duals) const { - int worstFactorIx = -1; - // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either - // inactive or a good inequality constraint, so we don't care! - double max_s = 0.0; - for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { - const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); - if (factor->active()) { - double s = duals.at(factor->dualKey())[0]; - if (s > max_s) { - worstFactorIx = factorIx; - max_s = s; + 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(); } - } - } - return worstFactorIx; -} - -//****************************************************************************** -std::pair computeStepSize(const InequalityFactorGraph& workingSet, - const VectorValues& xk, const VectorValues& p) const { - static bool debug = false; - - double minAlpha = std::numeric_limits::infinity(); - int closestFactorIx = -1; - for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { - const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); - double b = factor->getb()[0]; - // only check inactive factors - if (!factor->active()) { - // Compute a'*p - double aTp = factor->dotProductRow(p); - - // Check if a'*p >0. Don't care if it's not. - if (aTp <= 0) - continue; - - // Compute a'*xk - double aTx = factor->dotProductRow(xk); - - // alpha = (b - a'*xk) / (a'*p) - double alpha = (b - aTx) / aTp; - if (debug) - cout << "alpha: " << alpha << endl; - - // We want the minimum of all those max alphas - if (alpha < minAlpha) { - closestFactorIx = factorIx; - minAlpha = alpha; + else { + workingFactor->inactivate(); } + workingSet.push_back(workingFactor); } - + return workingSet; } - return std::make_pair(minAlpha, closestFactorIx); -} - //****************************************************************************** -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)); + /** 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 { - 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(); + // Initialize workingSet from the feasible initialValues + InequalityFactorGraph workingSet = identifyActiveConstraints( + lp_.inequalities, initialValues, duals); + LPState state(initialValues, duals, workingSet, false, 0); - if (fabs(error) < 1e-7) { - workingFactor->activate(); + /// main loop of the solver + while (!state.converged) { + state = iterate(state); } - else { - workingFactor->inactivate(); - } - workingSet.push_back(workingFactor); - } - return workingSet; -} -//****************************************************************************** -/** 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); } - return make_pair(state.values, state.duals); -} - //****************************************************************************** -/** - * Optimize without initial values - * TODO: Find a feasible initial solution wrt inequality constraints - */ + /** + * Optimize without initial values + * TODO: Find a feasible initial solution wrt inequality constraints + */ // pair optimize() const { // // // Initialize workingSet from the feasible initialValues diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index f953cc1d6..6691f3deb 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -19,7 +19,7 @@ #include #include #include - +#include #include using namespace std; @@ -27,7 +27,8 @@ using namespace std; namespace gtsam { //****************************************************************************** -QPSolver::QPSolver(const QP& qp) : qp_(qp) { +QPSolver::QPSolver(const QP& qp) : + qp_(qp) { baseGraph_ = qp_.cost; baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end()); costVariableIndex_ = VariableIndex(qp_.cost); @@ -37,13 +38,13 @@ QPSolver::QPSolver(const QP& qp) : qp_(qp) { constrainedKeys_.merge(qp_.inequalities.keys()); } -//****************************************************************************** +//***************************************************cc*************************** VectorValues QPSolver::solveWithCurrentWorkingSet( const InequalityFactorGraph& workingSet) const { GaussianFactorGraph workingGraph = baseGraph_; BOOST_FOREACH(const LinearInequality::shared_ptr& factor, workingSet) { if (factor->active()) - workingGraph.push_back(factor); + workingGraph.push_back(factor); } return workingGraph.optimize(); } @@ -53,10 +54,11 @@ JacobianFactor::shared_ptr QPSolver::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 - std::vector > Aterms = collectDualJacobians + std::vector < std::pair > Aterms = collectDualJacobians < LinearEquality > (key, qp_.equalities, equalityVariableIndex_); - std::vector > AtermsInequalities = collectDualJacobians - < LinearInequality > (key, workingSet, inequalityVariableIndex_); + std::vector < std::pair > AtermsInequalities = + collectDualJacobians < LinearInequality + > (key, workingSet, inequalityVariableIndex_); Aterms.insert(Aterms.end(), AtermsInequalities.begin(), AtermsInequalities.end()); @@ -64,50 +66,15 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, if (Aterms.size() > 0) { Vector b = zero(delta.at(key).size()); if (costVariableIndex_.find(key) != costVariableIndex_.end()) { - BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { - GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); - b += factor->gradient(key, delta); - } - } - return boost::make_shared(Aterms, b); // compute the least-square approximation of dual variables - } - else { - return boost::make_shared(); - } -} - -//****************************************************************************** -GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph( - const InequalityFactorGraph& workingSet, const VectorValues& delta) const { - GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); - BOOST_FOREACH(Key key, constrainedKeys_) { - // Each constrained key becomes a factor in the dual graph - JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, delta); - if (!dualFactor->empty()) - dualGraph->push_back(dualFactor); - } - return dualGraph; -} - -//****************************************************************************** -int QPSolver::identifyLeavingConstraint( - const InequalityFactorGraph& workingSet, - const VectorValues& lambdas) const { - int worstFactorIx = -1; - // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either - // inactive or a good inequality constraint, so we don't care! - double maxLambda = 0.0; - for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { - const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); - if (factor->active()) { - double lambda = lambdas.at(factor->dualKey())[0]; - if (lambda > maxLambda) { - worstFactorIx = factorIx; - maxLambda = lambda; - } + BOOST_FOREACH(size_t factorIx, costVariableIndex_[key]) { + GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); + b += factor->gradient(key, delta); } } - return worstFactorIx; + return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables +} else { + return boost::make_shared(); +} } //****************************************************************************** @@ -127,154 +94,122 @@ int QPSolver::identifyLeavingConstraint( * We want the minimum of all those alphas among all inactive inequality. */ boost::tuple QPSolver::computeStepSize( - const InequalityFactorGraph& workingSet, const VectorValues& xk, - const VectorValues& p) const { - static bool debug = false; - - double minAlpha = 1.0; - int closestFactorIx = -1; - for(size_t factorIx = 0; factorIxgetb()[0]; - // only check inactive factors - if (!factor->active()) { - // Compute a'*p - double aTp = factor->dotProductRow(p); - - // Check if a'*p >0. Don't care if it's not. - if (aTp <= 0) - continue; - - // Compute a'*xk - double aTx = factor->dotProductRow(xk); - - // alpha = (b - a'*xk) / (a'*p) - double alpha = (b - aTx) / aTp; - if (debug) - cout << "alpha: " << alpha << endl; - - // We want the minimum of all those max alphas - if (alpha < minAlpha) { - closestFactorIx = factorIx; - minAlpha = alpha; - } - } - - } - - return boost::make_tuple(minAlpha, closestFactorIx); + const InequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p) const { +return ActiveSetSolver::computeStepSize(workingSet, xk, p, 1); } //****************************************************************************** QPState QPSolver::iterate(const QPState& state) const { - static bool debug = false; +static bool debug = false; - // Algorithm 16.3 from Nocedal06book. - // Solve with the current working set eqn 16.39, but instead of solving for p solve for x - VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet); +// Algorithm 16.3 from Nocedal06book. +// Solve with the current working set eqn 16.39, but instead of solving for p solve for x +VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet); +if (debug) + newValues.print("New solution:"); + +// If we CAN'T move further +// if p_k = 0 is the original condition, modified by Duy to say that the state update is zero. +if (newValues.equals(state.values, 1e-7)) { + // Compute lambda from the dual graph if (debug) - newValues.print("New solution:"); + cout << "Building dual graph..." << endl; + GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, + newValues); + if (debug) + dualGraph->print("Dual graph: "); + VectorValues duals = dualGraph->optimize(); + if (debug) + duals.print("Duals :"); - // If we CAN'T move further - // if p_k = 0 is the original condition, modified by Duy to say that the state update is zero. - if (newValues.equals(state.values, 1e-7)) { - // Compute lambda from the dual graph - if (debug) - cout << "Building dual graph..." << endl; - GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, newValues); - if (debug) - dualGraph->print("Dual graph: "); - VectorValues duals = dualGraph->optimize(); - if (debug) - duals.print("Duals :"); + int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); + if (debug) + cout << "leavingFactor: " << leavingFactor << endl; - int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); - if (debug) - cout << "leavingFactor: " << leavingFactor << endl; - - // If all inequality constraints are satisfied: We have the solution!! - if (leavingFactor < 0) { - return QPState(newValues, duals, state.workingSet, true, state.iterations+1); - } - else { - // Inactivate the leaving constraint - InequalityFactorGraph newWorkingSet = state.workingSet; - newWorkingSet.at(leavingFactor)->inactivate(); - return QPState(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 - double alpha; - int factorIx; - VectorValues p = newValues - state.values; - boost::tie(alpha, factorIx) = // using 16.41 - computeStepSize(state.workingSet, state.values, p); - if (debug) - cout << "alpha, factorIx: " << alpha << " " << factorIx << " " - << endl; - - // also add to the working set the one that complains the most + // If all inequality constraints are satisfied: We have the solution!! + if (leavingFactor < 0) { + return QPState(newValues, duals, state.workingSet, true, + state.iterations + 1); + } else { + // Inactivate the leaving constraint InequalityFactorGraph newWorkingSet = state.workingSet; - if (factorIx >= 0) - newWorkingSet.at(factorIx)->activate(); - - // step! - newValues = state.values + alpha * p; - - return QPState(newValues, state.duals, newWorkingSet, false, state.iterations+1); + newWorkingSet.at(leavingFactor)->inactivate(); + return QPState(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 + double alpha; + int factorIx; + VectorValues p = newValues - state.values; + boost::tie(alpha, factorIx) = // using 16.41 + computeStepSize(state.workingSet, state.values, p); + if (debug) + cout << "alpha, factorIx: " << alpha << " " << factorIx << " " << endl; + + // 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 QPState(newValues, state.duals, newWorkingSet, false, + state.iterations + 1); +} } //****************************************************************************** InequalityFactorGraph QPSolver::identifyActiveConstraints( - const InequalityFactorGraph& inequalities, - const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const { - InequalityFactorGraph workingSet; - BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) { - LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); - if (useWarmStart == true && duals.exists(workingFactor->dualKey())) { - workingFactor->activate(); - } - else { - if (useWarmStart == true && duals.size() > 0) { - workingFactor->inactivate(); - } else { - double error = workingFactor->error(initialValues); - // TODO: find a feasible initial point for QPSolver. - // For now, we just throw an exception, since we don't have an LPSolver to do this yet - if (error > 0) - throw InfeasibleInitialValues(); + const InequalityFactorGraph& inequalities, const VectorValues& initialValues, + const VectorValues& duals, bool useWarmStart) const { +InequalityFactorGraph workingSet; +BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) { + LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); + if (useWarmStart == true && duals.exists(workingFactor->dualKey())) { + workingFactor->activate(); + } + else { + if (useWarmStart == true && duals.size() > 0) { + workingFactor->inactivate(); + } else { + double error = workingFactor->error(initialValues); + // TODO: find a feasible initial point for QPSolver. + // For now, we just throw an exception, since we don't have an LPSolver to do this yet + if (error > 0) + throw InfeasibleInitialValues(); - if (fabs(error)<1e-7) { - workingFactor->activate(); - } - else { - workingFactor->inactivate(); - } + if (fabs(error)<1e-7) { + workingFactor->activate(); + } + else { + workingFactor->inactivate(); } } - workingSet.push_back(workingFactor); } - return workingSet; + workingSet.push_back(workingFactor); +} +return workingSet; } //****************************************************************************** pair QPSolver::optimize( - const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const { + const VectorValues& initialValues, const VectorValues& duals, + bool useWarmStart) const { - // Initialize workingSet from the feasible initialValues - InequalityFactorGraph workingSet = - identifyActiveConstraints(qp_.inequalities, initialValues, duals, useWarmStart); - QPState state(initialValues, duals, workingSet, false, 0); +// Initialize workingSet from the feasible initialValues +InequalityFactorGraph workingSet = identifyActiveConstraints(qp_.inequalities, + initialValues, duals, useWarmStart); +QPState state(initialValues, duals, workingSet, false, 0); - /// main loop of the solver - while (!state.converged) { - state = iterate(state); - } +/// main loop of the solver +while (!state.converged) { + state = iterate(state); +} - return make_pair(state.values, state.duals); +return make_pair(state.values, state.duals); } } /* namespace gtsam */ diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 812b512fa..00ae8e6a7 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -20,47 +20,21 @@ #include #include +#include +#include #include #include namespace gtsam { - -/// This struct holds the state of QPSolver at each iteration -struct QPState { - VectorValues values; - VectorValues duals; - InequalityFactorGraph workingSet; - bool converged; - size_t iterations; - - /// default constructor - QPState() : - values(), duals(), workingSet(), converged(false), iterations(0) { - } - - /// constructor with initial values - QPState(const VectorValues& initialValues, const VectorValues& initialDuals, - const InequalityFactorGraph& initialWorkingSet, bool _converged, size_t _iterations) : - values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged( - _converged), iterations(_iterations) { - } -}; - /** * This QPSolver uses the active set method to solve a quadratic programming problem * defined in the QP struct. * Note: This version of QPSolver only works with a feasible initial value. */ -class QPSolver { +class QPSolver: public ActiveSetSolver { const QP& qp_; //!< factor graphs of the QP problem, can't be modified! - GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. - //!< used to initialize the working set factor graph, - //!< to which active inequalities will be added - VariableIndex costVariableIndex_, equalityVariableIndex_, - inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs - FastSet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs public: /// Constructor @@ -70,109 +44,11 @@ public: VectorValues solveWithCurrentWorkingSet( const InequalityFactorGraph& workingSet) const; - /// @name Build the dual graph - /// @{ - - /// Collect the Jacobian terms for a dual factor - template - std::vector > collectDualJacobians(Key key, - const FactorGraph& graph, - const VariableIndex& variableIndex) const { - std::vector > Aterms; - if (variableIndex.find(key) != variableIndex.end()) { - BOOST_FOREACH(size_t factorIx, variableIndex[key]) { - typename FACTOR::shared_ptr factor = graph.at(factorIx); - if (!factor->active()) continue; - Matrix Ai = factor->getA(factor->find(key)).transpose(); - Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); - } - } - return Aterms; - } - /// Create a dual factor JacobianFactor::shared_ptr createDualFactor(Key key, - const InequalityFactorGraph& workingSet, - const VectorValues& delta) const; - - /** - * Build the dual graph to solve for the Lagrange multipliers. - * - * The Lagrangian function is: - * L(X,lambdas) = f(X) - \sum_k lambda_k * c_k(X), - * where the unconstrained part is - * f(X) = 0.5*X'*G*X - X'*g + 0.5*f0 - * and the linear equality constraints are - * c1(X), c2(X), ..., cm(X) - * - * Take the derivative of L wrt X at the solution and set it to 0, we have - * \grad f(X) = \sum_k lambda_k * \grad c_k(X) (*) - * - * For each set of rows of (*) corresponding to a variable xi involving in some constraints - * we have: - * \grad f(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi - * \grad c_k(xi) = \frac{\partial c_k}{\partial xi}' - * - * Note: If xi does not involve in any constraint, we have the trivial condition - * \grad f(Xi) = 0, which should be satisfied as a usual condition for unconstrained variables. - * - * So each variable xi involving in some constraints becomes a linear factor A*lambdas - b = 0 - * on the constraints' lambda multipliers, as follows: - * - The jacobian term A_k for each lambda_k is \grad c_k(xi) - * - The constant term b is \grad f(xi), which can be computed from all unconstrained - * Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi - */ - GaussianFactorGraph::shared_ptr buildDualGraph( - const InequalityFactorGraph& workingSet, - const VectorValues& delta) const; - + const InequalityFactorGraph& workingSet, const VectorValues& delta) const; /// @} - /** - * The goal of this function is to find currently active inequality constraints - * that violate the condition to be active. The one that violates the condition - * the most will be removed from the active set. See Nocedal06book, pg 469-471 - * - * Find the BAD active inequality that pulls x strongest to the wrong direction - * of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0) - * - * For active inequality constraints (those that are enforced as equality constraints - * in the current working set), we want lambda < 0. - * This is because: - * - From the Lagrangian L = f - lambda*c, we know that the constraint force - * is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay - * on the constraint surface, the constraint force has to balance out with - * other unconstrained forces that are pulling x towards the unconstrained - * minimum point. The other unconstrained forces are pulling x toward (-\grad f), - * hence the constraint force has to be exactly \grad f, so that the total - * force is 0. - * - We also know that at the constraint surface c(x)=0, \grad c points towards + (>= 0), - * while we are solving for - (<=0) constraint. - * - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction - * i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied. - * That means we want lambda < 0. - * - This is because when the constrained force pulls x towards the infeasible region (+), - * the unconstrained force is pulling x towards the opposite direction into - * the feasible region (again because the total force has to be 0 to make x stay still) - * So we can drop this constraint to have a lower error but feasible solution. - * - * In short, active inequality constraints with lambda > 0 are BAD, because they - * violate the condition to be active. - * - * And we want to remove the worst one with the largest lambda from the active set. - * - */ - int identifyLeavingConstraint(const InequalityFactorGraph& workingSet, - const VectorValues& lambdas) const; - - /** - * Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1] - * - * @return a tuple of (alpha, factorIndex, sigmaIndex) where (factorIndex, sigmaIndex) - * is the constraint that has minimum alpha, or (-1,-1) if alpha = 1. - * This constraint will be added to the working set and become active - * in the next iteration - */ boost::tuple computeStepSize( const InequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const; @@ -185,8 +61,8 @@ public: */ InequalityFactorGraph identifyActiveConstraints( const InequalityFactorGraph& inequalities, - const VectorValues& initialValues, - const VectorValues& duals = VectorValues(), bool useWarmStart = true) const; + const VectorValues& initialValues, const VectorValues& duals = + VectorValues(), bool useWarmStart = true) const; /** Optimize with a provided initial values * For this version, it is the responsibility of the caller to provide @@ -194,28 +70,9 @@ public: * @return a pair of solutions */ std::pair optimize( - const VectorValues& initialValues, const VectorValues& duals = VectorValues(), bool useWarmStart = true) const; + const VectorValues& initialValues, const VectorValues& duals = + VectorValues(), bool useWarmStart = true) const; }; -/* ************************************************************************* */ -/** An exception indicating that the noise model dimension passed into a - * JacobianFactor has a different dimensionality than the factor. */ -class InfeasibleInitialValues : public ThreadsafeException { -public: - InfeasibleInitialValues() {} - virtual ~InfeasibleInitialValues() throw() {} - - virtual const char* what() const throw() { - if(description_.empty()) - description_ = "An infeasible intial value was provided for the QPSolver.\n" - "This current version of QPSolver does not handle infeasible" - "initial point due to the lack of a LPSolver.\n"; - return description_.c_str(); - } - -private: - mutable std::string description_; -}; - } /* namespace gtsam */ diff --git a/gtsam_unstable/linear/QPState.h b/gtsam_unstable/linear/QPState.h new file mode 100644 index 000000000..d88a88c5f --- /dev/null +++ b/gtsam_unstable/linear/QPState.h @@ -0,0 +1,29 @@ +// +// Created by ivan on 1/25/16. +// + +#pragma once + +namespace gtsam { +/// This struct holds the state of QPSolver at each iteration +struct QPState { + VectorValues values; + VectorValues duals; + InequalityFactorGraph workingSet; + bool converged; + size_t iterations; + + /// default constructor + QPState() : + values(), duals(), workingSet(), converged(false), iterations(0) { + } + + /// constructor with initial values + QPState(const VectorValues& initialValues, const VectorValues& initialDuals, + const InequalityFactorGraph& initialWorkingSet, bool _converged, + size_t _iterations) : + values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged( + _converged), iterations(_iterations) { + } +}; +} diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index d8de4fd7a..259de3430 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -21,6 +21,7 @@ #include #include +#include using namespace std; using namespace gtsam; @@ -115,8 +116,7 @@ TEST(QPSolver, dual) { QPSolver solver(qp); - GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph( - qp.inequalities, initialValues); + GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph(qp.inequalities, initialValues); VectorValues dual = dualGraph->optimize(); VectorValues expectedDual; expectedDual.insert(0, (Vector(1) << 2.0).finished());