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-inl.h
14 * @brief Implmentation of ActiveSetSolver.
15 * @author Ivan Dario Jimenez
16 * @author Duy Nguyen Ta
17 * @date 2/11/16
18 */
19
20#pragma once
21
22#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
23
24/******************************************************************************/
25// Convenient macros to reduce syntactic noise. undef later.
26#define Template template <class PROBLEM, class POLICY, class INITSOLVER>
27#define This ActiveSetSolver<PROBLEM, POLICY, INITSOLVER>
28
29/******************************************************************************/
30
31namespace gtsam {
32
33/* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints
34 * If some inactive inequality constraints complain about the full step (alpha = 1),
35 * we have to adjust alpha to stay within the inequality constraints' feasible regions.
36 *
37 * For each inactive inequality j:
38 * - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints
39 * - We want: aj'*(xk + alpha*p) - bj <= 0
40 * - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0
41 * it's good!
42 * - We only care when aj'*p > 0. In this case, we need to choose alpha so that
43 * aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p)
44 * We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p)
45 *
46 * We want the minimum of all those alphas among all inactive inequality.
47 */
48Template std::tuple<double, int> This::computeStepSize(
49 const InequalityFactorGraph& workingSet, const VectorValues& xk,
50 const VectorValues& p, const double& maxAlpha) const {
51 double minAlpha = maxAlpha;
52 int closestFactorIx = -1;
53 for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
54 const LinearInequality::shared_ptr& factor = workingSet.at(i: factorIx);
55 double b = factor->getb()[0];
56 // only check inactive factors
57 if (!factor->active()) {
58 // Compute a'*p
59 double aTp = factor->dotProductRow(p);
60
61 // Check if a'*p >0. Don't care if it's not.
62 if (aTp <= 0)
63 continue;
64
65 // Compute a'*xk
66 double aTx = factor->dotProductRow(p: xk);
67
68 // alpha = (b - a'*xk) / (a'*p)
69 double alpha = (b - aTx) / aTp;
70 // We want the minimum of all those max alphas
71 if (alpha < minAlpha) {
72 closestFactorIx = factorIx;
73 minAlpha = alpha;
74 }
75 }
76 }
77 return std::make_tuple(args&: minAlpha, args&: closestFactorIx);
78}
79
80/******************************************************************************/
81/*
82 * The goal of this function is to find currently active inequality constraints
83 * that violate the condition to be active. The one that violates the condition
84 * the most will be removed from the active set. See Nocedal06book, pg 469-471
85 *
86 * Find the BAD active inequality that pulls x strongest to the wrong direction
87 * of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0)
88 *
89 * For active inequality constraints (those that are enforced as equality constraints
90 * in the current working set), we want lambda < 0.
91 * This is because:
92 * - From the Lagrangian L = f - lambda*c, we know that the constraint force
93 * is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay
94 * on the constraint surface, the constraint force has to balance out with
95 * other unconstrained forces that are pulling x towards the unconstrained
96 * minimum point. The other unconstrained forces are pulling x toward (-\grad f),
97 * hence the constraint force has to be exactly \grad f, so that the total
98 * force is 0.
99 * - We also know that at the constraint surface c(x)=0, \grad c points towards + (>= 0),
100 * while we are solving for - (<=0) constraint.
101 * - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction
102 * i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied.
103 * That means we want lambda < 0.
104 * - This is because when the constrained force pulls x towards the infeasible region (+),
105 * the unconstrained force is pulling x towards the opposite direction into
106 * the feasible region (again because the total force has to be 0 to make x stay still)
107 * So we can drop this constraint to have a lower error but feasible solution.
108 *
109 * In short, active inequality constraints with lambda > 0 are BAD, because they
110 * violate the condition to be active.
111 *
112 * And we want to remove the worst one with the largest lambda from the active set.
113 *
114 */
115Template int This::identifyLeavingConstraint(
116 const InequalityFactorGraph& workingSet,
117 const VectorValues& lambdas) const {
118 int worstFactorIx = -1;
119 // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either
120 // inactive or a good inequality constraint, so we don't care!
121 double maxLambda = 0.0;
122 for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
123 const LinearInequality::shared_ptr& factor = workingSet.at(i: factorIx);
124 if (factor->active()) {
125 double lambda = lambdas.at(j: factor->dualKey())[0];
126 if (lambda > maxLambda) {
127 worstFactorIx = factorIx;
128 maxLambda = lambda;
129 }
130 }
131 }
132 return worstFactorIx;
133}
134
135//******************************************************************************
136Template JacobianFactor::shared_ptr This::createDualFactor(
137 Key key, const InequalityFactorGraph& workingSet,
138 const VectorValues& delta) const {
139 // Transpose the A matrix of constrained factors to have the jacobian of the
140 // dual key
141 TermsContainer Aterms = collectDualJacobians<LinearEquality>(
142 key, problem_.equalities, equalityVariableIndex_);
143 TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>(
144 key, workingSet, inequalityVariableIndex_);
145 Aterms.insert(position: Aterms.end(), first: AtermsInequalities.begin(),
146 last: AtermsInequalities.end());
147
148 // Collect the gradients of unconstrained cost factors to the b vector
149 if (Aterms.size() > 0) {
150 Vector b = problem_.costGradient(key, delta);
151 // to compute the least-square approximation of dual variables
152 return std::make_shared<JacobianFactor>(args&: Aterms, args&: b);
153 } else {
154 return nullptr;
155 }
156}
157
158/******************************************************************************/
159/* This function will create a dual graph that solves for the
160 * lagrange multipliers for the current working set.
161 * You can use lagrange multipliers as a necessary condition for optimality.
162 * The factor graph that is being solved is f' = -lambda * g'
163 * where f is the optimized function and g is the function resulting from
164 * aggregating the working set.
165 * The lambdas give you information about the feasibility of a constraint.
166 * if lambda < 0 the constraint is Ok
167 * if lambda = 0 you are on the constraint
168 * if lambda > 0 you are violating the constraint.
169 */
170Template GaussianFactorGraph This::buildDualGraph(
171 const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
172 GaussianFactorGraph dualGraph;
173 for (Key key : constrainedKeys_) {
174 // Each constrained key becomes a factor in the dual graph
175 auto dualFactor = createDualFactor(key, workingSet, delta);
176 if (dualFactor) dualGraph.push_back(dualFactor);
177 }
178 return dualGraph;
179}
180
181//******************************************************************************
182Template GaussianFactorGraph
183This::buildWorkingGraph(const InequalityFactorGraph& workingSet,
184 const VectorValues& xk) const {
185 GaussianFactorGraph workingGraph;
186 workingGraph.push_back(POLICY::buildCostFunction(problem_, xk));
187 workingGraph.push_back(problem_.equalities);
188 for (const LinearInequality::shared_ptr& factor : workingSet)
189 if (factor->active()) workingGraph.push_back(factor);
190 return workingGraph;
191}
192
193//******************************************************************************
194Template typename This::State This::iterate(
195 const typename This::State& state) const {
196 // Algorithm 16.3 from Nocedal06book.
197 // Solve with the current working set eqn 16.39, but solve for x not p
198 auto workingGraph = buildWorkingGraph(workingSet: state.workingSet, xk: state.values);
199 VectorValues newValues = workingGraph.optimize();
200 // If we CAN'T move further
201 // if p_k = 0 is the original condition, modified by Duy to say that the state
202 // update is zero.
203 if (newValues.equals(x: state.values, tol: 1e-7)) {
204 // Compute lambda from the dual graph
205 auto dualGraph = buildDualGraph(workingSet: state.workingSet, delta: newValues);
206 VectorValues duals = dualGraph.optimize();
207 int leavingFactor = identifyLeavingConstraint(workingSet: state.workingSet, lambdas: duals);
208 // If all inequality constraints are satisfied: We have the solution!!
209 if (leavingFactor < 0) {
210 return State(newValues, duals, state.workingSet, true,
211 state.iterations + 1);
212 } else {
213 // Inactivate the leaving constraint
214 InequalityFactorGraph newWorkingSet = state.workingSet;
215 newWorkingSet.at(i: leavingFactor)->inactivate();
216 return State(newValues, duals, newWorkingSet, false,
217 state.iterations + 1);
218 }
219 } else {
220 // If we CAN make some progress, i.e. p_k != 0
221 // Adapt stepsize if some inactive constraints complain about this move
222 VectorValues p = newValues - state.values;
223 const auto [alpha, factorIx] = // using 16.41
224 computeStepSize(workingSet: state.workingSet, xk: state.values, p, maxAlpha: POLICY::maxAlpha);
225 // also add to the working set the one that complains the most
226 InequalityFactorGraph newWorkingSet = state.workingSet;
227 if (factorIx >= 0)
228 newWorkingSet.at(factorIx)->activate();
229 // step!
230 newValues = state.values + alpha * p;
231 return State(newValues, state.duals, newWorkingSet, false,
232 state.iterations + 1);
233 }
234}
235
236//******************************************************************************
237Template InequalityFactorGraph This::identifyActiveConstraints(
238 const InequalityFactorGraph& inequalities,
239 const VectorValues& initialValues, const VectorValues& duals,
240 bool useWarmStart) const {
241 InequalityFactorGraph workingSet;
242 for (const LinearInequality::shared_ptr& factor : inequalities) {
243 LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor));
244 if (useWarmStart && duals.size() > 0) {
245 if (duals.exists(j: workingFactor->dualKey())) workingFactor->activate();
246 else workingFactor->inactivate();
247 } else {
248 double error = workingFactor->error(c: initialValues);
249 // Safety guard. This should not happen unless users provide a bad init
250 if (error > 0) throw InfeasibleInitialValues();
251 if (std::abs(x: error) < 1e-7)
252 workingFactor->activate();
253 else
254 workingFactor->inactivate();
255 }
256 workingSet.push_back(factor: workingFactor);
257 }
258 return workingSet;
259}
260
261//******************************************************************************
262Template std::pair<VectorValues, VectorValues> This::optimize(
263 const VectorValues& initialValues, const VectorValues& duals,
264 bool useWarmStart) const {
265 // Initialize workingSet from the feasible initialValues
266 InequalityFactorGraph workingSet = identifyActiveConstraints(
267 inequalities: problem_.inequalities, initialValues, duals, useWarmStart);
268 State state(initialValues, duals, workingSet, false, 0);
269
270 /// main loop of the solver
271 while (!state.converged) state = iterate(state);
272
273 return std::make_pair(state.values, state.duals);
274}
275
276//******************************************************************************
277Template std::pair<VectorValues, VectorValues> This::optimize() const {
278 INITSOLVER initSolver(problem_);
279 VectorValues initValues = initSolver.solve();
280 return optimize(initValues);
281}
282
283}
284
285#undef Template
286#undef This
287