Go to the documentation of this file.
35 template <
class PROBLEM,
class POLICY,
class INITSOLVER>
89 std::pair<VectorValues, VectorValues>
optimize(
92 bool useWarmStart =
false)
const;
98 std::pair<VectorValues, VectorValues>
optimize()
const;
124 template<
typename FACTOR>
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())
138 Matrix Ai = factor->getA(factor->find(
key)).transpose();
139 Aterms.push_back(std::make_pair(factor->dualKey(), Ai));
178 bool useWarmStart =
false)
const;
190 template <
class PROBLEM>
192 auto keys = problem.cost.keys();
194 if (!problem.equalities.empty())
196 if (!problem.inequalities.empty())
const_iterator find(Key key) const
Find the iterator for the requested variable entry.
Linear Factor Graph where all factors are Gaussians.
VectorValues duals
current values of dual variables at each step
int identifyLeavingConstraint(const InequalityFactorGraph &workingSet, const VectorValues &lambdas) const
Identifies active constraints that shouldn't be active anymore.
TermsContainer collectDualJacobians(Key key, const FactorGraph< FACTOR > &graph, const VariableIndex &variableIndex) const
Key maxKey(const PROBLEM &problem)
JacobianFactor::shared_ptr createDualFactor(Key key, const InequalityFactorGraph &workingSet, const VectorValues &delta) const
Factor graph of all LinearInequality factors.
void merge(const FastSet &other)
This struct contains the state information for a single iteration.
State(const VectorValues &initialValues, const VectorValues &initialDuals, const InequalityFactorGraph &initialWorkingSet, bool _converged, size_t _iterations)
Constructor with initial values.
std::tuple< double, int > computeStepSize(const InequalityFactorGraph &workingSet, const VectorValues &xk, const VectorValues &p, const double &maxAlpha) const
InequalityFactorGraph workingSet
VariableIndex equalityVariableIndex_
Implmentation of ActiveSetSolver.
InequalityFactorGraph identifyActiveConstraints(const InequalityFactorGraph &inequalities, const VectorValues &initialValues, const VectorValues &duals=VectorValues(), bool useWarmStart=false) const
Identify active constraints based on initial values.
std::pair< VectorValues, VectorValues > optimize() const
std::vector< std::pair< Key, Matrix > > TermsContainer
Vector of key matrix pairs. Matrices are usually the A term for a factor.
const gtsam::Symbol key('X', 0)
bool converged
True if the algorithm has converged to a solution.
const_iterator end() const
Iterator to the first variable entry.
ActiveSetSolver(const PROBLEM &problem)
Constructor.
GaussianFactorGraph buildDualGraph(const InequalityFactorGraph &workingSet, const VectorValues &delta) const
Just for testing...
VariableIndex inequalityVariableIndex_
std::shared_ptr< This > shared_ptr
shared_ptr to this class
State()
Default constructor.
VectorValues values
current best values at each step
GaussianFactorGraph buildWorkingGraph(const InequalityFactorGraph &workingSet, const VectorValues &xk=VectorValues()) const
const PROBLEM & problem_
the particular [convex] problem to solve
NonlinearFactorGraph graph
std::uint64_t Key
Integer nonlinear key type.
State iterate(const State &state) const
Iterate 1 step, return a new state with a new workingSet and values.
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:31:51