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 = std::shared_ptr<LP>;
53 
57 private:
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 
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 
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 }
EqualityFactorGraph equalities
Linear equality constraints: cE(x) = 0.
Definition: LP.h:55
const gtsam::Symbol key('X', 0)
void print(const string &s="") const
print
Definition: LP.h:67
bool equals(const This &fg, double tol=1e-9) const
Check equality up to tolerance.
bool isFeasible(const VectorValues &x) const
check feasibility
Definition: LP.h:62
std::shared_ptr< LP > shared_ptr
Definition: LP.h:52
InequalityFactorGraph inequalities
Linear inequality constraints: cI(x) <= 0.
Definition: LP.h:56
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
Definition: LinearCost.h:96
Definition: BFloat16.h:88
Factor graph of all LinearInequality factors.
bool equals(const InequalityFactorGraph &other, double tol=1e-9) const
Vector & at(Key j)
Definition: VectorValues.h:139
KeyDimMap cachedConstrainedKeyDimMap_
cached key-dim map of all variables in the constraints
Definition: LP.h:58
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
Factor graph of all LinearEquality factors.
void g(const string &key, int i)
Definition: testBTree.cpp:41
const_iterator find(Key key) const
find
Definition: Factor.h:139
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
Definition: LinearCost.h:91
const_iterator end() const
Definition: Factor.h:148
Definition: LP.h:51
Eigen::VectorXd Vector
Definition: Vector.h:38
KeyDimMap collectKeyDim(const LinearGraph &linearGraph)
Definition: LP.h:38
bool equals(const LP &other, double tol=1e-9) const
equals
Definition: LP.h:75
LinearCost cost
Linear cost factor.
Definition: LP.h:54
Vector costGradient(Key key, const VectorValues &delta) const
Definition: LP.h:90
double error(const VectorValues &x) const
Array< double, 1, 3 > e(1./3., 0.5, 2.)
RealScalar s
traits
Definition: chartTesting.h:28
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
const KeyDimMap & constrainedKeyDimMap() const
Definition: LP.h:80
const G double tol
Definition: Group.h:86
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
double error(const VectorValues &x) const
Compute error of a guess.
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
LinearCost derived from JacobianFactor to support linear cost functions c&#39;x.
constABlock getA(const_iterator variable) const


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