LPInitSolver.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 
23 
24 namespace gtsam {
25 
26 /******************************************************************************/
28  // Build the graph to solve for the initial value of the initial problem
30  VectorValues x0 = initOfInitGraph->optimize();
31  double y0 = compute_y0(x0);
32  Key yKey = maxKey(lp_) + 1; // the unique key for y0
33  VectorValues xy0(x0);
34  xy0.insert(yKey, Vector::Constant(1, 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(xy0).first;
42  double yOpt = xyInit.at(yKey)[0];
43  xyInit.erase(yKey);
44  if (yOpt > 0)
46  else
47  return xyInit;
48 }
49 
50 /******************************************************************************/
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 =
57  lp_.inequalities); // Cx-y <= d
58  return initLP;
59 }
60 
61 /******************************************************************************/
63  // first add equality constraints Ax = b
66 
67  // create factor ||x||^2 and add to the graph
68  const KeyDimMap& constrainedKeyDim = lp_.constrainedKeyDimMap();
69  for (const auto& [key, _] : constrainedKeyDim) {
70  size_t dim = constrainedKeyDim.at(key);
71  initGraph->push_back(
72  JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim)));
73  }
74  return initGraph;
75 }
76 
77 /******************************************************************************/
79  double y0 = -std::numeric_limits<double>::infinity();
80  for (const auto& factor : lp_.inequalities) {
81  double error = factor->error(x0);
82  if (error > y0) y0 = error;
83  }
84  return y0;
85 }
86 
87 /******************************************************************************/
88 std::vector<std::pair<Key, Matrix> > LPInitSolver::collectTerms(
89  const LinearInequality& factor) const {
90  std::vector<std::pair<Key, Matrix> > terms;
91  for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) {
92  terms.push_back(make_pair(*it, factor.getA(it)));
93  }
94  return terms;
95 }
96 
97 /******************************************************************************/
99  Key yKey, const InequalityFactorGraph& inequalities) const {
100  InequalityFactorGraph slackInequalities;
101  for (const auto& factor : lp_.inequalities) {
102  std::vector<std::pair<Key, Matrix> > terms = collectTerms(*factor); // Cx
103  terms.push_back(make_pair(yKey, Matrix::Constant(1, 1, -1.0))); // -y
104  double d = factor->getb()[0];
105  slackInequalities.push_back(LinearInequality(terms, d, factor->dualKey()));
106  }
107  return slackInequalities;
108 }
109 
110 }
EqualityFactorGraph equalities
Linear equality constraints: cE(x) = 0.
Definition: LP.h:55
const gtsam::Symbol key('X', 0)
double compute_y0(const VectorValues &x0) const
y = max_j ( Cj*x0 - dj ) – due to the inequality constraints y >= Cx - d
std::shared_ptr< LP > shared_ptr
Definition: LP.h:52
InequalityFactorGraph inequalities
Linear inequality constraints: cI(x) <= 0.
Definition: LP.h:56
LP::shared_ptr buildInitialLP(Key yKey) const
build initial LP
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
iterator insert(const std::pair< Key, Vector > &key_value)
Vector & at(Key j)
Definition: VectorValues.h:139
Key maxKey(const PROBLEM &problem)
std::pair< VectorValues, VectorValues > optimize(const VectorValues &initialValues, const VectorValues &duals=VectorValues(), bool useWarmStart=false) const
const_iterator end() const
Definition: Factor.h:148
Definition: LP.h:51
InequalityFactorGraph addSlackVariableToInequalities(Key yKey, const InequalityFactorGraph &inequalities) const
Turn Cx <= d into Cx - y <= d factors.
std::shared_ptr< This > shared_ptr
shared_ptr to this class
VectorValues solve() const
Throw when the problem is either infeasible or unbounded.
traits
Definition: chartTesting.h:28
static Symbol x0('x', 0)
GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const
constexpr descr< N - 1 > _(char const (&text)[N])
Definition: descr.h:109
void erase(Key var)
Definition: VectorValues.h:226
const KeyDimMap & constrainedKeyDimMap() const
Definition: LP.h:80
static double error
Definition: testRot3.cpp:37
Policy of ActiveSetSolver to solve Linear Programming Problems.
std::map< Key, size_t > KeyDimMap
Mapping between variable&#39;s key and its corresponding dimensionality.
Definition: LP.h:32
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:82
const_iterator begin() const
Definition: Factor.h:145
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
constABlock getA(const_iterator variable) const
std::vector< std::pair< Key, Matrix > > collectTerms(const LinearInequality &factor) const
Collect all terms of a factor into a container.
This finds a feasible solution for an LP problem.


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:35