12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

[REFACTOR] Extracted common components from QPSolver and LPSolver into ActiveSetSolver.

release/4.3a0
Ivan Jimenez 2016-01-25 19:24:37 -05:00
parent 2978664cbd
commit 84662bc5d9
8 changed files with 375 additions and 481 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

152
gtsam_unstable/linear/ActiveSetSolver.h Normal file
View File

@ -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 <boost/range/adaptor/map.hpp>
namespace gtsam {
class ActiveSetSolver {
protected:
typedef std::vector<std::pair<Key, Matrix> > 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<double, int> 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<typename FACTOR>
TermsContainer collectDualJacobians(Key key, const FactorGraph<FACTOR> &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;
}
};
}

6
gtsam_unstable/linear/InfeasibleInitialValues.h
View File

@ -5,13 +5,13 @@
* @author Duy-Nguyen Ta * @author Duy-Nguyen Ta
*/ */
#pragma once #pragma once
namespace gtsam { 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< class InfeasibleInitialValues: public ThreadsafeException<
InfeasibleInitialValues> { InfeasibleInitialValues> {
public: public:

11
gtsam_unstable/linear/LPInitSolverMatlab.h
View File

@ -117,8 +117,8 @@ private:
/// Collect all terms of a factor into a container. TODO: avoid memcpy? /// Collect all terms of a factor into a container. TODO: avoid memcpy?
TermsContainer collectTerms(const LinearInequality& factor) const { std::vector<std::pair<Key, Matrix> > collectTerms(const LinearInequality& factor) const {
TermsContainer terms; std::vector<std::pair<Key, Matrix> > terms;
for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) { for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) {
terms.push_back(make_pair(*it, factor.getA(it))); terms.push_back(make_pair(*it, factor.getA(it)));
} }
@ -126,11 +126,12 @@ private:
} }
/// Turn Cx <= d into Cx - y <= d factors /// 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; InequalityFactorGraph slackInequalities;
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, lp_.inequalities) { BOOST_FOREACH(const LinearInequality::shared_ptr& factor, lp_.inequalities) {
TermsContainer terms = collectTerms(*factor); // Cx std::vector<std::pair<Key, Matrix> > terms = collectTerms(*factor); // Cx
terms.push_back(make_pair(yKey, Matrix::Constant(1, 1, -1.0))); // -y terms.push_back(make_pair(yKey, Matrix::Constant(1, 1, -1.0)));// -y
double d = factor->getb()[0]; double d = factor->getb()[0];
slackInequalities.push_back(LinearInequality(terms, d, factor->dualKey())); slackInequalities.push_back(LinearInequality(terms, d, factor->dualKey()));
} }

136
gtsam_unstable/linear/LPSolver.h
View File

