gtsam  4.0.0
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
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)
8  * See LICENSE for the license information
10  * -------------------------------------------------------------------------- */
19 #pragma once
23 #include <boost/range/adaptor/map.hpp>
25 namespace gtsam {
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) {}
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  };
64 protected:
65  const PROBLEM& problem_;
66  VariableIndex equalityVariableIndex_,
72  typedef std::vector<std::pair<Key, Matrix> > TermsContainer;
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  }
90  std::pair<VectorValues, VectorValues> optimize(
91  const VectorValues& initialValues,
92  const VectorValues& duals = VectorValues(),
93  bool useWarmStart = false) const;
99  std::pair<VectorValues, VectorValues> optimize() const;
101 protected:
117  boost::tuple<double, int> computeStepSize(
118  const InequalityFactorGraph& workingSet, const VectorValues& xk,
119  const VectorValues& p, const double& maxAlpha) const;
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 =;
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  }
151  Key key, const InequalityFactorGraph& workingSet,
152  const VectorValues& delta) const;
154 public:
158  const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
168  const InequalityFactorGraph& workingSet,
169  const VectorValues& xk = VectorValues()) const;
172  State iterate(const State& state) const;
176  const InequalityFactorGraph& inequalities,
177  const VectorValues& initialValues,
178  const VectorValues& duals = VectorValues(),
179  bool useWarmStart = false) const;
182  int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
183  const VectorValues& lambdas) const;
185 };
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 }
202 } // namespace gtsam
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 ...
size_t iterations
Definition: ActiveSetSolver.h:46
bool converged
True if the algorithm has converged to a solution.
Definition: ActiveSetSolver.h:45
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:93
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
VectorValues values
current best values at each step
Definition: ActiveSetSolver.h:41
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:74
Key maxKey(const PROBLEM &problem)
Find the max key in a problem.
Definition: ActiveSetSolver.h:192
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
const_iterator find(Key key) const
Find the iterator for the requested variable entry.
Definition: VariableIndex.h:152
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43
Collection of all Linear Inequality constraints Ax-b <= 0 of a Programming problem as a Factor Graph.
Definition: InequalityFactorGraph.h:32
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:65
This struct contains the state information for a single iteration.
Definition: ActiveSetSolver.h:40
InequalityFactorGraph workingSet
Definition: ActiveSetSolver.h:43
int identifyLeavingConstraint(const InequalityFactorGraph &workingSet, const VectorValues &lambdas) const
Identifies active constraints that shouldn't be active anymore.
Default constructor.
Definition: ActiveSetSolver.h:50
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
This class implements the active set algorithm for solving convex Programming problems.
Definition: ActiveSetSolver.h:37
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:73
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition: BayesTree.h:32
KeySet constrainedKeys_
Definition: ActiveSetSolver.h:69
State(const VectorValues &initialValues, const VectorValues &initialDuals, const InequalityFactorGraph &initialWorkingSet, bool _converged, size_t _iterations)
Constructor with initial values.
Definition: ActiveSetSolver.h:54
const PROBLEM & problem_
the particular [convex] problem to solve
Definition: ActiveSetSolver.h:65
const_iterator end() const
Iterator to the first variable entry.
Definition: VariableIndex.h:149
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.
VariableIndex inequalityVariableIndex_
Definition: ActiveSetSolver.h:66
void merge(const FastSet &other)
insert another set: handy for MATLAB access
Definition: FastSet.h:121
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
Factor graph of all LinearInequality factors.
ActiveSetSolver(const PROBLEM &problem)
Definition: ActiveSetSolver.h:77
Linear Factor Graph where all factors are Gaussians.
GaussianFactorGraph::shared_ptr buildDualGraph(const InequalityFactorGraph &workingSet, const VectorValues &delta) const
Just for testing...
State iterate(const State &state) const
Iterate 1 step, return a new state with a new workingSet and values.
std::pair< VectorValues, VectorValues > optimize() const
For this version the caller will not have to provide an initial value.
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...
Implmentation of ActiveSetSolver.
VectorValues duals
current values of dual variables at each step
Definition: ActiveSetSolver.h:42
InequalityFactorGraph identifyActiveConstraints(const InequalityFactorGraph &inequalities, const VectorValues &initialValues, const VectorValues &duals=VectorValues(), bool useWarmStart=false) const
Identify active constraints based on initial values.