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 testQPSolver.cpp
14 * @brief Test simple QP solver for a linear inequality constraint
15 * @date Apr 10, 2014
16 * @author Duy-Nguyen Ta
17 * @author Ivan Dario Jimenez
18 */
19
20#include <gtsam/config.h>
21#if GTSAM_USE_BOOST_FEATURES
22#include <gtsam_unstable/linear/QPSParser.h>
23#endif
24
25#include <gtsam/base/Testable.h>
26#include <gtsam/inference/Symbol.h>
27#include <gtsam_unstable/linear/QPSolver.h>
28
29#include <CppUnitLite/TestHarness.h>
30
31using namespace std;
32using namespace gtsam;
33using namespace gtsam::symbol_shorthand;
34
35static const Vector kOne = Vector::Ones(newSize: 1), kZero = Vector::Zero(size: 1);
36
37/* ************************************************************************* */
38// Create test graph according to Forst10book_pg171Ex5
39QP createTestCase() {
40 QP qp;
41
42 // Objective functions x1^2 - x1*x2 + x2^2 - 3*x1 + 5
43 // Note the Hessian encodes:
44 // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 +
45 // 0.5*f
46 // Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
47 // TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of
48 // 10 because the form of the equation
49 // Should be 0.5x'Gx + gx + f : Nocedal 449
50 qp.cost.push_back(factor: HessianFactor(X(j: 1), X(j: 2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1,
51 2.0 * I_1x1, Z_1x1, 10.0));
52
53 // Inequality constraints
54 qp.inequalities.add(args: X(j: 1), args: I_1x1, args: X(j: 2), args: I_1x1, args: 2,
55 args: 0); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
56 qp.inequalities.add(args: X(j: 1), args: -I_1x1, args: 0, args: 1); // -x1 <= 0
57 qp.inequalities.add(args: X(j: 2), args: -I_1x1, args: 0, args: 2); // -x2 <= 0
58 qp.inequalities.add(args: X(j: 1), args: I_1x1, args: 1.5, args: 3); // x1 <= 3/2
59
60 return qp;
61}
62
63TEST(QPSolver, TestCase) {
64 VectorValues values;
65 double x1 = 5, x2 = 7;
66 values.insert(j: X(j: 1), value: x1 * I_1x1);
67 values.insert(j: X(j: 2), value: x2 * I_1x1);
68 QP qp = createTestCase();
69 DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9);
70 DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9);
71}
72
73TEST(QPSolver, constraintsAux) {
74 QP qp = createTestCase();
75
76 QPSolver solver(qp);
77
78 VectorValues lambdas;
79 lambdas.insert(j: 0, value: (Vector(1) << -0.5).finished());
80 lambdas.insert(j: 1, value: kZero);
81 lambdas.insert(j: 2, value: (Vector(1) << 0.3).finished());
82 lambdas.insert(j: 3, value: (Vector(1) << 0.1).finished());
83 int factorIx = solver.identifyLeavingConstraint(workingSet: qp.inequalities, lambdas);
84 LONGS_EQUAL(2, factorIx);
85
86 VectorValues lambdas2;
87 lambdas2.insert(j: 0, value: (Vector(1) << -0.5).finished());
88 lambdas2.insert(j: 1, value: kZero);
89 lambdas2.insert(j: 2, value: (Vector(1) << -0.3).finished());
90 lambdas2.insert(j: 3, value: (Vector(1) << -0.1).finished());
91 int factorIx2 = solver.identifyLeavingConstraint(workingSet: qp.inequalities, lambdas: lambdas2);
92 LONGS_EQUAL(-1, factorIx2);
93}
94
95/* ************************************************************************* */
96// Create a simple test graph with one equality constraint
97QP createEqualityConstrainedTest() {
98 QP qp;
99
100 // Objective functions x1^2 + x2^2
101 // Note the Hessian encodes:
102 // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 +
103 // 0.5*f
104 // Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
105 qp.cost.push_back(factor: HessianFactor(X(j: 1), X(j: 2), 2.0 * I_1x1, Z_1x1, Z_1x1,
106 2.0 * I_1x1, Z_1x1, 0.0));
107
108 // Equality constraints
109 // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
110 Matrix A1 = I_1x1;
111 Matrix A2 = I_1x1;
112 Vector b = -kOne;
113 qp.equalities.add(args: X(j: 1), args&: A1, args: X(j: 2), args&: A2, args&: b, args: 0);
114
115 return qp;
116}
117
118TEST(QPSolver, dual) {
119 QP qp = createEqualityConstrainedTest();
120
121 // Initials values
122 VectorValues initialValues;
123 initialValues.insert(j: X(j: 1), value: I_1x1);
124 initialValues.insert(j: X(j: 2), value: I_1x1);
125
126 QPSolver solver(qp);
127
128 auto dualGraph = solver.buildDualGraph(workingSet: qp.inequalities, delta: initialValues);
129 VectorValues dual = dualGraph.optimize();
130 VectorValues expectedDual;
131 expectedDual.insert(j: 0, value: (Vector(1) << 2.0).finished());
132 CHECK(assert_equal(expectedDual, dual, 1e-10));
133}
134
135/* ************************************************************************* */
136TEST(QPSolver, indentifyActiveConstraints) {
137 QP qp = createTestCase();
138 QPSolver solver(qp);
139
140 VectorValues currentSolution;
141 currentSolution.insert(j: X(j: 1), value: Z_1x1);
142 currentSolution.insert(j: X(j: 2), value: Z_1x1);
143
144 auto workingSet =
145 solver.identifyActiveConstraints(inequalities: qp.inequalities, initialValues: currentSolution);
146
147 CHECK(!workingSet.at(0)->active()); // inactive
148 CHECK(workingSet.at(1)->active()); // active
149 CHECK(workingSet.at(2)->active()); // active
150 CHECK(!workingSet.at(3)->active()); // inactive
151
152 VectorValues solution = solver.buildWorkingGraph(workingSet).optimize();
153 VectorValues expected;
154 expected.insert(j: X(j: 1), value: kZero);
155 expected.insert(j: X(j: 2), value: kZero);
156 CHECK(assert_equal(expected, solution, 1e-100));
157}
158
159/* ************************************************************************* */
160TEST(QPSolver, iterate) {
161 QP qp = createTestCase();
162 QPSolver solver(qp);
163
164 VectorValues currentSolution;
165 currentSolution.insert(j: X(j: 1), value: Z_1x1);
166 currentSolution.insert(j: X(j: 2), value: Z_1x1);
167
168 std::vector<VectorValues> expected(4), expectedDuals(4);
169 expected[0].insert(j: X(j: 1), value: kZero);
170 expected[0].insert(j: X(j: 2), value: kZero);
171 expectedDuals[0].insert(j: 1, value: (Vector(1) << 3).finished());
172 expectedDuals[0].insert(j: 2, value: kZero);
173
174 expected[1].insert(j: X(j: 1), value: (Vector(1) << 1.5).finished());
175 expected[1].insert(j: X(j: 2), value: kZero);
176 expectedDuals[1].insert(j: 3, value: (Vector(1) << 1.5).finished());
177
178 expected[2].insert(j: X(j: 1), value: (Vector(1) << 1.5).finished());
179 expected[2].insert(j: X(j: 2), value: (Vector(1) << 0.75).finished());
180
181 expected[3].insert(j: X(j: 1), value: (Vector(1) << 1.5).finished());
182 expected[3].insert(j: X(j: 2), value: (Vector(1) << 0.5).finished());
183
184 auto workingSet =
185 solver.identifyActiveConstraints(inequalities: qp.inequalities, initialValues: currentSolution);
186
187 QPSolver::State state(currentSolution, VectorValues(), workingSet, false,
188 100);
189
190 // int it = 0;
191 while (!state.converged) {
192 state = solver.iterate(state);
193 // These checks will fail because the expected solutions obtained from
194 // Forst10book do not follow exactly what we implemented from Nocedal06book.
195 // Specifically, we do not re-identify active constraints and
196 // do not recompute dual variables after every step!!!
197 // CHECK(assert_equal(expected[it], state.values, 1e-10));
198 // CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10));
199 // it++;
200 }
201
202 CHECK(assert_equal(expected[3], state.values, 1e-10));
203}
204
205/* ************************************************************************* */
206TEST(QPSolver, optimizeForst10book_pg171Ex5) {
207 QP qp = createTestCase();
208 QPSolver solver(qp);
209 VectorValues initialValues;
210 initialValues.insert(j: X(j: 1), value: Z_1x1);
211 initialValues.insert(j: X(j: 2), value: Z_1x1);
212 VectorValues solution = solver.optimize(initialValues).first;
213 VectorValues expected;
214 expected.insert(j: X(j: 1), value: (Vector(1) << 1.5).finished());
215 expected.insert(j: X(j: 2), value: (Vector(1) << 0.5).finished());
216 CHECK(assert_equal(expected, solution, 1e-100));
217}
218
219/* ************************************************************************* */
220#if GTSAM_USE_BOOST_FEATURES
221pair<QP, QP> testParser(QPSParser parser) {
222 QP exampleqp = parser.Parse();
223 QP expected;
224 Key X1(Symbol('X', 1)), X2(Symbol('X', 2));
225 // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
226 expected.cost.push_back(factor: HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1,
227 -1.5 * kOne, 10.0 * I_1x1, 2.0 * kOne,
228 8.0));
229
230 expected.inequalities.add(args&: X1, args: -2.0 * I_1x1, args&: X2, args: -I_1x1, args: -2,
231 args: 0); // 2x + y >= 2
232 expected.inequalities.add(args&: X1, args: -I_1x1, args&: X2, args: 2.0 * I_1x1, args: 6, args: 1); // -x + 2y <= 6
233 expected.inequalities.add(args&: X1, args: I_1x1, args: 20, args: 4); // x<= 20
234 expected.inequalities.add(args&: X1, args: -I_1x1, args: 0, args: 2); // x >= 0
235 expected.inequalities.add(args&: X2, args: -I_1x1, args: 0, args: 3); // y > = 0
236 return {expected, exampleqp};
237}
238
239TEST(QPSolver, ParserSyntaticTest) {
240 auto result = testParser(parser: QPSParser("QPExample.QPS"));
241 CHECK(assert_equal(result.first.cost, result.second.cost, 1e-7));
242 CHECK(assert_equal(result.first.inequalities, result.second.inequalities,
243 1e-7));
244 CHECK(assert_equal(result.first.equalities, result.second.equalities, 1e-7));
245}
246
247TEST(QPSolver, ParserSemanticTest) {
248 auto result = testParser(parser: QPSParser("QPExample.QPS"));
249 VectorValues expected = QPSolver(result.first).optimize().first;
250 VectorValues actual = QPSolver(result.second).optimize().first;
251 CHECK(assert_equal(actual, expected, 1e-7));
252}
253
254TEST(QPSolver, QPExampleTest) {
255 QP problem = QPSParser("QPExample.QPS").Parse();
256 auto solver = QPSolver(problem);
257 VectorValues actual = solver.optimize().first;
258 VectorValues expected;
259 expected.insert(j: Symbol('X', 1), value: 0.7625 * I_1x1);
260 expected.insert(j: Symbol('X', 2), value: 0.4750 * I_1x1);
261 double error_expected = problem.cost.error(x: expected);
262 double error_actual = problem.cost.error(x: actual);
263 CHECK(assert_equal(expected, actual, 1e-7))
264 CHECK(assert_equal(error_expected, error_actual))
265}
266
267TEST(QPSolver, HS21) {
268 QP problem = QPSParser("HS21.QPS").Parse();
269 VectorValues expected;
270 expected.insert(j: Symbol('X', 1), value: 2.0 * I_1x1);
271 expected.insert(j: Symbol('X', 2), value: 0.0 * I_1x1);
272 VectorValues actual = QPSolver(problem).optimize().first;
273 double error_actual = problem.cost.error(x: actual);
274 CHECK(assert_equal(-99.9599999, error_actual, 1e-7))
275 CHECK(assert_equal(expected, actual))
276}
277
278TEST(QPSolver, HS35) {
279 QP problem = QPSParser("HS35.QPS").Parse();
280 VectorValues actual = QPSolver(problem).optimize().first;
281 double error_actual = problem.cost.error(x: actual);
282 CHECK(assert_equal(1.11111111e-01, error_actual, 1e-7))
283}
284
285TEST(QPSolver, HS35MOD) {
286 QP problem = QPSParser("HS35MOD.QPS").Parse();
287 VectorValues actual = QPSolver(problem).optimize().first;
288 double error_actual = problem.cost.error(x: actual);
289 CHECK(assert_equal(2.50000001e-01, error_actual, 1e-7))
290}
291
292TEST(QPSolver, HS51) {
293 QP problem = QPSParser("HS51.QPS").Parse();
294 VectorValues actual = QPSolver(problem).optimize().first;
295 double error_actual = problem.cost.error(x: actual);
296 CHECK(assert_equal(8.88178420e-16, error_actual, 1e-7))
297}
298
299TEST(QPSolver, HS52) {
300 QP problem = QPSParser("HS52.QPS").Parse();
301 VectorValues actual = QPSolver(problem).optimize().first;
302 double error_actual = problem.cost.error(x: actual);
303 CHECK(assert_equal(5.32664756, error_actual, 1e-7))
304}
305
306TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of
307 // tolerance than the rest
308 QP problem = QPSParser("HS268.QPS").Parse();
309 VectorValues actual = QPSolver(problem).optimize().first;
310 double error_actual = problem.cost.error(x: actual);
311 CHECK(assert_equal(5.73107049e-07, error_actual, 1e-6))
312}
313
314TEST(QPSolver, QPTEST) { // REQUIRES Jacobian Fix
315 QP problem = QPSParser("QPTEST.QPS").Parse();
316 VectorValues actual = QPSolver(problem).optimize().first;
317 double error_actual = problem.cost.error(x: actual);
318 CHECK(assert_equal(0.437187500e01, error_actual, 1e-7))
319}
320#endif
321
322/* ************************************************************************* */
323// Create Matlab's test graph as in
324// http://www.mathworks.com/help/optim/ug/quadprog.html
325QP createTestMatlabQPEx() {
326 QP qp;
327
328 // Objective functions 0.5*x1^2 + x2^2 - x1*x2 - 2*x1 -6*x2
329 // Note the Hessian encodes:
330 // 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 +
331 // 0.5*f
332 // Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
333 qp.cost.push_back(factor: HessianFactor(X(j: 1), X(j: 2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1,
334 2.0 * I_1x1, 6 * I_1x1, 1000.0));
335
336 // Inequality constraints
337 qp.inequalities.add(args: X(j: 1), args: I_1x1, args: X(j: 2), args: I_1x1, args: 2, args: 0); // x1 + x2 <= 2
338 qp.inequalities.add(args: X(j: 1), args: -I_1x1, args: X(j: 2), args: 2 * I_1x1, args: 2, args: 1); //-x1 + 2*x2 <=2
339 qp.inequalities.add(args: X(j: 1), args: 2 * I_1x1, args: X(j: 2), args: I_1x1, args: 3, args: 2); // 2*x1 + x2 <=3
340 qp.inequalities.add(args: X(j: 1), args: -I_1x1, args: 0, args: 3); // -x1 <= 0
341 qp.inequalities.add(args: X(j: 2), args: -I_1x1, args: 0, args: 4); // -x2 <= 0
342
343 return qp;
344}
345
346///* *************************************************************************
347///*/
348TEST(QPSolver, optimizeMatlabEx) {
349 QP qp = createTestMatlabQPEx();
350 QPSolver solver(qp);
351 VectorValues initialValues;
352 initialValues.insert(j: X(j: 1), value: Z_1x1);
353 initialValues.insert(j: X(j: 2), value: Z_1x1);
354 VectorValues solution = solver.optimize(initialValues).first;
355 VectorValues expected;
356 expected.insert(j: X(j: 1), value: (Vector(1) << 2.0 / 3.0).finished());
357 expected.insert(j: X(j: 2), value: (Vector(1) << 4.0 / 3.0).finished());
358 CHECK(assert_equal(expected, solution, 1e-7));
359}
360
361///* *************************************************************************
362///*/
363TEST(QPSolver, optimizeMatlabExNoinitials) {
364 QP qp = createTestMatlabQPEx();
365 QPSolver solver(qp);
366 VectorValues solution = solver.optimize().first;
367 VectorValues expected;
368 expected.insert(j: X(j: 1), value: (Vector(1) << 2.0 / 3.0).finished());
369 expected.insert(j: X(j: 2), value: (Vector(1) << 4.0 / 3.0).finished());
370 CHECK(assert_equal(expected, solution, 1e-7));
371}
372
373/* ************************************************************************* */
374// Create test graph as in Nocedal06book, Ex 16.4, pg. 475
375QP createTestNocedal06bookEx16_4() {
376 QP qp;
377
378 qp.cost.add(key1: X(j: 1), A1: I_1x1, b: I_1x1);
379 qp.cost.add(key1: X(j: 2), A1: I_1x1, b: 2.5 * I_1x1);
380
381 // Inequality constraints
382 qp.inequalities.add(args: X(j: 1), args: -I_1x1, args: X(j: 2), args: 2 * I_1x1, args: 2, args: 0);
383 qp.inequalities.add(args: X(j: 1), args: I_1x1, args: X(j: 2), args: 2 * I_1x1, args: 6, args: 1);
384 qp.inequalities.add(args: X(j: 1), args: I_1x1, args: X(j: 2), args: -2 * I_1x1, args: 2, args: 2);
385 qp.inequalities.add(args: X(j: 1), args: -I_1x1, args: 0.0, args: 3);
386 qp.inequalities.add(args: X(j: 2), args: -I_1x1, args: 0.0, args: 4);
387
388 return qp;
389}
390
391TEST(QPSolver, optimizeNocedal06bookEx16_4) {
392 QP qp = createTestNocedal06bookEx16_4();
393 QPSolver solver(qp);
394 VectorValues initialValues;
395 initialValues.insert(j: X(j: 1), value: (Vector(1) << 2.0).finished());
396 initialValues.insert(j: X(j: 2), value: Z_1x1);
397
398 VectorValues solution = solver.optimize(initialValues).first;
399 VectorValues expected;
400 expected.insert(j: X(j: 1), value: (Vector(1) << 1.4).finished());
401 expected.insert(j: X(j: 2), value: (Vector(1) << 1.7).finished());
402 CHECK(assert_equal(expected, solution, 1e-7));
403}
404
405/* ************************************************************************* */
406TEST(QPSolver, failedSubproblem) {
407 QP qp;
408 qp.cost.add(key1: X(j: 1), A1: I_2x2, b: Z_2x1);
409 qp.cost.push_back(factor: HessianFactor(X(j: 1), Z_2x2, Z_2x1, 100.0));
410 qp.inequalities.add(args: X(j: 1), args&: (Matrix(1, 2) << -1.0, 0.0).finished(), args: -1.0, args: 0);
411
412 VectorValues expected;
413 expected.insert(j: X(j: 1), value: (Vector(2) << 1.0, 0.0).finished());
414
415 VectorValues initialValues;
416 initialValues.insert(j: X(j: 1), value: (Vector(2) << 10.0, 100.0).finished());
417
418 QPSolver solver(qp);
419 VectorValues solution = solver.optimize(initialValues).first;
420
421 CHECK(assert_equal(expected, solution, 1e-7));
422}
423
424/* ************************************************************************* */
425TEST(QPSolver, infeasibleInitial) {
426 QP qp;
427 qp.cost.add(key1: X(j: 1), A1: I_2x2, b: Vector::Zero(size: 2));
428 qp.cost.push_back(factor: HessianFactor(X(j: 1), Z_2x2, Vector::Zero(size: 2), 100.0));
429 qp.inequalities.add(args: X(j: 1), args&: (Matrix(1, 2) << -1.0, 0.0).finished(), args: -1.0, args: 0);
430
431 VectorValues expected;
432 expected.insert(j: X(j: 1), value: (Vector(2) << 1.0, 0.0).finished());
433
434 VectorValues initialValues;
435 initialValues.insert(j: X(j: 1), value: (Vector(2) << -10.0, 100.0).finished());
436
437 QPSolver solver(qp);
438 VectorValues solution;
439 CHECK_EXCEPTION(solver.optimize(initialValues), InfeasibleInitialValues);
440}
441
442/* ************************************************************************* */
443int main() {
444 TestResult tr;
445 return TestRegistry::runAllTests(result&: tr);
446}
447/* ************************************************************************* */
448