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
12/**
13 * @file ActiveSetSolver.h
14 * @brief Active set method for solving LP, QP problems
15 * @author Ivan Dario Jimenez
16 * @author Duy Nguyen Ta
17 * @date 1/25/16
18 */
19#pragma once
20
21#include <gtsam/linear/GaussianFactorGraph.h>
22#include <gtsam_unstable/linear/InequalityFactorGraph.h>
23
24namespace gtsam {
25
26/**
27 * This class implements the active set algorithm for solving convex
28 * Programming problems.
29 *
30 * @tparam PROBLEM Type of the problem to solve, e.g. LP (linear program) or
31 * QP (quadratic program).
32 * @tparam POLICY specific detail policy tailored for the particular program
33 * @tparam INITSOLVER Solver for an initial feasible solution of this problem.
34 */
35template <class PROBLEM, class POLICY, class INITSOLVER>
36class ActiveSetSolver {
37public:
38 /// This struct contains the state information for a single iteration
39 struct State {
40 VectorValues values; //!< current best values at each step
41 VectorValues duals; //!< current values of dual variables at each step
42 InequalityFactorGraph workingSet; /*!< keep track of current active/inactive
43 inequality constraints */
44 bool converged; //!< True if the algorithm has converged to a solution
45 size_t iterations; /*!< Number of iterations. Incremented at the end of
46 each iteration. */
47
48 /// Default constructor
49 State()
50 : values(), duals(), workingSet(), converged(false), iterations(0) {}
51
52 /// Constructor with initial values
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
63protected:
64 const PROBLEM& problem_; //!< the particular [convex] problem to solve
65 VariableIndex equalityVariableIndex_,
66 inequalityVariableIndex_; /*!< index to corresponding factors to build
67 dual graphs */
68 KeySet constrainedKeys_; /*!< all constrained keys, will become factors in
69 dual graphs */
70
71 /// Vector of key matrix pairs. Matrices are usually the A term for a factor.
72 typedef std::vector<std::pair<Key, Matrix> > TermsContainer;
73
74public:
75 /// Constructor
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(other: problem_.inequalities.keys());
81 }
82
83 /**
84 * Optimize with provided initial values
85 * For this version, it is the responsibility of the caller to provide
86 * a feasible initial value, otherwise, an exception will be thrown.
87 * @return a pair of <primal, dual> solutions
88 */
89 std::pair<VectorValues, VectorValues> optimize(
90 const VectorValues& initialValues,
91 const VectorValues& duals = VectorValues(),
92 bool useWarmStart = false) const;
93
94 /**
95 * For this version the caller will not have to provide an initial value
96 * @return a pair of <primal, dual> solutions
97 */
98 std::pair<VectorValues, VectorValues> optimize() const;
99
100protected:
101 /**
102 * Compute minimum step size alpha to move from the current point @p xk to the
103 * next feasible point along a direction @p p: x' = xk + alpha*p,
104 * where alpha \f$\in\f$ [0,maxAlpha].
105 *
106 * For QP, maxAlpha = 1. For LP: maxAlpha = Inf.
107 *
108 * @return a tuple of (minAlpha, closestFactorIndex) where closestFactorIndex
109 * is the closest inactive inequality constraint that blocks xk to move
110 * further and that has the minimum alpha, or (-1, maxAlpha) if there is no
111 * such inactive blocking constraint.
112 *
113 * If there is a blocking constraint, the closest one will be added to the
114 * working set and become active in the next iteration.
115 */
116 std::tuple<double, int> computeStepSize(
117 const InequalityFactorGraph& workingSet, const VectorValues& xk,
118 const VectorValues& p, const double& maxAlpha) const;
119
120 /**
121 * Finds the active constraints in the given factor graph and returns the
122 * Dual Jacobians used to build a dual factor graph.
123 */
124 template<typename FACTOR>
125 TermsContainer collectDualJacobians(Key key, const FactorGraph<FACTOR>& graph,
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
145 /**
146 * Creates a dual factor from the current workingSet and the key of the
147 * the variable used to created the dual factor.
148 */
149 JacobianFactor::shared_ptr createDualFactor(
150 Key key, const InequalityFactorGraph& workingSet,
151 const VectorValues& delta) const;
152
153public: /// Just for testing...
154
155 /// Builds a dual graph from the current working set.
156 GaussianFactorGraph buildDualGraph(const InequalityFactorGraph &workingSet,
157 const VectorValues &delta) const;
158
159 /**
160 * Build a working graph of cost, equality and active inequality constraints
161 * to solve at each iteration.
162 * @param workingSet the collection of all cost and constrained factors
163 * @param xk current solution, used to build a special quadratic cost in LP
164 * @return a new better solution
165 */
166 GaussianFactorGraph buildWorkingGraph(
167 const InequalityFactorGraph& workingSet,
168 const VectorValues& xk = VectorValues()) const;
169
170 /// Iterate 1 step, return a new state with a new workingSet and values
171 State iterate(const State& state) const;
172
173 /// Identify active constraints based on initial values.
174 InequalityFactorGraph identifyActiveConstraints(
175 const InequalityFactorGraph& inequalities,
176 const VectorValues& initialValues,
177 const VectorValues& duals = VectorValues(),
178 bool useWarmStart = false) const;
179
180 /// Identifies active constraints that shouldn't be active anymore.
181 int identifyLeavingConstraint(const InequalityFactorGraph& workingSet,
182 const VectorValues& lambdas) const;
183
184};
185
186/**
187 * Find the max key in a problem.
188 * Useful to determine unique keys for additional slack variables
189 */
190template <class PROBLEM>
191Key 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
203#include <gtsam_unstable/linear/ActiveSetSolver-inl.h>
204