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  for (const auto& [key, dim] : lp_.constrainedKeyDimMap()) {
69  initGraph->push_back(
70  JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim)));
71  }
72  return initGraph;
73 }
74 
75 /******************************************************************************/
77  double y0 = -std::numeric_limits<double>::infinity();
78  for (const auto& factor : lp_.inequalities) {
79  double error = factor->error(x0);
80  if (error > y0) y0 = error;
81  }
82  return y0;
83 }
84 
85 /******************************************************************************/
86 std::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(make_pair(*it, factor.getA(it)));
91  }
92  return terms;
93 }
94 
95 /******************************************************************************/
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); // Cx
101  terms.push_back(make_pair(yKey, Matrix::Constant(1, 1, -1.0))); // -y
102  double d = factor->getb()[0];
103  slackInequalities.push_back(LinearInequality(terms, d, factor->dualKey()));
104  }
105  return slackInequalities;
106 }
107 
108 }
gtsam::GaussianFactorGraph::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactorGraph.h:82
gtsam::InfeasibleOrUnboundedProblem
Definition: InfeasibleOrUnboundedProblem.h:23
d
static const double d[K][N]
Definition: igam.h:11
gtsam::VectorValues::insert
iterator insert(const std::pair< Key, Vector > &key_value)
Definition: VectorValues.cpp:90
gtsam::VectorValues::at
Vector & at(Key j)
Definition: VectorValues.h:142
gtsam::maxKey
Key maxKey(const PROBLEM &problem)
Definition: ActiveSetSolver.h:191
gtsam::LP::constrainedKeyDimMap
const KeyDimMap & constrainedKeyDimMap() const
Definition: LP.h:80
gtsam::LP::equalities
EqualityFactorGraph equalities
Linear equality constraints: cE(x) = 0.
Definition: LP.h:55
gtsam::JacobianFactor
Definition: JacobianFactor.h:91
gtsam::ActiveSetSolver
Definition: ActiveSetSolver.h:36
gtsam::LinearInequality
Definition: LinearInequality.h:33
InfeasibleOrUnboundedProblem.h
Throw when the problem is either infeasible or unbounded.
gtsam::LPInitSolver::collectTerms
std::vector< std::pair< Key, Matrix > > collectTerms(const LinearInequality &factor) const
Collect all terms of a factor into a container.
Definition: LPInitSolver.cpp:86
y0
double y0(double x)
Definition: j0.c:220
pruning_fixture::factor
DecisionTreeFactor factor(D &C &B &A, "0.0 0.0 0.0 0.60658897 0.61241912 0.61241969 0.61247685 0.61247742 0.0 " "0.0 0.0 0.99995287 1.0 1.0 1.0 1.0")
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::LinearCost
Definition: LinearCost.h:31
gtsam::LPInitSolver::compute_y0
double compute_y0(const VectorValues &x0) const
y = max_j ( Cj*x0 - dj ) – due to the inequality constraints y >= Cx - d
Definition: LPInitSolver.cpp:76
x0
static Symbol x0('x', 0)
gtsam::LP::inequalities
InequalityFactorGraph inequalities
Linear inequality constraints: cI(x) <= 0.
Definition: LP.h:56
gtsam::ActiveSetSolver::optimize
std::pair< VectorValues, VectorValues > optimize(const VectorValues &initialValues, const VectorValues &duals=VectorValues(), bool useWarmStart=false) const
gtsam::LPInitSolver::buildInitOfInitGraph
GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const
Definition: LPInitSolver.cpp:62
key
const gtsam::Symbol key('X', 0)
gtsam::InequalityFactorGraph
Definition: InequalityFactorGraph.h:32
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:83
gtsam
traits
Definition: SFMdata.h:40
error
static double error
Definition: testRot3.cpp:37
gtsam::FactorGraph::push_back
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:147
LPInitSolver.h
This finds a feasible solution for an LP problem.
LP
static const double LP[]
Definition: unity.c:29
LPSolver.h
Policy of ActiveSetSolver to solve Linear Programming Problems.
gtsam::LPInitSolver::addSlackVariableToInequalities
InequalityFactorGraph addSlackVariableToInequalities(Key yKey, const InequalityFactorGraph &inequalities) const
Turn Cx <= d into Cx - y <= d factors.
Definition: LPInitSolver.cpp:96
gtsam::VectorValues::erase
void erase(Key var)
Definition: VectorValues.h:229
gtsam::LPInitSolver::solve
VectorValues solve() const
Definition: LPInitSolver.cpp:27
gtsam::LPInitSolver::lp_
const LP & lp_
Definition: LPInitSolver.h:55
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::LPInitSolver::buildInitialLP
LP::shared_ptr buildInitialLP(Key yKey) const
build initial LP
Definition: LPInitSolver.cpp:51
gtsam::LP::shared_ptr
std::shared_ptr< LP > shared_ptr
Definition: LP.h:52


gtsam
Author(s):
autogenerated on Sun Dec 22 2024 04:11:57