#include <ActiveSetSolver.h>
Classes | |
struct | State |
This struct contains the state information for a single iteration. More... | |
Public Member Functions | |
ActiveSetSolver (const PROBLEM &problem) | |
Constructor. More... | |
GaussianFactorGraph | buildDualGraph (const InequalityFactorGraph &workingSet, const VectorValues &delta) const |
Just for testing... More... | |
GaussianFactorGraph | buildWorkingGraph (const InequalityFactorGraph &workingSet, const VectorValues &xk=VectorValues()) const |
InequalityFactorGraph | identifyActiveConstraints (const InequalityFactorGraph &inequalities, const VectorValues &initialValues, const VectorValues &duals=VectorValues(), bool useWarmStart=false) const |
Identify active constraints based on initial values. More... | |
int | identifyLeavingConstraint (const InequalityFactorGraph &workingSet, const VectorValues &lambdas) const |
Identifies active constraints that shouldn't be active anymore. More... | |
State | iterate (const State &state) const |
Iterate 1 step, return a new state with a new workingSet and values. More... | |
std::pair< VectorValues, VectorValues > | optimize () const |
std::pair< VectorValues, VectorValues > | optimize (const VectorValues &initialValues, const VectorValues &duals=VectorValues(), bool useWarmStart=false) const |
Protected Types | |
typedef std::vector< std::pair< Key, Matrix > > | TermsContainer |
Vector of key matrix pairs. Matrices are usually the A term for a factor. More... | |
Protected Member Functions | |
template<typename FACTOR > | |
TermsContainer | collectDualJacobians (Key key, const FactorGraph< FACTOR > &graph, const VariableIndex &variableIndex) const |
std::tuple< double, int > | computeStepSize (const InequalityFactorGraph &workingSet, const VectorValues &xk, const VectorValues &p, const double &maxAlpha) const |
JacobianFactor::shared_ptr | createDualFactor (Key key, const InequalityFactorGraph &workingSet, const VectorValues &delta) const |
Protected Attributes | |
KeySet | constrainedKeys_ |
VariableIndex | equalityVariableIndex_ |
VariableIndex | inequalityVariableIndex_ |
const PROBLEM & | problem_ |
the particular [convex] problem to solve More... | |
This class implements the active set algorithm for solving convex Programming problems.
PROBLEM | Type of the problem to solve, e.g. LP (linear program) or QP (quadratic program). |
POLICY | specific detail policy tailored for the particular program |
INITSOLVER | Solver for an initial feasible solution of this problem. |
Definition at line 36 of file ActiveSetSolver.h.
|
protected |
Vector of key matrix pairs. Matrices are usually the A term for a factor.
Definition at line 72 of file ActiveSetSolver.h.
|
inline |
Constructor.
Definition at line 76 of file ActiveSetSolver.h.
GaussianFactorGraph gtsam::ActiveSetSolver< PROBLEM, POLICY, INITSOLVER >::buildDualGraph | ( | const InequalityFactorGraph & | workingSet, |
const VectorValues & | delta | ||
) | const |
Just for testing...
Builds a dual graph from the current working set.
GaussianFactorGraph gtsam::ActiveSetSolver< PROBLEM, POLICY, INITSOLVER >::buildWorkingGraph | ( | const InequalityFactorGraph & | workingSet, |
const VectorValues & | xk = VectorValues() |
||
) | const |
Build a working graph of cost, equality and active inequality constraints to solve at each iteration.
workingSet | the collection of all cost and constrained factors |
xk | current solution, used to build a special quadratic cost in LP |
|
inlineprotected |
Finds the active constraints in the given factor graph and returns the Dual Jacobians used to build a dual factor graph.
Definition at line 125 of file ActiveSetSolver.h.
|
protected |
Compute minimum step size alpha to move from the current point xk
to the next feasible point along a direction p:
x' = xk + alpha*p, where alpha [0,maxAlpha].
For QP, maxAlpha = 1. For LP: maxAlpha = Inf.
If there is a blocking constraint, the closest one will be added to the working set and become active in the next iteration.
|
protected |
Creates a dual factor from the current workingSet and the key of the the variable used to created the dual factor.
InequalityFactorGraph gtsam::ActiveSetSolver< PROBLEM, POLICY, INITSOLVER >::identifyActiveConstraints | ( | const InequalityFactorGraph & | inequalities, |
const VectorValues & | initialValues, | ||
const VectorValues & | duals = VectorValues() , |
||
bool | useWarmStart = false |
||
) | const |
Identify active constraints based on initial values.
int gtsam::ActiveSetSolver< PROBLEM, POLICY, INITSOLVER >::identifyLeavingConstraint | ( | const InequalityFactorGraph & | workingSet, |
const VectorValues & | lambdas | ||
) | const |
Identifies active constraints that shouldn't be active anymore.
State gtsam::ActiveSetSolver< PROBLEM, POLICY, INITSOLVER >::iterate | ( | const State & | state | ) | const |
Iterate 1 step, return a new state with a new workingSet and values.
std::pair<VectorValues, VectorValues> gtsam::ActiveSetSolver< PROBLEM, POLICY, INITSOLVER >::optimize | ( | ) | const |
For this version the caller will not have to provide an initial value
std::pair<VectorValues, VectorValues> gtsam::ActiveSetSolver< PROBLEM, POLICY, INITSOLVER >::optimize | ( | const VectorValues & | initialValues, |
const VectorValues & | duals = VectorValues() , |
||
bool | useWarmStart = false |
||
) | const |
Optimize with provided initial values For this version, it is the responsibility of the caller to provide a feasible initial value, otherwise, an exception will be thrown.
|
protected |
all constrained keys, will become factors in dual graphs
Definition at line 68 of file ActiveSetSolver.h.
|
protected |
Definition at line 65 of file ActiveSetSolver.h.
|
protected |
index to corresponding factors to build dual graphs
Definition at line 66 of file ActiveSetSolver.h.
|
protected |
the particular [convex] problem to solve
Definition at line 64 of file ActiveSetSolver.h.