gtsam  4.1.0
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
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 
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 
29 namespace 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  */
46 Template 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  */
113 Template 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 //******************************************************************************
134 Template 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  */
168 Template 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 //******************************************************************************
180 Template GaussianFactorGraph
181 This::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 //******************************************************************************
192 Template 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
219  // Adapt stepsize if some inactive constraints complain about this move
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 //******************************************************************************
237 Template 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 //******************************************************************************
262 Template 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 //******************************************************************************
277 Template 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
gtsam::optimize
Point3 optimize(const NonlinearFactorGraph &graph, const Values &values, Key landmarkKey)
Optimize for triangulation.
Definition: triangulation.cpp:73
InfeasibleInitialValues.h
Exception thrown when given Infeasible Initial Values.
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::LinearInequality::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: LinearInequality.h:37
gtsam::JacobianFactor::shared_ptr
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: JacobianFactor.h:96
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28