26 #define Template template <class PROBLEM, class POLICY, class INITSOLVER>
27 #define This ActiveSetSolver<PROBLEM, POLICY, INITSOLVER>
48 Template std::tuple<double, int> This::computeStepSize(
49 const InequalityFactorGraph& workingSet,
const VectorValues& xk,
51 double minAlpha = maxAlpha;
52 int closestFactorIx = -1;
53 for (
size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
55 double b = factor->getb()[0];
57 if (!factor->active()) {
59 double aTp = factor->dotProductRow(
p);
66 double aTx = factor->dotProductRow(xk);
69 double alpha = (
b - aTx) / aTp;
71 if (
alpha < minAlpha) {
72 closestFactorIx = factorIx;
115 Template int This::identifyLeavingConstraint(
116 const InequalityFactorGraph& workingSet,
118 int worstFactorIx = -1;
121 double maxLambda = 0.0;
122 for (
size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) {
124 if (factor->active()) {
125 double lambda = lambdas.at(factor->dualKey())[0];
127 worstFactorIx = factorIx;
132 return worstFactorIx;
137 Key key,
const InequalityFactorGraph& workingSet,
141 TermsContainer Aterms = collectDualJacobians<LinearEquality>(
142 key, problem_.equalities, equalityVariableIndex_);
143 TermsContainer AtermsInequalities = collectDualJacobians<LinearInequality>(
144 key, workingSet, inequalityVariableIndex_);
145 Aterms.insert(Aterms.end(), AtermsInequalities.begin(),
146 AtermsInequalities.end());
149 if (Aterms.size() > 0) {
152 return std::make_shared<JacobianFactor>(Aterms,
b);
170 Template GaussianFactorGraph This::buildDualGraph(
172 GaussianFactorGraph dualGraph;
173 for (
Key key : constrainedKeys_) {
175 auto dualFactor = createDualFactor(
key, workingSet,
delta);
176 if (dualFactor) dualGraph.push_back(dualFactor);
183 This::buildWorkingGraph(
const InequalityFactorGraph& workingSet,
185 GaussianFactorGraph workingGraph;
186 workingGraph.push_back(POLICY::buildCostFunction(problem_, xk));
187 workingGraph.push_back(problem_.equalities);
189 if (factor->active()) workingGraph.push_back(factor);
198 auto workingGraph = buildWorkingGraph(
state.workingSet,
state.values);
203 if (newValues.equals(
state.values, 1
e-7)) {
205 auto dualGraph = buildDualGraph(
state.workingSet, newValues);
207 int leavingFactor = identifyLeavingConstraint(
state.workingSet, duals);
209 if (leavingFactor < 0) {
210 return State(newValues, duals,
state.workingSet,
true,
211 state.iterations + 1);
214 InequalityFactorGraph newWorkingSet =
state.workingSet;
215 newWorkingSet.at(leavingFactor)->inactivate();
216 return State(newValues, duals, newWorkingSet,
false,
217 state.iterations + 1);
223 const auto [
alpha, factorIx] =
224 computeStepSize(
state.workingSet,
state.values,
p, POLICY::maxAlpha);
226 InequalityFactorGraph newWorkingSet =
state.workingSet;
228 newWorkingSet.at(factorIx)->activate();
231 return State(newValues,
state.duals, newWorkingSet,
false,
232 state.iterations + 1);
237 Template InequalityFactorGraph This::identifyActiveConstraints(
238 const InequalityFactorGraph& inequalities,
240 bool useWarmStart)
const {
241 InequalityFactorGraph workingSet;
244 if (useWarmStart && duals.size() > 0) {
245 if (duals.exists(workingFactor->dualKey())) workingFactor->activate();
246 else workingFactor->inactivate();
248 double error = workingFactor->error(initialValues);
250 if (
error > 0)
throw InfeasibleInitialValues();
252 workingFactor->activate();
254 workingFactor->inactivate();
256 workingSet.push_back(workingFactor);
264 bool useWarmStart)
const {
266 InequalityFactorGraph workingSet = identifyActiveConstraints(
267 problem_.inequalities, initialValues, duals, useWarmStart);
268 State state(initialValues, duals, workingSet,
false, 0);
273 return std::make_pair(
state.values,
state.duals);
278 INITSOLVER initSolver(problem_);