gtsam  4.1.0
gtsam
ActiveSetSolver.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2 
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
5  * All Rights Reserved
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7 
8  * See LICENSE for the license information
9 
10  * -------------------------------------------------------------------------- */
11 
19 #pragma once
20 
23 #include <boost/range/adaptor/map.hpp>
24 
25 namespace gtsam {
26 
36 template <class PROBLEM, class POLICY, class INITSOLVER>
38 public:
40  struct State {
45  bool converged;
46  size_t iterations;
49  State()
51  : values(), duals(), workingSet(), converged(false), iterations(0) {}
52 
54  State(const VectorValues& initialValues, const VectorValues& initialDuals,
55  const InequalityFactorGraph& initialWorkingSet, bool _converged,
56  size_t _iterations)
57  : values(initialValues),
58  duals(initialDuals),
59  workingSet(initialWorkingSet),
60  converged(_converged),
61  iterations(_iterations) {}
62  };
63 
64 protected:
65  const PROBLEM& problem_;
66  VariableIndex equalityVariableIndex_,
72  typedef std::vector<std::pair<Key, Matrix> > TermsContainer;
74 
75 public:
77  ActiveSetSolver(const PROBLEM& problem) : problem_(problem) {
78  equalityVariableIndex_ = VariableIndex(problem_.equalities);
80  constrainedKeys_ = problem_.equalities.keys();
81  constrainedKeys_.merge(problem_.inequalities.keys());
82  }
83 
90  std::pair<VectorValues, VectorValues> optimize(
91  const VectorValues& initialValues,
92  const VectorValues& duals = VectorValues(),
93  bool useWarmStart = false) const;
94 
99  std::pair<VectorValues, VectorValues> optimize() const;
100 
101 protected:
117  boost::tuple<double, int> computeStepSize(
118  const InequalityFactorGraph& workingSet, const VectorValues& xk,
119  const VectorValues& p, const double& maxAlpha) const;
120 
125  template<typename FACTOR>
127  const VariableIndex& variableIndex) const {
128  /*
129  * Iterates through each factor in the factor graph and checks
130  * whether it's active. If the factor is active it reutrns the A
131  * term of the factor.
132  */
133  TermsContainer Aterms;
134  if (variableIndex.find(key) != variableIndex.end()) {
135  for (size_t factorIx : variableIndex[key]) {
136  typename FACTOR::shared_ptr factor = graph.at(factorIx);
137  if (!factor->active())
138  continue;
139  Matrix Ai = factor->getA(factor->find(key)).transpose();
140  Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
141  }
142  }
143  return Aterms;
144  }
145 
151  Key key, const InequalityFactorGraph& workingSet,
152  const VectorValues& delta) const;
153 
154 public:
155 
158  const VectorValues &delta) const;
159 
168  const InequalityFactorGraph& workingSet,
169  const VectorValues& xk = VectorValues()) const;
170 
172  State iterate(const State& state) const;
173 
176  const InequalityFactorGraph& inequalities,
177  const VectorValues& initialValues,
178  const VectorValues& duals = VectorValues(),
179  bool useWarmStart = false) const;
180 
183  const VectorValues& lambdas) const;
184 
185 };
186 
191 template <class PROBLEM>
192 Key maxKey(const PROBLEM& problem) {
193  auto keys = problem.cost.keys();
194  Key maxKey = *std::max_element(keys.begin(), keys.end());
195  if (!problem.equalities.empty())
196  maxKey = std::max(maxKey, *problem.equalities.keys().rbegin());
197  if (!problem.inequalities.empty())
198  maxKey = std::max(maxKey, *problem.inequalities.keys().rbegin());
199  return maxKey;
200 }
201 
202 } // namespace gtsam
203 
gtsam::GaussianFactorGraph
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:68
gtsam::ActiveSetSolver::State::duals
VectorValues duals
current values of dual variables at each step
Definition: ActiveSetSolver.h:42
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
gtsam::ActiveSetSolver::TermsContainer
std::vector< std::pair< Key, Matrix > > TermsContainer
Vector of key matrix pairs. Matrices are usually the A term for a factor.
Definition: ActiveSetSolver.h:73
gtsam::ActiveSetSolver::constrainedKeys_
KeySet constrainedKeys_
Definition: ActiveSetSolver.h:69
gtsam::ActiveSetSolver::buildDualGraph
GaussianFactorGraph buildDualGraph(const InequalityFactorGraph &workingSet, const VectorValues &delta) const
Just for testing...
gtsam::ActiveSetSolver::buildWorkingGraph
GaussianFactorGraph 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.
gtsam::ActiveSetSolver
This class implements the active set algorithm for solving convex Programming problems.
Definition: ActiveSetSolver.h:37
gtsam::ActiveSetSolver::collectDualJacobians
TermsContainer collectDualJacobians(Key key, const FactorGraph< FACTOR > &graph, const VariableIndex &variableIndex) const
Finds the active constraints in the given factor graph and returns the Dual Jacobians used to build a...
Definition: ActiveSetSolver.h:126
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::ActiveSetSolver::optimize
std::pair< VectorValues, VectorValues > 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 pro...
gtsam::ActiveSetSolver::computeStepSize
boost::tuple< double, int > computeStepSize(const InequalityFactorGraph &workingSet, const VectorValues &xk, const VectorValues &p, const double &maxAlpha) const
Compute minimum step size alpha to move from the current point xk to the next feasible point along a ...
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:96
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::ActiveSetSolver::State::converged
bool converged
True if the algorithm has converged to a solution.
Definition: ActiveSetSolver.h:45
gtsam::FastSet::merge
void merge(const FastSet &other)
insert another set: handy for MATLAB access
Definition: FastSet.h:121
gtsam::ActiveSetSolver::State::values
VectorValues values
current best values at each step
Definition: ActiveSetSolver.h:41
gtsam::ActiveSetSolver::iterate
State iterate(const State &state) const
Iterate 1 step, return a new state with a new workingSet and values.
gtsam::ActiveSetSolver::problem_
const PROBLEM & problem_
the particular [convex] problem to solve
Definition: ActiveSetSolver.h:65
gtsam::ActiveSetSolver::optimize
std::pair< VectorValues, VectorValues > optimize() const
For this version the caller will not have to provide an initial value.
gtsam::FactorGraph
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition: FactorGraph.h:94
gtsam::InequalityFactorGraph
Collection of all Linear Inequality constraints Ax-b <= 0 of a Programming problem as a Factor Graph.
Definition: InequalityFactorGraph.h:32
gtsam::ActiveSetSolver::State::State
State(const VectorValues &initialValues, const VectorValues &initialDuals, const InequalityFactorGraph &initialWorkingSet, bool _converged, size_t _iterations)
Constructor with initial values.
Definition: ActiveSetSolver.h:54
gtsam::ActiveSetSolver::identifyActiveConstraints
InequalityFactorGraph identifyActiveConstraints(const InequalityFactorGraph &inequalities, const VectorValues &initialValues, const VectorValues &duals=VectorValues(), bool useWarmStart=false) const
Identify active constraints based on initial values.
gtsam::ActiveSetSolver::State
This struct contains the state information for a single iteration.
Definition: ActiveSetSolver.h:40
gtsam::FastSet< Key >
gtsam::maxKey
Key maxKey(const PROBLEM &problem)
Find the max key in a problem.
Definition: ActiveSetSolver.h:192
gtsam::VariableIndex::end
const_iterator end() const
Iterator to the first variable entry.
Definition: VariableIndex.h:159
gtsam::ActiveSetSolver::ActiveSetSolver
ActiveSetSolver(const PROBLEM &problem)
Constructor.
Definition: ActiveSetSolver.h:77
gtsam::ActiveSetSolver::inequalityVariableIndex_
VariableIndex inequalityVariableIndex_
Definition: ActiveSetSolver.h:67
gtsam::ActiveSetSolver::identifyLeavingConstraint
int identifyLeavingConstraint(const InequalityFactorGraph &workingSet, const VectorValues &lambdas) const
Identifies active constraints that shouldn't be active anymore.
gtsam::ActiveSetSolver::State::iterations
size_t iterations
Definition: ActiveSetSolver.h:46
gtsam::ActiveSetSolver::createDualFactor
JacobianFactor::shared_ptr createDualFactor(Key key, const InequalityFactorGraph &workingSet, const VectorValues &delta) const
Creates a dual factor from the current workingSet and the key of the the variable used to created the...
gtsam::ActiveSetSolver::State::workingSet
InequalityFactorGraph workingSet
Definition: ActiveSetSolver.h:43
InequalityFactorGraph.h
Factor graph of all LinearInequality factors.
ActiveSetSolver-inl.h
Implmentation of ActiveSetSolver.
gtsam::VariableIndex
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43
gtsam::VariableIndex::find
const_iterator find(Key key) const
Find the iterator for the requested variable entry.
Definition: VariableIndex.h:162
gtsam::ActiveSetSolver::State::State
State()
Default constructor.
Definition: ActiveSetSolver.h:50