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
*/
#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:

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?
TermsContainer collectTerms(const LinearInequality& factor) const {
TermsContainer terms;
std::vector<std::pair<Key, Matrix> > collectTerms(const LinearInequality& factor) const {
std::vector<std::pair<Key, Matrix> > 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<std::pair<Key, Matrix> > 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()));
}

216
gtsam_unstable/linear/LPSolver.h
View File

@ -6,22 +6,22 @@
*/
#pragma once
#include <gtsam_unstable/linear/LPState.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 {
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
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
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<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));
//******************************************************************************
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<JacobianFactor>();
}
}
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<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<JacobianFactor>();
boost::tuple<double, int> computeStepSize(
const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p) const {
return ActiveSetSolver::computeStepSize(workingSet, xk, p,
std::numeric_limits<double>::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<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;
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<VectorValues, VectorValues> 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<VectorValues, VectorValues> 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<VectorValues, VectorValues> optimize() const {
//
// // Initialize workingSet from the feasible initialValues

279
gtsam_unstable/linear/QPSolver.cpp
View File

@ -19,7 +19,7 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <boost/range/adaptor/map.hpp>
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<std::pair<Key, Matrix> > Aterms = collectDualJacobians
std::vector < std::pair<Key, Matrix> > Aterms = collectDualJacobians
< LinearEquality > (key, qp_.equalities, equalityVariableIndex_);
std::vector<std::pair<Key, Matrix> > AtermsInequalities = collectDualJacobians
< LinearInequality > (key, workingSet, inequalityVariableIndex_);
std::vector < std::pair<Key, Matrix> > 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<JacobianFactor>(Aterms, b); // compute the least-square approximation of dual variables
}
else {
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;
}
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<JacobianFactor>();
}
}
//******************************************************************************
@ -127,154 +94,122 @@ int QPSolver::identifyLeavingConstraint(
* We want the minimum of all those alphas among all inactive inequality.
*/
boost::tuple<double, int> 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; 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);
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<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
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 */

159
gtsam_unstable/linear/QPSolver.h
View File

@ -20,47 +20,21 @@
#include <gtsam/linear/VectorValues.h>
#include <gtsam_unstable/linear/QP.h>
#include <gtsam_unstable/linear/ActiveSetSolver.h>
#include <gtsam_unstable/linear/QPState.h>
#include <vector>
#include <set>
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<Key> 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<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
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<double, int> 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 <primal, dual> solutions
*/
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 */

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 <CppUnitLite/TestHarness.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
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());