gtsam  4.0.0
gtsam
QP.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 #include <gtsam/slam/dataset.h>
25 
26 namespace gtsam {
27 
31 struct QP {
35 
36 private:
37  mutable VariableIndex cachedCostVariableIndex_;
38 
39 public:
41  QP() :
42  cost(), equalities(), inequalities() {
43  }
44 
46  QP(const GaussianFactorGraph& _cost,
47  const EqualityFactorGraph& _linearEqualities,
48  const InequalityFactorGraph& _linearInequalities) :
49  cost(_cost), equalities(_linearEqualities), inequalities(
50  _linearInequalities) {
51  }
52 
54  void print(const std::string& s = "") {
55  std::cout << s << std::endl;
56  cost.print("Quadratic cost factors: ");
57  equalities.print("Linear equality factors: ");
58  inequalities.print("Linear inequality factors: ");
59  }
60 
61  const VariableIndex& costVariableIndex() const {
62  if (cachedCostVariableIndex_.size() == 0)
63  cachedCostVariableIndex_ = VariableIndex(cost);
64  return cachedCostVariableIndex_;
65  }
66 
67  Vector costGradient(Key key, const VectorValues& delta) const {
68  Vector g = Vector::Zero(delta.at(key).size());
69  if (costVariableIndex().find(key) != costVariableIndex().end()) {
70  for (size_t factorIx : costVariableIndex()[key]) {
71  GaussianFactor::shared_ptr factor = cost.at(factorIx);
72  g += factor->gradient(key, delta);
73  }
74  }
75  return g;
76  }
77 };
78 
79 } // namespace gtsam
80 
void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
print out graph
Definition: FactorGraph-inst.h:36
Factor graph of all LinearEquality factors.
utility functions for loading datasets
EqualityFactorGraph equalities
linear equality constraints: cE(x) = 0
Definition: QP.h:33
void print(const std::string &s="")
print
Definition: QP.h:54
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:57
const_iterator find(Key key) const
Find the iterator for the requested variable entry.
Definition: VariableIndex.h:152
Struct contains factor graphs of a Quadratic Programming problem.
Definition: QP.h:31
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43
Collection of all Linear Inequality constraints Ax-b <= 0 of a Programming problem as a Factor Graph.
Definition: InequalityFactorGraph.h:32
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:65
QP(const GaussianFactorGraph &_cost, const EqualityFactorGraph &_linearEqualities, const InequalityFactorGraph &_linearInequalities)
constructor
Definition: QP.h:46
size_t size() const
The number of variable entries. This is equal to the number of unique variable Keys.
Definition: VariableIndex.h:82
QP()
default constructor
Definition: QP.h:41
void print(const std::string &str, const KeyFormatter &keyFormatter=DefaultKeyFormatter) const
print
Definition: InequalityFactorGraph.h:40
const_iterator end() const
Iterator to the first variable entry.
Definition: VariableIndex.h:149
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
InequalityFactorGraph inequalities
linear inequality constraints: cI(x) <= 0
Definition: QP.h:34
const sharedFactor at(size_t i) const
Get a specific factor by index (this checks array bounds and may throw an exception,...
Definition: FactorGraph.h:283
Factor graph of all LinearInequality factors.
Linear Factor Graph where all factors are Gaussians.
GaussianFactorGraph cost
Quadratic cost factors.
Definition: QP.h:32
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
Collection of all Linear Equality constraints Ax=b of a Programming problem as a Factor Graph.
Definition: EqualityFactorGraph.h:30