@ -6,22 +6,22 @@
*/ */
#pragma once #pragma once
#include <gtsam_unstable/linear/LPState.h> #include <gtsam_unstable/linear/LPState.h>
#include <gtsam_unstable/linear/LP.h> #include <gtsam_unstable/linear/LP.h>
#include <gtsam_unstable/linear/ActiveSetSolver.h>
#include <boost/range/adaptor/map.hpp>
#include <gtsam/linear/VectorValues.h>
namespace gtsam { namespace gtsam {
typedef std::map<Key, size_t> KeyDimMap; typedef std::map<Key, size_t> KeyDimMap;
typedef std::vector<std::pair<Key, Matrix> > TermsContainer;
class LPSolver { class LPSolver: public ActiveSetSolver {
const LP& lp_; //!< the linear programming problem 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<Key> 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 KeyDimMap keysDim_; //!< key-dim map of all variables in the constraints, used to create zero priors
public: public:
/// Constructor
LPSolver(const LP& lp) : LPSolver(const LP& lp) :
lp_(lp) { lp_(lp) {
// Push back factors that are the same in every iteration to the base graph. // Push back factors that are the same in every iteration to the base graph.
@ -184,7 +184,7 @@ public:
return graph; return graph;
} }
//****************************************************************************** /// Find solution with the current working set
VectorValues solveWithCurrentWorkingSet(const VectorValues& xk, VectorValues solveWithCurrentWorkingSet(const VectorValues& xk,
const InequalityFactorGraph& workingSet) const { const InequalityFactorGraph& workingSet) const {
GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2 GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2
@ -196,32 +196,16 @@ public:
return workingGraph.optimize(); return workingGraph.optimize();
} }
//******************************************************************************
/// Collect the Jacobian terms for a dual factor
template<typename FACTOR>
TermsContainer collectDualJacobians(Key key, const FactorGraph<FACTOR>& 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;
}
//****************************************************************************** //******************************************************************************
JacobianFactor::shared_ptr createDualFactor(Key key, JacobianFactor::shared_ptr createDualFactor(Key key,
const InequalityFactorGraph& workingSet, const VectorValues& delta) const { const InequalityFactorGraph& workingSet,
const VectorValues& delta) const {
// Transpose the A matrix of constrained factors to have the jacobian of the dual key // Transpose the A matrix of constrained factors to have the jacobian of the dual key
TermsContainer Aterms = collectDualJacobians<LinearEquality>(key, TermsContainer Aterms = collectDualJacobians < LinearEquality
lp_.equalities, equalityVariableIndex_); > (key, lp_.equalities, equalityVariableIndex_);
TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>( TermsContainer AtermsInequalities = collectDualJacobians < LinearInequality
key, workingSet, inequalityVariableIndex_); > (key, workingSet, inequalityVariableIndex_);
Aterms.insert(Aterms.end(), AtermsInequalities.begin(), Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
AtermsInequalities.end()); AtermsInequalities.end());
@ -235,82 +219,18 @@ JacobianFactor::shared_ptr createDualFactor(Key key,
} else { } else {
return boost::make_shared<JacobianFactor>(); return boost::make_shared<JacobianFactor>();
} }
} }
//****************************************************************************** //******************************************************************************
GaussianFactorGraph::shared_ptr buildDualGraph( boost::tuple<double, int> computeStepSize(
const InequalityFactorGraph& workingSet, const VectorValues& delta) const { const InequalityFactorGraph& workingSet, const VectorValues& xk,
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); const VectorValues& p) const {
BOOST_FOREACH(Key key, constrainedKeys_) { return ActiveSetSolver::computeStepSize(workingSet, xk, p,
// Each constrained key becomes a factor in the dual graph std::numeric_limits<double>::infinity());
JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet,
delta);
if (!dualFactor->empty()) dualGraph->push_back(dualFactor);
} }
return dualGraph;
}
//****************************************************************************** //******************************************************************************
int identifyLeavingConstraint(const InequalityFactorGraph& workingSet, InequalityFactorGraph identifyActiveConstraints(
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;
}
}
}
return worstFactorIx;
}
//******************************************************************************
std::pair<double, int> computeStepSize(const InequalityFactorGraph& workingSet,
const VectorValues& xk, const VectorValues& p) const {
static bool debug = false;
double minAlpha = std::numeric_limits<double>::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;
}
}
}
return std::make_pair(minAlpha, closestFactorIx);
}
//******************************************************************************
InequalityFactorGraph identifyActiveConstraints(
const InequalityFactorGraph& inequalities, const InequalityFactorGraph& inequalities,
const VectorValues& initialValues, const VectorValues& duals) const { const VectorValues& initialValues, const VectorValues& duals) const {
InequalityFactorGraph workingSet; InequalityFactorGraph workingSet;
@ -331,18 +251,18 @@ InequalityFactorGraph identifyActiveConstraints(
workingSet.push_back(workingFactor); workingSet.push_back(workingFactor);
} }
return workingSet; return workingSet;
} }
//****************************************************************************** //******************************************************************************
/** Optimize with the provided feasible initial values /** Optimize with the provided feasible initial values
* TODO: throw exception if the initial values is not feasible wrt inequality constraints * TODO: throw exception if the initial values is not feasible wrt inequality constraints
*/ */
pair<VectorValues, VectorValues> optimize(const VectorValues& initialValues, pair<VectorValues, VectorValues> optimize(const VectorValues& initialValues,
const VectorValues& duals = VectorValues()) const { const VectorValues& duals = VectorValues()) const {
// Initialize workingSet from the feasible initialValues // Initialize workingSet from the feasible initialValues
InequalityFactorGraph workingSet = identifyActiveConstraints(lp_.inequalities, InequalityFactorGraph workingSet = identifyActiveConstraints(
initialValues, duals); lp_.inequalities, initialValues, duals);
LPState state(initialValues, duals, workingSet, false, 0); LPState state(initialValues, duals, workingSet, false, 0);
/// main loop of the solver /// main loop of the solver
@ -351,10 +271,10 @@ pair<VectorValues, VectorValues> optimize(const VectorValues& initialValues,
} }
return make_pair(state.values, state.duals); return make_pair(state.values, state.duals);
} }
//****************************************************************************** //******************************************************************************
/** /**
* Optimize without initial values * Optimize without initial values
* TODO: Find a feasible initial solution wrt inequality constraints * TODO: Find a feasible initial solution wrt inequality constraints
*/ */

