gtsam 4.1.1 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
27namespace gtsam {
28
29using namespace std;
30
32using 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 */
37template <class LinearGraph>
38KeyDimMap 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
51struct LP {
52 using shared_ptr = boost::shared_ptr<LP>;
53
57private:
58 mutable KeyDimMap cachedConstrainedKeyDimMap_;
59
60public:
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)
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());
93 if (it != cost.end()) g = cost.getA(it).transpose();
94 return g;
95 }
96};
97
99template<> struct traits<LP> : public Testable<LP> {
100};
101
102}
Factor graph of all LinearInequality factors.
Factor graph of all LinearEquality factors.
LinearCost derived from JacobianFactor to support linear cost functions c'x.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::map< Key, size_t > KeyDimMap
Mapping between variable's key and its corresponding dimensionality.
Definition: LP.h:32
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
A manifold defines a space in which there is a notion of a linear tangent space that can be centered ...
Definition: concepts.h:30
A helper that implements the traits interface for GTSAM types.
Definition: Testable.h:151
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
print out graph
Definition: FactorGraph-inst.h:36
bool equals(const This &fg, double tol=1e-9) const
Check equality.
Definition: FactorGraph-inst.h:49
const_iterator find(Key key) const
find
Definition: Factor.h:122
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:68
const_iterator end() const
Iterator at end of involved variable keys.
Definition: Factor.h:131
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
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
Collection of all Linear Equality constraints Ax=b of a Programming problem as a Factor Graph.
Definition: EqualityFactorGraph.h:30
double error(const VectorValues &x) const
Compute error of a guess.
Definition: EqualityFactorGraph.h:40
Collection of all Linear Inequality constraints Ax-b <= 0 of a Programming problem as a Factor Graph.
Definition: InequalityFactorGraph.h:32
double error(const VectorValues &x) const
Compute error of a guess.
Definition: InequalityFactorGraph.h:59
bool equals(const InequalityFactorGraph &other, double tol=1e-9) const
equals
Definition: InequalityFactorGraph.h:47
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: InequalityFactorGraph.h:40
This class defines a linear cost function c'x which is a JacobianFactor with only one row.
Definition: LinearCost.h:31
bool equals(const GaussianFactor &lf, double tol=1e-9) const override
equals
Definition: LinearCost.h:91
void print(const std::string &s="", const KeyFormatter &formatter=DefaultKeyFormatter) const override
print
Definition: LinearCost.h:96
Data structure of a Linear Program.
Definition: LP.h:51
bool equals(const LP &other, double tol=1e-9) const
equals
Definition: LP.h:75
void print(const string &s="") const
print
Definition: LP.h:67
bool isFeasible(const VectorValues &x) const
check feasibility
Definition: LP.h:62
InequalityFactorGraph inequalities
Linear inequality constraints: cI(x) <= 0.
Definition: LP.h:56
LinearCost cost
Linear cost factor.
Definition: LP.h:54
EqualityFactorGraph equalities
Linear equality constraints: cE(x) = 0.
Definition: LP.h:55