gtsam  4.0.0 gtsam
LP.h
Go to the documentation of this file.
1 /* ----------------------------------------------------------------------------
2
3  * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4  * Atlanta, Georgia 30332-0415
6  * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
9
10  * -------------------------------------------------------------------------- */
11
19 #pragma once
20
23
24
25 #include <string>
26
27 namespace gtsam {
28
29 using namespace std;
30
32 using KeyDimMap = std::map<Key, size_t>;
33 /*
34  * Iterates through every factor in a linear graph and generates a
35  * mapping between every factor key and it's corresponding dimensionality.
36  */
37 template <class LinearGraph>
38 KeyDimMap collectKeyDim(const LinearGraph& linearGraph) {
39  KeyDimMap keyDimMap;
40  for (const typename LinearGraph::sharedFactor& factor : linearGraph) {
41  if (!factor) continue;
42  for (Key key : factor->keys())
43  keyDimMap[key] = factor->getDim(factor->find(key));
44  }
45  return keyDimMap;
46 }
47
51 struct LP {
52  using shared_ptr = boost::shared_ptr<LP>;
53
57 private:
58  mutable KeyDimMap cachedConstrainedKeyDimMap_;
59
60 public:
62  bool isFeasible(const VectorValues& x) const {
63  return (equalities.error(x) == 0 && inequalities.error(x) == 0);
64  }
65
67  void print(const string& s = "") const {
68  std::cout << s << std::endl;
69  cost.print("Linear cost: ");
70  equalities.print("Linear equality factors: ");
71  inequalities.print("Linear inequality factors: ");
72  }
73
75  bool equals(const LP& other, double tol = 1e-9) const {
76  return cost.equals(other.cost) && equalities.equals(other.equalities)
77  && inequalities.equals(other.inequalities);
78  }
79
80  const KeyDimMap& constrainedKeyDimMap() const {
81  if (!cachedConstrainedKeyDimMap_.empty())
82  return cachedConstrainedKeyDimMap_;
83  // Collect key-dim map of all variables in the constraints
84  cachedConstrainedKeyDimMap_ = collectKeyDim(equalities);
85  KeyDimMap keysDim2 = collectKeyDim(inequalities);
86  cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end());
87  return cachedConstrainedKeyDimMap_;
88  }
89
90  Vector costGradient(Key key, const VectorValues& delta) const {
91  Vector g = Vector::Zero(delta.at(key).size());
92  Factor::const_iterator it = cost.find(key);
93  if (it != cost.end()) g = cost.getA(it).transpose();
94  return g;
95  }
96 };
97
99 template<> struct traits<LP> : public Testable<LP> {
100 };
101
102 }
void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
print out graph
Definition: FactorGraph-inst.h:36
virtual bool equals(const GaussianFactor &lf, double tol=1e-9) const
equals
Definition: LinearCost.h:91
Factor graph of all LinearEquality factors.
bool equals(const InequalityFactorGraph &other, double tol=1e-9) const
equals
Definition: InequalityFactorGraph.h:46
double error(const VectorValues &x) const
Compute error of a guess.
Definition: InequalityFactorGraph.h:53
const_iterator find(Key key) const
find
Definition: Factor.h:115
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
std::map< Key, size_t > KeyDimMap
Mapping between variable's key and its corresponding dimensionality.
Definition: LP.h:32
Collection of all Linear Inequality constraints Ax-b <= 0 of a Programming problem as a Factor Graph.
Definition: InequalityFactorGraph.h:32
InequalityFactorGraph inequalities
Linear inequality constraints: cI(x) <= 0.
Definition: LP.h:56
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
virtual void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const
print
Definition: LinearCost.h:96
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
bool equals(const LP &other, double tol=1e-9) const
equals
Definition: LP.h:75
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:73
void print(const std::string &str, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition: InequalityFactorGraph.h:40
Data structure of a Linear Program.
Definition: LP.h:51
This class defines a linear cost function c'x which is a JacobianFactor with only one row.
Definition: LinearCost.h:31
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
LinearCost derived from JacobianFactor to support linear cost functions c'x.
bool equals(const This &fg, double tol=1e-9) const
Check equality.
Definition: FactorGraph-inst.h:49
double error(const VectorValues &x) const
Compute error of a guess.
Definition: EqualityFactorGraph.h:35
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
bool isFeasible(const VectorValues &x) const
check feasibility
Definition: LP.h:62
LinearCost cost
Linear cost factor.
Definition: LP.h:54
Factor graph of all LinearInequality factors.
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:67
void print(const string &s="") const
print
Definition: LP.h:67
const_iterator end() const
Iterator at end of involved variable keys.
Definition: Factor.h:124
EqualityFactorGraph equalities
Linear equality constraints: cE(x) = 0.
Definition: LP.h:55
Collection of all Linear Equality constraints Ax=b of a Programming problem as a Factor Graph.
Definition: EqualityFactorGraph.h:30