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 
24 namespace gtsam {
25 
35 template <class PROBLEM, class POLICY, class INITSOLVER>
37 public:
39  struct State {
44  bool converged;
45  size_t iterations;
48  State()
50  : values(), duals(), workingSet(), converged(false), iterations(0) {}
51 
53  State(const VectorValues& initialValues, const VectorValues& initialDuals,
54  const InequalityFactorGraph& initialWorkingSet, bool _converged,
55  size_t _iterations)
56  : values(initialValues),
57  duals(initialDuals),
58  workingSet(initialWorkingSet),
59  converged(_converged),
60  iterations(_iterations) {}
61  };
62 
63 protected:
64  const PROBLEM& problem_;
71  typedef std::vector<std::pair<Key, Matrix> > TermsContainer;
73 
74 public:
76  ActiveSetSolver(const PROBLEM& problem) : problem_(problem) {
77  equalityVariableIndex_ = VariableIndex(problem_.equalities);
78  inequalityVariableIndex_ = VariableIndex(problem_.inequalities);
79  constrainedKeys_ = problem_.equalities.keys();
80  constrainedKeys_.merge(problem_.inequalities.keys());
81  }
82 
89  std::pair<VectorValues, VectorValues> optimize(
90  const VectorValues& initialValues,
91  const VectorValues& duals = VectorValues(),
92  bool useWarmStart = false) const;
93 
98  std::pair<VectorValues, VectorValues> optimize() const;
99 
100 protected:
116  std::tuple<double, int> computeStepSize(
118  const VectorValues& p, const double& maxAlpha) const;
119 
124  template<typename FACTOR>
126  const VariableIndex& variableIndex) const {
127  /*
128  * Iterates through each factor in the factor graph and checks
129  * whether it's active. If the factor is active it reutrns the A
130  * term of the factor.
131  */
132  TermsContainer Aterms;
133  if (variableIndex.find(key) != variableIndex.end()) {
134  for (size_t factorIx : variableIndex[key]) {
135  typename FACTOR::shared_ptr factor = graph.at(factorIx);
136  if (!factor->active())
137  continue;
138  Matrix Ai = factor->getA(factor->find(key)).transpose();
139  Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
140  }
141  }
142  return Aterms;
143  }
144 
150  Key key, const InequalityFactorGraph& workingSet,
151  const VectorValues& delta) const;
152 
153 public:
154 
157  const VectorValues &delta) const;
158 
167  const InequalityFactorGraph& workingSet,
168  const VectorValues& xk = VectorValues()) const;
169 
171  State iterate(const State& state) const;
172 
175  const InequalityFactorGraph& inequalities,
176  const VectorValues& initialValues,
177  const VectorValues& duals = VectorValues(),
178  bool useWarmStart = false) const;
179 
181  int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
182  const VectorValues& lambdas) const;
183 
184 };
185 
190 template <class PROBLEM>
191 Key maxKey(const PROBLEM& problem) {
192  auto keys = problem.cost.keys();
193  Key maxKey = *std::max_element(keys.begin(), keys.end());
194  if (!problem.equalities.empty())
195  maxKey = std::max(maxKey, *problem.equalities.keys().rbegin());
196  if (!problem.inequalities.empty())
197  maxKey = std::max(maxKey, *problem.inequalities.keys().rbegin());
198  return maxKey;
199 }
200 
201 } // namespace gtsam
202 
ActiveSetSolver(const PROBLEM &problem)
Constructor.
const gtsam::Symbol key('X', 0)
bool converged
True if the algorithm has converged to a solution.
#define max(a, b)
Definition: datatypes.h:20
const_iterator find(Key key) const
Find the iterator for the requested variable entry.
VectorValues values
current best values at each step
GaussianFactorGraph buildDualGraph(const InequalityFactorGraph &workingSet, const VectorValues &delta) const
Just for testing...
InequalityFactorGraph identifyActiveConstraints(const InequalityFactorGraph &inequalities, const VectorValues &initialValues, const VectorValues &duals=VectorValues(), bool useWarmStart=false) const
Identify active constraints based on initial values.
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
std::pair< VectorValues, VectorValues > optimize() const
void merge(const FastSet &other)
Definition: FastSet.h:121
Factor graph of all LinearInequality factors.
NonlinearFactorGraph graph
Key maxKey(const PROBLEM &problem)
const_iterator end() const
Iterator to the first variable entry.
const PROBLEM & problem_
the particular [convex] problem to solve
State iterate(const State &state) const
Iterate 1 step, return a new state with a new workingSet and values.
TermsContainer collectDualJacobians(Key key, const FactorGraph< FACTOR > &graph, const VariableIndex &variableIndex) const
State()
Default constructor.
InequalityFactorGraph workingSet
GaussianFactorGraph buildWorkingGraph(const InequalityFactorGraph &workingSet, const VectorValues &xk=VectorValues()) const
std::tuple< double, int > computeStepSize(const InequalityFactorGraph &workingSet, const VectorValues &xk, const VectorValues &p, const double &maxAlpha) const
VariableIndex inequalityVariableIndex_
Linear Factor Graph where all factors are Gaussians.
int identifyLeavingConstraint(const InequalityFactorGraph &workingSet, const VectorValues &lambdas) const
Identifies active constraints that shouldn&#39;t be active anymore.
State(const VectorValues &initialValues, const VectorValues &initialDuals, const InequalityFactorGraph &initialWorkingSet, bool _converged, size_t _iterations)
Constructor with initial values.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
traits
Definition: chartTesting.h:28
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:343
This struct contains the state information for a single iteration.
float * p
JacobianFactor::shared_ptr createDualFactor(Key key, const InequalityFactorGraph &workingSet, const VectorValues &delta) const
VectorValues duals
current values of dual variables at each step
const KeyVector keys
std::vector< std::pair< Key, Matrix > > TermsContainer
Vector of key matrix pairs. Matrices are usually the A term for a factor.
VariableIndex equalityVariableIndex_
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
Implmentation of ActiveSetSolver.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:33:53