testLPSolver.cpp
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 #include <gtsam/base/Testable.h>
24 #include <gtsam/inference/Symbol.h>
30 
32 
33 using namespace std;
34 using namespace gtsam;
35 using namespace gtsam::symbol_shorthand;
36 
37 static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1);
38 
39 /* ************************************************************************* */
48  LP lp;
49  lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2)
50  lp.inequalities.add(1, Vector2(-1, 0), 0, 1); // x1 >= 0
51  lp.inequalities.add(1, Vector2(0, -1), 0, 2); // x2 >= 0
52  lp.inequalities.add(1, Vector2(1, 2), 4, 3); // x1 + 2*x2 <= 4
53  lp.inequalities.add(1, Vector2(4, 2), 12, 4); // 4x1 + 2x2 <= 12
54  lp.inequalities.add(1, Vector2(-1, 1), 1, 5); // -x1 + x2 <= 1
55  return lp;
56 }
57 
58 /* ************************************************************************* */
59 namespace gtsam {
60 
61 TEST(LPInitSolver, InfiniteLoopSingleVar) {
62  LP lp;
63  lp.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha
64  lp.inequalities.add(1, Vector3(-2, -1, -1), -2, 1); //-2x-y-a <= -2
65  lp.inequalities.add(1, Vector3(-1, 2, -1), 6, 2); // -x+2y-a <= 6
66  lp.inequalities.add(1, Vector3(-1, 0, -1), 0, 3); // -x - a <= 0
67  lp.inequalities.add(1, Vector3(1, 0, -1), 20, 4); // x - a <= 20
68  lp.inequalities.add(1, Vector3(0, -1, -1), 0, 5); // -y - a <= 0
69  LPSolver solver(lp);
70  VectorValues starter;
71  starter.insert(1, Vector3(0, 0, 2));
72  const auto [results, duals] = solver.optimize(starter);
74  expected.insert(1, Vector3(13.5, 6.5, -6.5));
75  CHECK(assert_equal(results, expected, 1e-7));
76 }
77 
78 TEST(LPInitSolver, InfiniteLoopMultiVar) {
79  LP lp;
80  Key X = symbol('X', 1);
81  Key Y = symbol('Y', 1);
82  Key Z = symbol('Z', 1);
83  lp.cost = LinearCost(Z, kOne); // min alpha
84  lp.inequalities.add(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2,
85  1); //-2x-y-alpha <= -2
86  lp.inequalities.add(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6,
87  2); // -x+2y-alpha <= 6
88  lp.inequalities.add(X, -1.0 * kOne, Z, -1.0 * kOne, 0,
89  3); // -x - alpha <= 0
90  lp.inequalities.add(X, 1.0 * kOne, Z, -1.0 * kOne, 20,
91  4); // x - alpha <= 20
92  lp.inequalities.add(Y, -1.0 * kOne, Z, -1.0 * kOne, 0,
93  5); // -y - alpha <= 0
94  LPSolver solver(lp);
95  VectorValues starter;
96  starter.insert(X, kZero);
97  starter.insert(Y, kZero);
98  starter.insert(Z, Vector::Constant(1, 2.0));
99  const auto [results, duals] = solver.optimize(starter);
101  expected.insert(X, Vector::Constant(1, 13.5));
102  expected.insert(Y, Vector::Constant(1, 6.5));
103  expected.insert(Z, Vector::Constant(1, -6.5));
104  CHECK(assert_equal(results, expected, 1e-7));
105 }
106 
107 TEST(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(1, Vector::Zero(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(1, Vector2(-1, 0), 2, Vector::Constant(1, -1),
127  0, 1); // -x1 - y <= 0
128  expectedInitLP.inequalities.add(1, Vector2(0, -1), 2, Vector::Constant(1, -1),
129  0, 2); // -x2 - y <= 0
130  expectedInitLP.inequalities.add(1, Vector2(1, 2), 2, Vector::Constant(1, -1),
131  4,
132  3); // x1 + 2*x2 - y <= 4
133  expectedInitLP.inequalities.add(1, Vector2(4, 2), 2, Vector::Constant(1, -1),
134  12,
135  4); // 4x1 + 2x2 - y <= 12
136  expectedInitLP.inequalities.add(1, Vector2(-1, 1), 2, Vector::Constant(1, -1),
137  1,
138  5); // -x1 + x2 - y <= 1
139  CHECK(assert_equal(expectedInitLP, *initLP, 1e-10));
140  LPSolver lpSolveInit(*initLP);
141  VectorValues xy0(x0);
142  xy0.insert(yKey, Vector::Constant(1, y0));
143  VectorValues xyInit = lpSolveInit.optimize(xy0).first;
144  VectorValues expected_init;
145  expected_init.insert(1, Vector::Ones(2));
146  expected_init.insert(2, Vector::Constant(1, -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 /* ************************************************************************* */
161 TEST(LPSolver, OverConstrainedLinearSystem) {
163  Matrix A1 = Vector3(1, 1, 1);
164  Matrix A2 = Vector3(1, -1, 2);
165  Vector b = Vector3(1, 5, 6);
166  graph.add(1, A1, 2, A2, b, noiseModel::Constrained::All(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 
174 TEST(LPSolver, overConstrainedLinearSystem2) {
176  graph.add(1, I_1x1, 2, I_1x1, kOne, noiseModel::Constrained::All(1));
177  graph.add(1, I_1x1, 2, -I_1x1, 5 * kOne, noiseModel::Constrained::All(1));
178  graph.add(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, noiseModel::Constrained::All(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 /* ************************************************************************* */
186 TEST(LPSolver, SimpleTest1) {
187  LP lp = simpleLP1();
188  LPSolver lpSolver(lp);
190  init.insert(1, Vector::Zero(2));
191 
192  VectorValues x1 =
194  VectorValues expected_x1;
195  expected_x1.insert(1, Vector::Ones(2));
196  CHECK(assert_equal(expected_x1, x1, 1e-10));
197 
198  const auto [result, duals] = lpSolver.optimize(init);
199  VectorValues expectedResult;
200  expectedResult.insert(1, Vector2(8. / 3., 2. / 3.));
201  CHECK(assert_equal(expectedResult, result, 1e-10));
202 }
203 
204 /* ************************************************************************* */
205 TEST(LPSolver, TestWithoutInitialValues) {
206  LP lp = simpleLP1();
207  LPSolver lpSolver(lp);
208  VectorValues expectedResult;
209  expectedResult.insert(1, Vector2(8. / 3., 2. / 3.));
210  const auto [result, duals] = lpSolver.optimize();
211  CHECK(assert_equal(expectedResult, result));
212 }
213 
220 /* ************************************************************************* */
222  LinearCost cost(1, Vector3(2., 4., 6.));
223  VectorValues x;
224  x.insert(1, Vector3(1., 3., 5.));
225  double error = cost.error(x);
226  double expectedError = 44.0;
227  DOUBLES_EQUAL(expectedError, error, 1e-100);
228 }
229 
230 /* ************************************************************************* */
231 int main() {
232  TestResult tr;
233  return TestRegistry::runAllTests(tr);
234 }
235 /* ************************************************************************* */
TEST(LPSolver, OverConstrainedLinearSystem)
static Matrix A1
#define CHECK(condition)
Definition: Test.h:108
const char Y
void add(Args &&... args)
Add a linear inequality, forwards arguments to LinearInequality.
double compute_y0(const VectorValues &x0) const
y = max_j ( Cj*x0 - dj ) – due to the inequality constraints y >= Cx - d
bool isFeasible(const VectorValues &x) const
check feasibility
Definition: LP.h:62
Scalar * b
Definition: benchVecAdd.cpp:17
Concept check for values that can be used in unit tests.
std::shared_ptr< LP > shared_ptr
Definition: LP.h:52
InequalityFactorGraph inequalities
Linear inequality constraints: cI(x) <= 0.
Definition: LP.h:56
static int runAllTests(TestResult &result)
Eigen::Vector3d Vector3
Definition: Vector.h:43
Matrix expected
Definition: testMatrix.cpp:971
#define DOUBLES_EQUAL(expected, actual, threshold)
Definition: Test.h:141
LP::shared_ptr buildInitialLP(Key yKey) const
build initial LP
LP simpleLP1()
bool assert_equal(const Matrix &expected, const Matrix &actual, double tol)
Definition: Matrix.cpp:40
Exception thrown when given Infeasible Initial Values.
Eigen::MatrixXd Matrix
Definition: base/Matrix.h:39
Definition: BFloat16.h:88
BiCGSTAB< SparseMatrix< double > > solver
iterator insert(const std::pair< Key, Vector > &key_value)
Factor graph of all LinearInequality factors.
double error(const VectorValues &x) const
NonlinearFactorGraph graph
std::pair< VectorValues, VectorValues > optimize(const VectorValues &initialValues, const VectorValues &duals=VectorValues(), bool useWarmStart=false) const
Factor graph of all LinearEquality factors.
VectorValues optimize(const Eliminate &function=EliminationTraitsType::DefaultEliminate) const
double error(const VectorValues &c) const override
Definition: LinearCost.h:113
#define Z
Definition: icosphere.cpp:21
Factor Graph Values.
Definition: LP.h:51
void add(const GaussianFactor &factor)
Eigen::VectorXd Vector
Definition: Vector.h:38
Values result
LinearCost cost
Linear cost factor.
Definition: LP.h:54
GaussianFactorGraph buildWorkingGraph(const InequalityFactorGraph &workingSet, const VectorValues &xk=VectorValues()) const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
Linear Factor Graph where all factors are Gaussians.
Key symbol(unsigned char c, std::uint64_t j)
std::shared_ptr< This > shared_ptr
shared_ptr to this class
VectorValues solve() const
traits
Definition: chartTesting.h:28
static const Vector kZero
static Symbol x0('x', 0)
GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const
Eigen::Vector2d Vector2
Definition: Vector.h:42
std::map< std::string, Array< float, 1, 8, DontAlign|RowMajor > > results
Pose3 x1
Definition: testPose3.cpp:663
detail::initimpl::constructor< Args... > init()
Binds an existing constructor taking arguments Args...
Definition: pybind11.h:1882
static const Vector kOne
int main()
static double error
Definition: testRot3.cpp:37
Policy of ActiveSetSolver to solve Linear Programming Problems.
Factor Graph Base Class.
#define X
Definition: icosphere.cpp:20
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
This finds a feasible solution for an LP problem.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:38:40