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
12/**
13 * @file LP.h
14 * @brief Struct used to hold a Linear Programming Problem
15 * @author Ivan Dario Jimenez
16 * @date 1/24/16
17 */
18
19#pragma once
20
21#include <gtsam_unstable/linear/LinearCost.h>
22#include <gtsam_unstable/linear/EqualityFactorGraph.h>
23#include <gtsam_unstable/linear/InequalityFactorGraph.h>
24
25#include <string>
26
27namespace gtsam {
28
29using namespace std;
30
31/// Mapping between variable's key and its corresponding dimensionality
32using KeyDimMap = std::map<Key, uint32_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
48/**
49 * Data structure of a Linear Program
50 */
51struct LP {
52 using shared_ptr = std::shared_ptr<LP>;
53
54 LinearCost cost; //!< Linear cost factor
55 EqualityFactorGraph equalities; //!< Linear equality constraints: cE(x) = 0
56 InequalityFactorGraph inequalities; //!< Linear inequality constraints: cI(x) <= 0
57private:
58 mutable KeyDimMap cachedConstrainedKeyDimMap_; //!< cached key-dim map of all variables in the constraints
59
60public:
61 /// check feasibility
62 bool isFeasible(const VectorValues& x) const {
63 return (equalities.error(x) == 0 && inequalities.error(x) == 0);
64 }
65
66 /// print
67 void print(const string& s = "") const {
68 std::cout << s << std::endl;
69 cost.print(s: "Linear cost: ");
70 equalities.print(s: "Linear equality factors: ");
71 inequalities.print(str: "Linear inequality factors: ");
72 }
73
74 /// equals
75 bool equals(const LP& other, double tol = 1e-9) const {
76 return cost.equals(lf: other.cost) && equalities.equals(fg: other.equalities)
77 && inequalities.equals(other: 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(linearGraph: equalities);
85 KeyDimMap keysDim2 = collectKeyDim(linearGraph: inequalities);
86 cachedConstrainedKeyDimMap_.insert(first: keysDim2.begin(), last: keysDim2.end());
87 return cachedConstrainedKeyDimMap_;
88 }
89
90 Vector costGradient(Key key, const VectorValues& delta) const {
91 Vector g = Vector::Zero(size: delta.at(j: key).size());
92 Factor::const_iterator it = cost.find(key);
93 if (it != cost.end()) g = cost.getA(variable: it).transpose();
94 return g;
95 }
96};
97
98/// traits
99template<> struct traits<LP> : public Testable<LP> {
100};
101
102}
103