157
gtsam_unstable/linear/QPSolver.cpp
View File

@ -19,7 +19,7 @@
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/inference/FactorGraph-inst.h> #include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam_unstable/linear/QPSolver.h> #include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <boost/range/adaptor/map.hpp> #include <boost/range/adaptor/map.hpp>
using namespace std; using namespace std;
@ -27,7 +27,8 @@ using namespace std;
namespace gtsam { namespace gtsam {
//****************************************************************************** //******************************************************************************
QPSolver::QPSolver(const QP& qp) : qp_(qp) { QPSolver::QPSolver(const QP& qp) :
qp_(qp) {
baseGraph_ = qp_.cost; baseGraph_ = qp_.cost;
baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end()); baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end());
costVariableIndex_ = VariableIndex(qp_.cost); costVariableIndex_ = VariableIndex(qp_.cost);
@ -37,7 +38,7 @@ QPSolver::QPSolver(const QP& qp) : qp_(qp) {
constrainedKeys_.merge(qp_.inequalities.keys()); constrainedKeys_.merge(qp_.inequalities.keys());
} }
//****************************************************************************** //***************************************************cc***************************
VectorValues QPSolver::solveWithCurrentWorkingSet( VectorValues QPSolver::solveWithCurrentWorkingSet(
const InequalityFactorGraph& workingSet) const { const InequalityFactorGraph& workingSet) const {
GaussianFactorGraph workingGraph = baseGraph_; GaussianFactorGraph workingGraph = baseGraph_;
@ -53,10 +54,11 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
const InequalityFactorGraph& workingSet, const VectorValues& delta) const { const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
// Transpose the A matrix of constrained factors to have the jacobian of the dual key // Transpose the A matrix of constrained factors to have the jacobian of the dual key
std::vector<std::pair<Key, Matrix> > Aterms = collectDualJacobians std::vector < std::pair<Key, Matrix> > Aterms = collectDualJacobians
< LinearEquality > (key, qp_.equalities, equalityVariableIndex_); < LinearEquality > (key, qp_.equalities, equalityVariableIndex_);
std::vector<std::pair<Key, Matrix> > AtermsInequalities = collectDualJacobians std::vector < std::pair<Key, Matrix> > AtermsInequalities =
< LinearInequality > (key, workingSet, inequalityVariableIndex_); collectDualJacobians < LinearInequality
> (key, workingSet, inequalityVariableIndex_);
Aterms.insert(Aterms.end(), AtermsInequalities.begin(), Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
AtermsInequalities.end()); AtermsInequalities.end());
@ -69,45 +71,10 @@ JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key,
b += factor->gradient(key, delta); b += factor->gradient(key, delta);
} }
} }
return boost::make_shared<JacobianFactor>(Aterms, b); // compute the least-square approximation of dual variables return boost::make_shared < JacobianFactor > (Aterms, b); // compute the least-square approximation of dual variables
} } else {
else {
return boost::make_shared<JacobianFactor>(); return boost::make_shared<JacobianFactor>();
}
} }
//******************************************************************************
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;
}
}
}
return worstFactorIx;
} }
//****************************************************************************** //******************************************************************************
@ -129,59 +96,27 @@ int QPSolver::identifyLeavingConstraint(
boost::tuple<double, int> QPSolver::computeStepSize( boost::tuple<double, int> QPSolver::computeStepSize(
const InequalityFactorGraph& workingSet, const VectorValues& xk, const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p) const { const VectorValues& p) const {
static bool debug = false; return ActiveSetSolver::computeStepSize(workingSet, xk, p, 1);
double minAlpha = 1.0;
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;
}
}
}
return boost::make_tuple(minAlpha, closestFactorIx);
} }
//****************************************************************************** //******************************************************************************
QPState QPSolver::iterate(const QPState& state) const { QPState QPSolver::iterate(const QPState& state) const {
static bool debug = false; static bool debug = false;
// Algorithm 16.3 from Nocedal06book. // Algorithm 16.3 from Nocedal06book.
// Solve with the current working set eqn 16.39, but instead of solving for p solve for x // Solve with the current working set eqn 16.39, but instead of solving for p solve for x
VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet); VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet);
if (debug) if (debug)
newValues.print("New solution:"); newValues.print("New solution:");
// If we CAN'T move further // 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 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)) { if (newValues.equals(state.values, 1e-7)) {
// Compute lambda from the dual graph // Compute lambda from the dual graph
if (debug) if (debug)
cout << "Building dual graph..." << endl; cout << "Building dual graph..." << endl;
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, newValues); GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
newValues);
if (debug) if (debug)
dualGraph->print("Dual graph: "); dualGraph->print("Dual graph: ");
VectorValues duals = dualGraph->optimize(); VectorValues duals = dualGraph->optimize();
@ -194,16 +129,15 @@ QPState QPSolver::iterate(const QPState& state) const {
// If all inequality constraints are satisfied: We have the solution!! // If all inequality constraints are satisfied: We have the solution!!
if (leavingFactor < 0) { if (leavingFactor < 0) {
return QPState(newValues, duals, state.workingSet, true, state.iterations+1); return QPState(newValues, duals, state.workingSet, true,
} state.iterations + 1);
else { } else {
// Inactivate the leaving constraint // Inactivate the leaving constraint
InequalityFactorGraph newWorkingSet = state.workingSet; InequalityFactorGraph newWorkingSet = state.workingSet;
newWorkingSet.at(leavingFactor)->inactivate(); newWorkingSet.at(leavingFactor)->inactivate();
return QPState(newValues, duals, newWorkingSet, false, state.iterations+1); return QPState(newValues, duals, newWorkingSet, false, state.iterations + 1);
} }
} } else {
else {
// If we CAN make some progress, i.e. p_k != 0 // If we CAN make some progress, i.e. p_k != 0
// Adapt stepsize if some inactive constraints complain about this move // Adapt stepsize if some inactive constraints complain about this move
double alpha; double alpha;
@ -212,8 +146,7 @@ QPState QPSolver::iterate(const QPState& state) const {
boost::tie(alpha, factorIx) = // using 16.41 boost::tie(alpha, factorIx) = // using 16.41
computeStepSize(state.workingSet, state.values, p); computeStepSize(state.workingSet, state.values, p);
if (debug) if (debug)
cout << "alpha, factorIx: " << alpha << " " << factorIx << " " cout << "alpha, factorIx: " << alpha << " " << factorIx << " " << endl;
<< endl;
// also add to the working set the one that complains the most // also add to the working set the one that complains the most
InequalityFactorGraph newWorkingSet = state.workingSet; InequalityFactorGraph newWorkingSet = state.workingSet;
@ -223,16 +156,17 @@ QPState QPSolver::iterate(const QPState& state) const {
// step! // step!
newValues = state.values + alpha * p; newValues = state.values + alpha * p;
return QPState(newValues, state.duals, newWorkingSet, false, state.iterations+1); return QPState(newValues, state.duals, newWorkingSet, false,
} state.iterations + 1);
}
} }
//****************************************************************************** //******************************************************************************
InequalityFactorGraph QPSolver::identifyActiveConstraints( InequalityFactorGraph QPSolver::identifyActiveConstraints(
const InequalityFactorGraph& inequalities, const InequalityFactorGraph& inequalities, const VectorValues& initialValues,
const VectorValues& initialValues, const VectorValues& duals, bool useWarmStart) const { const VectorValues& duals, bool useWarmStart) const {
InequalityFactorGraph workingSet; InequalityFactorGraph workingSet;
BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) { BOOST_FOREACH(const LinearInequality::shared_ptr& factor, inequalities) {
LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
if (useWarmStart == true && duals.exists(workingFactor->dualKey())) { if (useWarmStart == true && duals.exists(workingFactor->dualKey())) {
workingFactor->activate(); workingFactor->activate();
@ -256,25 +190,26 @@ InequalityFactorGraph QPSolver::identifyActiveConstraints(
} }
} }
workingSet.push_back(workingFactor); workingSet.push_back(workingFactor);
} }
return workingSet; return workingSet;
} }
//****************************************************************************** //******************************************************************************
pair<VectorValues, VectorValues> QPSolver::optimize( pair<VectorValues, VectorValues> 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 // Initialize workingSet from the feasible initialValues
InequalityFactorGraph workingSet = InequalityFactorGraph workingSet = identifyActiveConstraints(qp_.inequalities,
identifyActiveConstraints(qp_.inequalities, initialValues, duals, useWarmStart); initialValues, duals, useWarmStart);
QPState state(initialValues, duals, workingSet, false, 0); QPState state(initialValues, duals, workingSet, false, 0);
/// main loop of the solver /// main loop of the solver
while (!state.converged) { while (!state.converged) {
state = iterate(state); state = iterate(state);
} }
return make_pair(state.values, state.duals); return make_pair(state.values, state.duals);
} }
} /* namespace gtsam */ } /* namespace gtsam */

