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

gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:39:13