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 LPInitSolver implements the strategy in Matlab.
15 * @author Duy Nguyen Ta
16 * @author Ivan Dario Jimenez
17 * @date 1/24/16
18 */
19
20#pragma once
21
22#include <gtsam_unstable/dllexport.h>
23#include <gtsam_unstable/linear/LP.h>
24#include <gtsam/linear/GaussianFactorGraph.h>
25
26namespace gtsam {
27/**
28 * This LPInitSolver implements the strategy in Matlab:
29 * http://www.mathworks.com/help/optim/ug/linear-programming-algorithms.html#brozyzb-9
30 * Solve for x and y:
31 * min y
32 * st Ax = b
33 * Cx - y <= d
34 * where \f$y \in R\f$, \f$x \in R^n\f$, and Ax = b and Cx <= d is the constraints of the original problem.
35 *
36 * If the solution for this problem {x*,y*} has y* <= 0, we'll have x* a feasible initial point
37 * of the original problem
38 * otherwise, if y* > 0 or the problem has no solution, the original problem is infeasible.
39 *
40 * The initial value of this initial problem can be found by solving
41 * min ||x||^2
42 * s.t. Ax = b
43 * to have a solution x0
44 * then y = max_j ( Cj*x0 - dj ) -- due to the constraints y >= Cx - d
45 *
46 * WARNING: If some xj in the inequality constraints does not exist in the equality constraints,
47 * set them as zero for now. If that is the case, the original problem doesn't have a unique
48 * solution (it could be either infeasible or unbounded).
49 * So, if the initialization fails because we enforce xj=0 in the problematic
50 * inequality constraint, we can't conclude that the problem is infeasible.
51 * However, whether it is infeasible or unbounded, we don't have a unique solution anyway.
52 */
53class GTSAM_UNSTABLE_EXPORT LPInitSolver {
54private:
55 const LP& lp_;
56
57public:
58 /// Construct with an LP problem
59 LPInitSolver(const LP& lp) : lp_(lp) {}
60
61 ///@return a feasible initialization point
62 VectorValues solve() const;
63
64private:
65 /// build initial LP
66 LP::shared_ptr buildInitialLP(Key yKey) const;
67
68 /**
69 * Build the following graph to solve for an initial value of the initial problem
70 * min ||x||^2 s.t. Ax = b
71 */
72 GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const;
73
74 /// y = max_j ( Cj*x0 - dj ) -- due to the inequality constraints y >= Cx - d
75 double compute_y0(const VectorValues& x0) const;
76
77 /// Collect all terms of a factor into a container.
78 std::vector<std::pair<Key, Matrix>> collectTerms(
79 const LinearInequality& factor) const;
80
81 /// Turn Cx <= d into Cx - y <= d factors
82 InequalityFactorGraph addSlackVariableToInequalities(Key yKey,
83 const InequalityFactorGraph& inequalities) const;
84
85 // friend class for unit-testing private methods
86 friend class LPInitSolverInitializationTest;
87};
88
89}
90