gtsam 4.1.1
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>
25namespace gtsam {
36template <class PROBLEM, class POLICY, class INITSOLVER>
40 struct State {
45 bool converged;
46 size_t iterations;
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 };
65 const PROBLEM& problem_;
66 VariableIndex equalityVariableIndex_,
73 typedef std::vector<std::pair<Key, Matrix> > TermsContainer;
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;
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;
158 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;
183 const VectorValues& lambdas) const;
191template <class PROBLEM>
192Key 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;
202} // namespace gtsam
Linear Factor Graph where all factors are Gaussians.
Factor graph of all LinearInequality factors.
Implmentation of ActiveSetSolver.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
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:69
void merge(const FastSet &other)
insert another set: handy for MATLAB access
Definition: FastSet.h:121
A factor graph is a bipartite graph with factor nodes connected to variable nodes.
Definition: FactorGraph.h:93
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43
const_iterator find(Key key) const
Find the iterator for the requested variable entry.
Definition: VariableIndex.h:162
const_iterator end() const
Iterator to the first variable entry.
Definition: VariableIndex.h:159
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:69
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:96
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
This class implements the active set algorithm for solving convex Programming problems.
Definition: ActiveSetSolver.h:37
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...
InequalityFactorGraph identifyActiveConstraints(const InequalityFactorGraph &inequalities, const VectorValues &initialValues, const VectorValues &duals=VectorValues(), bool useWarmStart=false) const
Identify active constraints based on initial values.
KeySet constrainedKeys_
Definition: ActiveSetSolver.h:69
VariableIndex inequalityVariableIndex_
Definition: ActiveSetSolver.h:67
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...
const PROBLEM & problem_
the particular [convex] problem to solve
Definition: ActiveSetSolver.h:65
int identifyLeavingConstraint(const InequalityFactorGraph &workingSet, const VectorValues &lambdas) const
Identifies active constraints that shouldn't be active anymore.
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
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.
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 ...
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
ActiveSetSolver(const PROBLEM &problem)
Definition: ActiveSetSolver.h:77
std::pair< VectorValues, VectorValues > optimize() const
For this version the caller will not have to provide an initial value.
GaussianFactorGraph 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.
This struct contains the state information for a single iteration.
Definition: ActiveSetSolver.h:40
bool converged
True if the algorithm has converged to a solution.
Definition: ActiveSetSolver.h:45
State(const VectorValues &initialValues, const VectorValues &initialDuals, const InequalityFactorGraph &initialWorkingSet, bool _converged, size_t _iterations)
Constructor with initial values.
Definition: ActiveSetSolver.h:54
size_t iterations
Definition: ActiveSetSolver.h:46
VectorValues values
current best values at each step
Definition: ActiveSetSolver.h:41
Default constructor.
Definition: ActiveSetSolver.h:50
InequalityFactorGraph workingSet
Definition: ActiveSetSolver.h:43
VectorValues duals
current values of dual variables at each step
Definition: ActiveSetSolver.h:42
Collection of all Linear Inequality constraints Ax-b <= 0 of a Programming problem as a Factor Graph.
Definition: InequalityFactorGraph.h:32