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 LPInitSolver.h
14 * @brief This finds a feasible solution for an LP problem
15 * @author Duy Nguyen Ta
16 * @author Ivan Dario Jimenez
17 * @date 6/16/16
18 */
19
20#include <gtsam_unstable/linear/LPInitSolver.h>
21#include <gtsam_unstable/linear/LPSolver.h>
22#include <gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h>
23
24namespace gtsam {
25
26/******************************************************************************/
27VectorValues LPInitSolver::solve() const {
28 // Build the graph to solve for the initial value of the initial problem
29 GaussianFactorGraph::shared_ptr initOfInitGraph = buildInitOfInitGraph();
30 VectorValues x0 = initOfInitGraph->optimize();
31 double y0 = compute_y0(x0);
32 Key yKey = maxKey(problem: lp_) + 1; // the unique key for y0
33 VectorValues xy0(x0);
34 xy0.insert(j: yKey, value: Vector::Constant(size: 1, value: y0));
35
36 // Formulate and solve the initial LP
37 LP::shared_ptr initLP = buildInitialLP(yKey);
38
39 // solve the initialLP
40 LPSolver lpSolveInit(*initLP);
41 VectorValues xyInit = lpSolveInit.optimize(initialValues: xy0).first;
42 double yOpt = xyInit.at(j: yKey)[0];
43 xyInit.erase(var: yKey);
44 if (yOpt > 0)
45 throw InfeasibleOrUnboundedProblem();
46 else
47 return xyInit;
48}
49
50/******************************************************************************/
51LP::shared_ptr LPInitSolver::buildInitialLP(Key yKey) const {
52 LP::shared_ptr initLP(new LP());
53 initLP->cost = LinearCost(yKey, I_1x1); // min y
54 initLP->equalities = lp_.equalities; // st. Ax = b
55 initLP->inequalities =
56 addSlackVariableToInequalities(yKey,
57 inequalities: lp_.inequalities); // Cx-y <= d
58 return initLP;
59}
60
61/******************************************************************************/
62GaussianFactorGraph::shared_ptr LPInitSolver::buildInitOfInitGraph() const {
63 // first add equality constraints Ax = b
64 GaussianFactorGraph::shared_ptr initGraph(
65 new GaussianFactorGraph(lp_.equalities));
66
67 // create factor ||x||^2 and add to the graph
68 for (const auto& [key, dim] : lp_.constrainedKeyDimMap()) {
69 initGraph->push_back(
70 factor: JacobianFactor(key, Matrix::Identity(rows: dim, cols: dim), Vector::Zero(size: dim)));
71 }
72 return initGraph;
73}
74
75/******************************************************************************/
76double LPInitSolver::compute_y0(const VectorValues& x0) const {
77 double y0 = -std::numeric_limits<double>::infinity();
78 for (const auto& factor : lp_.inequalities) {
79 double error = factor->error(c: x0);
80 if (error > y0) y0 = error;
81 }
82 return y0;
83}
84
85/******************************************************************************/
86std::vector<std::pair<Key, Matrix> > LPInitSolver::collectTerms(
87 const LinearInequality& factor) const {
88 std::vector<std::pair<Key, Matrix> > terms;
89 for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) {
90 terms.push_back(x: make_pair(x: *it, y: factor.getA(variable: it)));
91 }
92 return terms;
93}
94
95/******************************************************************************/
96InequalityFactorGraph LPInitSolver::addSlackVariableToInequalities(
97 Key yKey, const InequalityFactorGraph& inequalities) const {
98 InequalityFactorGraph slackInequalities;
99 for (const auto& factor : lp_.inequalities) {
100 std::vector<std::pair<Key, Matrix> > terms = collectTerms(factor: *factor); // Cx
101 terms.push_back(x: make_pair(x&: yKey, y: Matrix::Constant(rows: 1, cols: 1, value: -1.0))); // -y
102 double d = factor->getb()[0];
103 slackInequalities.push_back(factor: LinearInequality(terms, d, factor->dualKey()));
104 }
105 return slackInequalities;
106}
107
108}
109