gtsam  4.1.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
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 
19 #pragma once
20 
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 }
gtsam::LP::print
void print(const string &s="") const
print
Definition: LP.h:67
gtsam::LP::equalities
EqualityFactorGraph equalities
Linear equality constraints: cE(x) = 0.
Definition: LP.h:55
gtsam::LP
Data structure of a Linear Program.
Definition: LP.h:51
gtsam::traits
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:61
gtsam::InequalityFactorGraph::equals
bool equals(const InequalityFactorGraph &other, double tol=1e-9) const
equals
Definition: InequalityFactorGraph.h:46
gtsam::VectorValues
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
gtsam::EqualityFactorGraph
Collection of all Linear Equality constraints Ax=b of a Programming problem as a Factor Graph.
Definition: EqualityFactorGraph.h:30
gtsam::LinearCost
This class defines a linear cost function c'x which is a JacobianFactor with only one row.
Definition: LinearCost.h:31
gtsam::FactorGraph::equals
bool equals(const This &fg, double tol=1e-9) const
Check equality.
Definition: FactorGraph-inst.h:51
LinearCost.h
LinearCost derived from JacobianFactor to support linear cost functions c'x.
gtsam
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
gtsam::Testable
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:150
gtsam::LP::equals
bool equals(const LP &other, double tol=1e-9) const
equals
Definition: LP.h:75
gtsam::Factor::find
const_iterator find(Key key) const
find
Definition: Factor.h:115
gtsam::InequalityFactorGraph
Collection of all Linear Inequality constraints Ax-b <= 0 of a Programming problem as a Factor Graph.
Definition: InequalityFactorGraph.h:32
gtsam::KeyDimMap
std::map< Key, size_t > KeyDimMap
Mapping between variable's key and its corresponding dimensionality.
Definition: LP.h:32
gtsam::FactorGraph::print
void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
print out graph
Definition: FactorGraph-inst.h:38
gtsam::Factor::const_iterator
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:67
gtsam::LinearCost::equals
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
equals
Definition: LinearCost.h:91
gtsam::LP::cost
LinearCost cost
Linear cost factor.
Definition: LP.h:54
gtsam::Factor::end
const_iterator end() const
Iterator at end of involved variable keys.
Definition: Factor.h:124
EqualityFactorGraph.h
Factor graph of all LinearEquality factors.
gtsam::JacobianFactor::getA
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:298
gtsam::LinearCost::print
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition: LinearCost.h:96
InequalityFactorGraph.h
Factor graph of all LinearInequality factors.
gtsam::InequalityFactorGraph::error
double error(const VectorValues &x) const
Compute error of a guess.
Definition: InequalityFactorGraph.h:58
gtsam::EqualityFactorGraph::error
double error(const VectorValues &x) const
Compute error of a guess.
Definition: EqualityFactorGraph.h:40
gtsam::InequalityFactorGraph::print
void print(const std::string &str, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition: InequalityFactorGraph.h:40
gtsam::LP::inequalities
InequalityFactorGraph inequalities
Linear inequality constraints: cI(x) <= 0.
Definition: LP.h:56
gtsam::LP::isFeasible
bool isFeasible(const VectorValues &x) const
check feasibility
Definition: LP.h:62