159
gtsam_unstable/linear/QPSolver.h
View File

@ -20,47 +20,21 @@
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam_unstable/linear/QP.h> #include <gtsam_unstable/linear/QP.h>
#include <gtsam_unstable/linear/ActiveSetSolver.h>
#include <gtsam_unstable/linear/QPState.h>
#include <vector> #include <vector>
#include <set> #include <set>
namespace gtsam { 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 * This QPSolver uses the active set method to solve a quadratic programming problem
* defined in the QP struct. * defined in the QP struct.
* Note: This version of QPSolver only works with a feasible initial value. * 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! 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<Key> constrainedKeys_; //!< all constrained keys, will become factors in dual graphs
public: public:
/// Constructor /// Constructor
@ -70,109 +44,11 @@ public:
VectorValues solveWithCurrentWorkingSet( VectorValues solveWithCurrentWorkingSet(
const InequalityFactorGraph& workingSet) const; const InequalityFactorGraph& workingSet) const;
/// @name Build the dual graph
/// @{
/// Collect the Jacobian terms for a dual factor
template<typename FACTOR>
std::vector<std::pair<Key, Matrix> > collectDualJacobians(Key key,
const FactorGraph<FACTOR>& graph,
const VariableIndex& variableIndex) const {
std::vector<std::pair<Key, Matrix> > 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 /// Create a dual factor
JacobianFactor::shared_ptr createDualFactor(Key key, JacobianFactor::shared_ptr createDualFactor(Key key,
const InequalityFactorGraph& workingSet, const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
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;
/// @} /// @}
/**
* 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<double, int> computeStepSize( boost::tuple<double, int> computeStepSize(
const InequalityFactorGraph& workingSet, const VectorValues& xk, const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p) const; const VectorValues& p) const;
@ -185,8 +61,8 @@ public:
*/ */
InequalityFactorGraph identifyActiveConstraints( InequalityFactorGraph identifyActiveConstraints(
const InequalityFactorGraph& inequalities, const InequalityFactorGraph& inequalities,
const VectorValues& initialValues, const VectorValues& initialValues, const VectorValues& duals =
const VectorValues& duals = VectorValues(), bool useWarmStart = true) const; VectorValues(), bool useWarmStart = true) const;
/** Optimize with a provided initial values /** Optimize with a provided initial values
* For this version, it is the responsibility of the caller to provide * For this version, it is the responsibility of the caller to provide
@ -194,28 +70,9 @@ public:
* @return a pair of <primal, dual> solutions * @return a pair of <primal, dual> solutions
*/ */
std::pair<VectorValues, VectorValues> optimize( std::pair<VectorValues, VectorValues> 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<InfeasibleInitialValues> {
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 */ } /* namespace gtsam */

29
gtsam_unstable/linear/QPState.h Normal file
View File

@ -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) {
}
};
}

4
gtsam_unstable/linear/tests/testQPSolver.cpp
View File

@ -21,6 +21,7 @@
#include <gtsam_unstable/linear/QPSolver.h> #include <gtsam_unstable/linear/QPSolver.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -115,8 +116,7 @@ TEST(QPSolver, dual) {
QPSolver solver(qp); QPSolver solver(qp);
GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph( GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph(qp.inequalities, initialValues);
qp.inequalities, initialValues);
VectorValues dual = dualGraph->optimize(); VectorValues dual = dualGraph->optimize();
VectorValues expectedDual; VectorValues expectedDual;
expectedDual.insert(0, (Vector(1) << 2.0).finished()); expectedDual.insert(0, (Vector(1) << 2.0).finished());