From 26a76476295e5da4d6c8319efc6cf7bc3d4c53a9 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 29 Jan 2016 09:07:14 -0800 Subject: [PATCH] Some refactoring, small edits, TODOs for Ivan --- gtsam_unstable/linear/ActiveSetSolver.h | 171 ++++++++-------- gtsam_unstable/linear/InequalityFactorGraph.h | 3 +- gtsam_unstable/linear/LPSolver.cpp | 109 +++++----- gtsam_unstable/linear/LPSolver.h | 26 ++- gtsam_unstable/linear/LPState.h | 2 + gtsam_unstable/linear/LinearInequality.h | 1 + gtsam_unstable/linear/QPSolver.cpp | 192 +++++++++--------- gtsam_unstable/linear/QPSolver.h | 17 +- 8 files changed, 262 insertions(+), 259 deletions(-) diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h index 288488331..560414419 100644 --- a/gtsam_unstable/linear/ActiveSetSolver.h +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -2,6 +2,7 @@ * @file ActiveSetSolver.h * @brief Abstract class above for solving problems with the abstract set method. * @author Ivan Dario Jimenez + * @author Duy Nguyen Ta * @date 1/25/16 */ #pragma once @@ -11,78 +12,39 @@ namespace gtsam { class ActiveSetSolver { -protected: +public: typedef std::vector > TermsContainer; + +protected: 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 { + /// 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)); + 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; } - return Aterms; -} /** * The goal of this function is to find currently active inequality constraints @@ -118,36 +80,83 @@ public: * 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; + 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; } - 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); + /// TODO: comment + 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; } - return dualGraph; -} + +protected: + + 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); + } + }; -} +} // namespace gtsam diff --git a/gtsam_unstable/linear/InequalityFactorGraph.h b/gtsam_unstable/linear/InequalityFactorGraph.h index 5c1b17125..2feade32a 100644 --- a/gtsam_unstable/linear/InequalityFactorGraph.h +++ b/gtsam_unstable/linear/InequalityFactorGraph.h @@ -18,8 +18,9 @@ #pragma once -#include #include +#include +#include namespace gtsam { diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index 4fd2d21a5..ccfb65fc0 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -6,24 +6,24 @@ */ #include -#include #include - +#include namespace gtsam { -LPSolver::LPSolver(const LP &lp) : - lp_(lp) { +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 + // 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 + // 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 + // 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 @@ -36,7 +36,7 @@ LPSolver::LPSolver(const LP &lp) : 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) { + for (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))); @@ -47,35 +47,36 @@ GaussianFactorGraph::shared_ptr LPSolver::createZeroPriors( LPState LPSolver::iterate(const LPState &state) const { // Solve with the current working set - // LP: project the objective neggradient to the constraint's null space + // LP: project the objective neg. gradient to the constraint's null space // to find the direction to move - VectorValues newValues = solveWithCurrentWorkingSet(state.values, - state.workingSet); + 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 + // Find and remove the bad inequality 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 + // 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); + 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 + // LP: see which inequality 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! + // 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); + 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); + return LPState(newValues, duals, newWorkingSet, false, state.iterations + 1); } } else { // If we CAN make some progress, i.e. p_k != 0 @@ -86,16 +87,14 @@ LPState LPSolver::iterate(const LPState &state) const { double alpha; int factorIx; VectorValues p = newValues - state.values; - boost::tie(alpha, factorIx) = // using 16.41 + 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(); + if (factorIx >= 0) newWorkingSet.at(factorIx)->activate(); // step! newValues = state.values + alpha * p; - return LPState(newValues, state.duals, newWorkingSet, false, - state.iterations + 1); + return LPState(newValues, state.duals, newWorkingSet, false, state.iterations + 1); } } @@ -114,37 +113,35 @@ GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors( } VectorValues LPSolver::solveWithCurrentWorkingSet( - const VectorValues &xk, - const InequalityFactorGraph &workingSet) const { - GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2 + 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) { + for (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, + 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_); + // 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()); + 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 + if (it != lp_.cost.end()) b = lp_.cost.getA(it).transpose(); + return boost::make_shared( + Aterms, b); // compute the least-square approximation of dual variables } else { return boost::make_shared(); } @@ -152,10 +149,9 @@ boost::shared_ptr LPSolver::createDualFactor( InequalityFactorGraph LPSolver::identifyActiveConstraints( const InequalityFactorGraph &inequalities, - const VectorValues &initialValues, - const VectorValues &duals) const { + const VectorValues &initialValues, const VectorValues &duals) const { InequalityFactorGraph workingSet; - BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) { + for (const LinearInequality::shared_ptr &factor : inequalities) { LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); double error = workingFactor->error(initialValues); @@ -165,8 +161,7 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints( if (fabs(error) < 1e-7) { workingFactor->activate(); - } - else { + } else { workingFactor->inactivate(); } workingSet.push_back(workingFactor); @@ -175,29 +170,25 @@ InequalityFactorGraph LPSolver::identifyActiveConstraints( } std::pair LPSolver::optimize( - const VectorValues &initialValues, - const VectorValues &duals) const { + const VectorValues &initialValues, const VectorValues &duals) const { { - // Initialize workingSet from the feasible initialValues - InequalityFactorGraph workingSet = identifyActiveConstraints( - lp_.inequalities, initialValues, duals); + InequalityFactorGraph workingSet = + identifyActiveConstraints(lp_.inequalities, initialValues, duals); LPState state(initialValues, duals, workingSet, false, 0); /// main loop of the solver - while (!state.converged) { + 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 InequalityFactorGraph &workingSet, const VectorValues &xk, const VectorValues &p) const { - return ActiveSetSolver::computeStepSize(workingSet, xk, p, - std::numeric_limits::infinity()); + 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 2d6a2433a..762431010 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -2,6 +2,7 @@ * @file LPSolver.h * @brief Class used to solve Linear Programming Problems as defined in LP.h * @author Ivan Dario Jimenez + * @author Duy Nguyen Ta * @date 1/24/16 */ @@ -10,9 +11,10 @@ #include #include #include -#include #include +#include + namespace gtsam { typedef std::map KeyDimMap; @@ -28,11 +30,12 @@ public: const LP& lp() const { return lp_; } + const KeyDimMap& keysDim() const { return keysDim_; } - //****************************************************************************** + /// TODO(comment) template KeyDimMap collectKeysDim(const LinearGraph& linearGraph) const { KeyDimMap keysDim; @@ -44,17 +47,13 @@ public: return keysDim; } - //****************************************************************************** - /** - * Create a zero prior for any keys in the graph that don't exist in the cost - */ + /// 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; - //****************************************************************************** + /// TODO(comment) LPState iterate(const LPState& state) const; - //****************************************************************************** /** * Create the factor ||x-xk - (-g)||^2 where xk is the current feasible solution * on the constraint surface and g is the gradient of the linear cost, @@ -74,28 +73,27 @@ public: VectorValues solveWithCurrentWorkingSet(const VectorValues& xk, const InequalityFactorGraph& workingSet) const; -//****************************************************************************** + /// TODO(comment) JacobianFactor::shared_ptr createDualFactor(Key key, const InequalityFactorGraph& workingSet, const VectorValues& delta) const; -//****************************************************************************** + /// TODO(comment) boost::tuple computeStepSize( const InequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const; -//****************************************************************************** + /// TODO(comment) 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 */ pair optimize(const VectorValues& initialValues, const VectorValues& duals = VectorValues()) const; -//****************************************************************************** /** * Optimize without initial values * TODO: Find a feasible initial solution wrt inequality constraints @@ -115,4 +113,4 @@ public: // return make_pair(state.values, state.duals); // } }; -} +} // namespace gtsam diff --git a/gtsam_unstable/linear/LPState.h b/gtsam_unstable/linear/LPState.h index 8b52170e4..fee5d3aa8 100644 --- a/gtsam_unstable/linear/LPState.h +++ b/gtsam_unstable/linear/LPState.h @@ -10,7 +10,9 @@ namespace gtsam { +// TODO: comment struct LPState { + // TODO: comment member variables VectorValues values; VectorValues duals; InequalityFactorGraph workingSet; diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h index 4d1e3aaee..ad0bb3f02 100644 --- a/gtsam_unstable/linear/LinearInequality.h +++ b/gtsam_unstable/linear/LinearInequality.h @@ -19,6 +19,7 @@ #pragma once #include +#include namespace gtsam { diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 5b5abb018..7473994b0 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -27,8 +27,7 @@ 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); @@ -42,39 +41,41 @@ QPSolver::QPSolver(const QP& qp) : 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); + for (const LinearInequality::shared_ptr& factor : workingSet) { + if (factor->active()) workingGraph.push_back(factor); } return workingGraph.optimize(); } //****************************************************************************** -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 < std::pair > Aterms = collectDualJacobians - < LinearEquality > (key, qp_.equalities, equalityVariableIndex_); - std::vector < std::pair > AtermsInequalities = - collectDualJacobians < LinearInequality - > (key, workingSet, inequalityVariableIndex_); +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(key, qp_.equalities, + equalityVariableIndex_); + std::vector > AtermsInequalities = + collectDualJacobians(key, workingSet, + inequalityVariableIndex_); Aterms.insert(Aterms.end(), AtermsInequalities.begin(), - AtermsInequalities.end()); + AtermsInequalities.end()); // Collect the gradients of unconstrained cost factors to the b vector 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); + for (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(); } - return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables -} else { - return boost::make_shared(); -} } //****************************************************************************** @@ -94,102 +95,101 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, * 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 { -return ActiveSetSolver::computeStepSize(workingSet, xk, p, 1); + const InequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p) const { + return ActiveSetSolver::computeStepSize(workingSet, xk, p, 1); } //****************************************************************************** QPState QPSolver::iterate(const QPState& state) const { -// 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 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 - GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, - newValues); - VectorValues duals = dualGraph->optimize(); - int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); - // If all inequality constraints are satisfied: We have the solution!! - if (leavingFactor < 0) { - return QPState(newValues, duals, state.workingSet, true, - state.iterations + 1); + // 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 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 + GaussianFactorGraph::shared_ptr dualGraph = + buildDualGraph(state.workingSet, newValues); + VectorValues duals = dualGraph->optimize(); + int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); + // 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 { - // Inactivate the leaving constraint + // 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); + // also add to the working set the one that complains the most InequalityFactorGraph newWorkingSet = state.workingSet; - newWorkingSet.at(leavingFactor)->inactivate(); - return QPState(newValues, duals, newWorkingSet, false, state.iterations + 1); + if (factorIx >= 0) newWorkingSet.at(factorIx)->activate(); + // step! + newValues = state.values + alpha * p; + return QPState(newValues, state.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); - // 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(); + 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 == true && duals.exists(workingFactor->dualKey())) { + workingFactor->activate(); } 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 { + 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(); + } } } + workingSet.push_back(workingFactor); } - workingSet.push_back(workingFactor); -} -return workingSet; + 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 0cede4a1d..fd30945e3 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -13,20 +13,22 @@ * @file QPSolver.h * @brief A quadratic programming solver implements the active set method * @date Apr 15, 2014 + * @author Ivan Dario Jimenez * @author Duy-Nguyen Ta */ #pragma once -#include #include #include #include +#include #include #include namespace gtsam { + /** * This QPSolver uses the active set method to solve a quadratic programming problem * defined in the QP struct. @@ -48,24 +50,23 @@ public: /// Create a dual factor JacobianFactor::shared_ptr createDualFactor(Key key, const InequalityFactorGraph& workingSet, const VectorValues& delta) const; - /// @} + /// TODO(comment) boost::tuple computeStepSize( const InequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const; - /** Iterate 1 step, return a new state with a new workingSet and values */ + /// 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. - */ + /// 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 a provided initial values + /** + * Optimize with provided initial values * For this version, it is the responsibility of the caller to provide * a feasible initial value, otherwise, an exception will be thrown. * @return a pair of solutions @@ -76,4 +77,4 @@ public: }; -} /* namespace gtsam */ +} // namespace gtsam