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

[REFACTOR] ActiveSetSolver to match commenting and format conventions.

release/4.3a0
Ivan Jimenez 2016-02-11 23:28:08 -05:00
parent f42c4f6a92
commit 89fc822259
2 changed files with 191 additions and 131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

151
gtsam_unstable/linear/ActiveSetSolver.cpp Normal file
View File

@ -0,0 +1,151 @@
/**
* @file ActiveSetSolver.cpp
* @brief Implmentation of ActiveSetSolver.
* @author Ivan Dario Jimenez
* @author Duy Nguyen Ta
* @date 2/11/16
*/
#include <gtsam_unstable/linear/ActiveSetSolver.h>
namespace gtsam {
/*
* Iterates through each factor in the factor graph and checks
* whether it's active. If the factor is active it reutrns the A
* term of the factor.
*/
template<typename FACTOR>
ActiveSetSolver::TermsContainer ActiveSetSolver::collectDualJacobians(Key key,
const FactorGraph<FACTOR>& graph,
const VariableIndex& variableIndex) const {
ActiveSetSolver::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 ActiveSetSolver::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;
}
/* This function will create a dual graph that solves for the
* lagrange multipliers for the current working set.
* You can use lagrange multipliers as a necessary condition for optimality.
* The factor graph that is being solved is f' = -lambda * g'
* where f is the optimized function and g is the function resulting from
* aggregating the working set.
* The lambdas give you information about the feasibility of a constraint.
* if lambda < 0 the constraint is Ok
* if lambda = 0 you are on the constraint
* if lambda > 0 you are violating the constraint.
*/
GaussianFactorGraph::shared_ptr ActiveSetSolver::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;
}
/*
* 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> ActiveSetSolver::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);
}
}

171
gtsam_unstable/linear/ActiveSetSolver.h
View File

@ -8,13 +8,21 @@
#pragma once #pragma once
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
#include <boost/range/adaptor/map.hpp> #include <boost/range/adaptor/map.hpp>
namespace gtsam { namespace gtsam {
/**
* This is a base class for all implementations of the active set algorithm for solving
* Programming problems. It provides services and variables all active set implementations
* share.
*/
class ActiveSetSolver { class ActiveSetSolver {
public: public:
typedef std::vector<std::pair<Key, Matrix> > TermsContainer;
typedef std::vector<std::pair<Key, Matrix> > TermsContainer; //!< vector of key matrix pairs
//Matrices are usually the A term for a factor.
protected: protected:
KeySet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs KeySet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs
GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities.
@ -24,149 +32,50 @@ protected:
inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs
public: public:
/// Create a dual factor /**
* Creates a dual factor from the current workingSet and the key of the
* the variable used to created the dual factor.
*/
virtual JacobianFactor::shared_ptr createDualFactor(Key key, virtual JacobianFactor::shared_ptr createDualFactor(Key key,
const InequalityFactorGraph& workingSet, const InequalityFactorGraph& workingSet,
const VectorValues& delta) const = 0; const VectorValues& delta) const = 0;
/// Collect the Jacobian terms for a dual factor /**
template <typename FACTOR> * Finds the active constraints in the given factor graph and returns the
TermsContainer collectDualJacobians( * Dual Jacobians used to build a dual factor graph.
Key key, const FactorGraph<FACTOR>& graph, */
const VariableIndex& variableIndex) const { template<typename FACTOR>
TermsContainer Aterms; TermsContainer collectDualJacobians(Key key, const FactorGraph<FACTOR>& graph,
if (variableIndex.find(key) != variableIndex.end()) { const VariableIndex& variableIndex) const;
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 * Identifies active constraints that shouldn't be active anymore.
* 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, int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
const VectorValues& lambdas) const { 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;
}
/* This function will create a dual graph that solves for the /**
* lagrange multipliers for the current working set. * Builds a dual graph from the current working set.
* You can use lagrange multipliers as a necessary condition for optimality.
* The factor graph that is being solved is f' = -lambda * g'
* where f is the optimized function and g is the function resulting from
* aggregating the working set.
* The lambdas give you information about the feasibility of a constraint.
* if lambda < 0 the constraint is Ok
* if lambda = 0 you are on the constraint
* if lambda > 0 you are violating the constraint.
*/ */
GaussianFactorGraph::shared_ptr buildDualGraph( GaussianFactorGraph::shared_ptr buildDualGraph(
const InequalityFactorGraph& workingSet, const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
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;
}
protected: protected:
ActiveSetSolver() : constrainedKeys_() {}
/** /**
* Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1] * Protected constructor because this class doesn't have any meaning without
* * a concrete Programming problem to solve.
* @return a tuple of (alpha, factorIndex, sigmaIndex) where (factorIndex, sigmaIndex) */
* is the constraint that has minimum alpha, or (-1,-1) if alpha = 1. ActiveSetSolver() :
* This constraint will be added to the working set and become active constrainedKeys_() {
* 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);
} }
/**
* Computes the distance to move from the current point being examined to the next
* location to be examined by the graph. This should only be used where there are less
* than two constraints active.
*/
boost::tuple<double, int> computeStepSize(
const InequalityFactorGraph& workingSet, const VectorValues& xk,
const VectorValues& p, const double& startAlpha) const;
}; };
} // namespace gtsam } // namespace gtsam