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 */
18
19#include <gtsam_unstable/linear/LPInitSolver.h>
20#include <gtsam_unstable/linear/LPSolver.h>
21
22#include <gtsam/base/Testable.h>
23#include <gtsam/inference/FactorGraph-inst.h>
24#include <gtsam/inference/Symbol.h>
25#include <gtsam/linear/GaussianFactorGraph.h>
26#include <gtsam/linear/VectorValues.h>
27#include <gtsam_unstable/linear/EqualityFactorGraph.h>
28#include <gtsam_unstable/linear/InequalityFactorGraph.h>
29#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
30
31#include <CppUnitLite/TestHarness.h>
32
33using namespace std;
34using namespace gtsam;
35using namespace gtsam::symbol_shorthand;
36
37static const Vector kOne = Vector::Ones(newSize: 1), kZero = Vector::Zero(size: 1);
38
39/* ************************************************************************* */
40/**
41 * min -x1-x2
42 * s.t. x1 + 2x2 <= 4
43 * 4x1 + 2x2 <= 12
44 * -x1 + x2 <= 1
45 * x1, x2 >= 0
46 */
47LP simpleLP1() {
48 LP lp;
49 lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2)
50 lp.inequalities.add(args: 1, args: Vector2(-1, 0), args: 0, args: 1); // x1 >= 0
51 lp.inequalities.add(args: 1, args: Vector2(0, -1), args: 0, args: 2); // x2 >= 0
52 lp.inequalities.add(args: 1, args: Vector2(1, 2), args: 4, args: 3); // x1 + 2*x2 <= 4
53 lp.inequalities.add(args: 1, args: Vector2(4, 2), args: 12, args: 4); // 4x1 + 2x2 <= 12
54 lp.inequalities.add(args: 1, args: Vector2(-1, 1), args: 1, args: 5); // -x1 + x2 <= 1
55 return lp;
56}
57
58/* ************************************************************************* */
59namespace gtsam {
60
61TEST(LPInitSolver, InfiniteLoopSingleVar) {
62 LP lp;
63 lp.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha
64 lp.inequalities.add(args: 1, args: Vector3(-2, -1, -1), args: -2, args: 1); //-2x-y-a <= -2
65 lp.inequalities.add(args: 1, args: Vector3(-1, 2, -1), args: 6, args: 2); // -x+2y-a <= 6
66 lp.inequalities.add(args: 1, args: Vector3(-1, 0, -1), args: 0, args: 3); // -x - a <= 0
67 lp.inequalities.add(args: 1, args: Vector3(1, 0, -1), args: 20, args: 4); // x - a <= 20
68 lp.inequalities.add(args: 1, args: Vector3(0, -1, -1), args: 0, args: 5); // -y - a <= 0
69 LPSolver solver(lp);
70 VectorValues starter;
71 starter.insert(j: 1, value: Vector3(0, 0, 2));
72 const auto [results, duals] = solver.optimize(initialValues: starter);
73 VectorValues expected;
74 expected.insert(j: 1, value: Vector3(13.5, 6.5, -6.5));
75 CHECK(assert_equal(results, expected, 1e-7));
76}
77
78TEST(LPInitSolver, InfiniteLoopMultiVar) {
79 LP lp;
80 Key X = symbol(c: 'X', j: 1);
81 Key Y = symbol(c: 'Y', j: 1);
82 Key Z = symbol(c: 'Z', j: 1);
83 lp.cost = LinearCost(Z, kOne); // min alpha
84 lp.inequalities.add(args&: X, args: -2.0 * kOne, args&: Y, args: -1.0 * kOne, args&: Z, args: -1.0 * kOne, args: -2,
85 args: 1); //-2x-y-alpha <= -2
86 lp.inequalities.add(args&: X, args: -1.0 * kOne, args&: Y, args: 2.0 * kOne, args&: Z, args: -1.0 * kOne, args: 6,
87 args: 2); // -x+2y-alpha <= 6
88 lp.inequalities.add(args&: X, args: -1.0 * kOne, args&: Z, args: -1.0 * kOne, args: 0,
89 args: 3); // -x - alpha <= 0
90 lp.inequalities.add(args&: X, args: 1.0 * kOne, args&: Z, args: -1.0 * kOne, args: 20,
91 args: 4); // x - alpha <= 20
92 lp.inequalities.add(args&: Y, args: -1.0 * kOne, args&: Z, args: -1.0 * kOne, args: 0,
93 args: 5); // -y - alpha <= 0
94 LPSolver solver(lp);
95 VectorValues starter;
96 starter.insert(j: X, value: kZero);
97 starter.insert(j: Y, value: kZero);
98 starter.insert(j: Z, value: Vector::Constant(size: 1, value: 2.0));
99 const auto [results, duals] = solver.optimize(initialValues: starter);
100 VectorValues expected;
101 expected.insert(j: X, value: Vector::Constant(size: 1, value: 13.5));
102 expected.insert(j: Y, value: Vector::Constant(size: 1, value: 6.5));
103 expected.insert(j: Z, value: Vector::Constant(size: 1, value: -6.5));
104 CHECK(assert_equal(results, expected, 1e-7));
105}
106
107TEST(LPInitSolver, Initialization) {
108 LP lp = simpleLP1();
109 LPInitSolver initSolver(lp);
110
111 GaussianFactorGraph::shared_ptr initOfInitGraph =
112 initSolver.buildInitOfInitGraph();
113 VectorValues x0 = initOfInitGraph->optimize();
114 VectorValues expected_x0;
115 expected_x0.insert(j: 1, value: Vector::Zero(size: 2));
116 CHECK(assert_equal(expected_x0, x0, 1e-10));
117
118 double y0 = initSolver.compute_y0(x0);
119 double expected_y0 = 0.0;
120 DOUBLES_EQUAL(expected_y0, y0, 1e-7);
121
122 Key yKey = 2;
123 LP::shared_ptr initLP = initSolver.buildInitialLP(yKey);
124 LP expectedInitLP;
125 expectedInitLP.cost = LinearCost(yKey, kOne);
126 expectedInitLP.inequalities.add(args: 1, args: Vector2(-1, 0), args: 2, args: Vector::Constant(size: 1, value: -1),
127 args: 0, args: 1); // -x1 - y <= 0
128 expectedInitLP.inequalities.add(args: 1, args: Vector2(0, -1), args: 2, args: Vector::Constant(size: 1, value: -1),
129 args: 0, args: 2); // -x2 - y <= 0
130 expectedInitLP.inequalities.add(args: 1, args: Vector2(1, 2), args: 2, args: Vector::Constant(size: 1, value: -1),
131 args: 4,
132 args: 3); // x1 + 2*x2 - y <= 4
133 expectedInitLP.inequalities.add(args: 1, args: Vector2(4, 2), args: 2, args: Vector::Constant(size: 1, value: -1),
134 args: 12,
135 args: 4); // 4x1 + 2x2 - y <= 12
136 expectedInitLP.inequalities.add(args: 1, args: Vector2(-1, 1), args: 2, args: Vector::Constant(size: 1, value: -1),
137 args: 1,
138 args: 5); // -x1 + x2 - y <= 1
139 CHECK(assert_equal(expectedInitLP, *initLP, 1e-10));
140 LPSolver lpSolveInit(*initLP);
141 VectorValues xy0(x0);
142 xy0.insert(j: yKey, value: Vector::Constant(size: 1, value: y0));
143 VectorValues xyInit = lpSolveInit.optimize(initialValues: xy0).first;
144 VectorValues expected_init;
145 expected_init.insert(j: 1, value: Vector::Ones(newSize: 2));
146 expected_init.insert(j: 2, value: Vector::Constant(size: 1, value: -1));
147 CHECK(assert_equal(expected_init, xyInit, 1e-10));
148
149 VectorValues x = initSolver.solve();
150 CHECK(lp.isFeasible(x));
151}
152} // namespace gtsam
153
154/* ************************************************************************* */
155/**
156 * TEST gtsam solver with an over-constrained system
157 * x + y = 1
158 * x - y = 5
159 * x + 2y = 6
160 */
161TEST(LPSolver, OverConstrainedLinearSystem) {
162 GaussianFactorGraph graph;
163 Matrix A1 = Vector3(1, 1, 1);
164 Matrix A2 = Vector3(1, -1, 2);
165 Vector b = Vector3(1, 5, 6);
166 graph.add(key1: 1, A1, key2: 2, A2, b, model: noiseModel::Constrained::All(dim: 3));
167
168 VectorValues x = graph.optimize();
169 // This check confirms that gtsam linear constraint solver can't handle
170 // over-constrained system
171 CHECK(graph[0]->error(x) != 0.0);
172}
173
174TEST(LPSolver, overConstrainedLinearSystem2) {
175 GaussianFactorGraph graph;
176 graph.add(key1: 1, A1: I_1x1, key2: 2, A2: I_1x1, b: kOne, model: noiseModel::Constrained::All(dim: 1));
177 graph.add(key1: 1, A1: I_1x1, key2: 2, A2: -I_1x1, b: 5 * kOne, model: noiseModel::Constrained::All(dim: 1));
178 graph.add(key1: 1, A1: I_1x1, key2: 2, A2: 2 * I_1x1, b: 6 * kOne, model: noiseModel::Constrained::All(dim: 1));
179 VectorValues x = graph.optimize();
180 // This check confirms that gtsam linear constraint solver can't handle
181 // over-constrained system
182 CHECK(graph.error(x) != 0.0);
183}
184
185/* ************************************************************************* */
186TEST(LPSolver, SimpleTest1) {
187 LP lp = simpleLP1();
188 LPSolver lpSolver(lp);
189 VectorValues init;
190 init.insert(j: 1, value: Vector::Zero(size: 2));
191
192 VectorValues x1 =
193 lpSolver.buildWorkingGraph(workingSet: InequalityFactorGraph(), xk: init).optimize();
194 VectorValues expected_x1;
195 expected_x1.insert(j: 1, value: Vector::Ones(newSize: 2));
196 CHECK(assert_equal(expected_x1, x1, 1e-10));
197
198 const auto [result, duals] = lpSolver.optimize(initialValues: init);
199 VectorValues expectedResult;
200 expectedResult.insert(j: 1, value: Vector2(8. / 3., 2. / 3.));
201 CHECK(assert_equal(expectedResult, result, 1e-10));
202}
203
204/* ************************************************************************* */
205TEST(LPSolver, TestWithoutInitialValues) {
206 LP lp = simpleLP1();
207 LPSolver lpSolver(lp);
208 VectorValues expectedResult;
209 expectedResult.insert(j: 1, value: Vector2(8. / 3., 2. / 3.));
210 const auto [result, duals] = lpSolver.optimize();
211 CHECK(assert_equal(expectedResult, result));
212}
213
214/**
215 * TODO: More TEST cases:
216 * - Infeasible
217 * - Unbounded
218 * - Underdetermined
219 */
220/* ************************************************************************* */
221TEST(LPSolver, LinearCost) {
222 LinearCost cost(1, Vector3(2., 4., 6.));
223 VectorValues x;
224 x.insert(j: 1, value: Vector3(1., 3., 5.));
225 double error = cost.error(c: x);
226 double expectedError = 44.0;
227 DOUBLES_EQUAL(expectedError, error, 1e-100);
228}
229
230/* ************************************************************************* */
231int main() {
232 TestResult tr;
233 return TestRegistry::runAllTests(result&: tr);
234}
235/* ************************************************************************* */
236