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