gtsam  4.0.0
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
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)
8  * See LICENSE for the license information
10  * -------------------------------------------------------------------------- */
24 #include <limits>
25 #include <algorithm>
27 namespace gtsam {
30 struct LPPolicy {
33  static constexpr double maxAlpha = std::numeric_limits<double>::infinity();
48  const VectorValues& xk) {
49  GaussianFactorGraph graph;
50  for (LinearCost::const_iterator it = lp.cost.begin(); it != lp.cost.end();
51  ++it) {
52  size_t dim = lp.cost.getDim(it);
53  Vector b =*it) - lp.cost.getA(it).transpose(); // b = xk-g
54  graph.emplace_shared<JacobianFactor>(*it, Matrix::Identity(dim, dim), b);
55  }
57  KeySet allKeys = lp.inequalities.keys();
58  allKeys.merge(lp.equalities.keys());
59  allKeys.merge(KeySet(lp.cost.keys()));
60  // Add corresponding factors for all variables that are not explicitly in
61  // the cost function. Gradients of the cost function wrt to these variables
62  // are zero (g=0), so b=xk
63  if (lp.cost.keys().size() != allKeys.size()) {
64  KeySet difference;
65  std::set_difference(allKeys.begin(), allKeys.end(), lp.cost.begin(),
66  lp.cost.end(),
67  std::inserter(difference, difference.end()));
68  for (Key k : difference) {
69  size_t dim = lp.constrainedKeyDimMap().at(k);
70  graph.emplace_shared<JacobianFactor>(k, Matrix::Identity(dim, dim),;
71  }
72  }
73  return graph;
74  }
75 };
77 using LPSolver = ActiveSetSolver<LP, LPPolicy, LPInitSolver>;
79 }
Active set method for solving LP, QP problems.
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
InequalityFactorGraph inequalities
Linear inequality constraints: cI(x) <= 0.
Definition: LP.h:56
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:65
constABlock getA(const_iterator variable) const
Get a view of the A matrix for the variable pointed to by the given key iterator.
Definition: JacobianFactor.h:267
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:73
static GaussianFactorGraph buildCostFunction(const LP &lp, const VectorValues &xk)
Create the factor ||x-xk - (-g)||^2 where xk is the current feasible solution on the constraint surfa...
Definition: LPSolver.h:47
Data structure of a Linear Program.
Definition: LP.h:51
virtual DenseIndex getDim(const_iterator variable) const
Return the dimension of the variable pointed to by the given key iterator todo: Remove this in favor ...
Definition: JacobianFactor.h:245
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:87
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition: FactorGraph-inst.h:75
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:118
void merge(const FastSet &other)
insert another set: handy for MATLAB access
Definition: FastSet.h:121
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
LinearCost cost
Linear cost factor.
Definition: LP.h:54
This finds a feasible solution for an LP problem.
Policy for ActivetSetSolver to solve Linear Programming.
Definition: LPSolver.h:30
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:67
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition: Factor.h:121
static constexpr double maxAlpha
Maximum alpha for line search x'=xk + alpha*p, where p is the cost gradient For LP,...
Definition: LPSolver.h:33
Struct used to hold a Linear Programming Problem.
const_iterator end() const
Iterator at end of involved variable keys.
Definition: Factor.h:124
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition: VectorValues.h:136
EqualityFactorGraph equalities
Linear equality constraints: cE(x) = 0.
Definition: LP.h:55