gtsam 4.1.1 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
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
9
10 * -------------------------------------------------------------------------- */
11
19#pragma once
20
23
24#include <gtsam/slam/dataset.h>
25
26namespace gtsam {
27
31struct QP {
35
36private:
37 mutable VariableIndex cachedCostVariableIndex_;
38
39public:
41 QP() :
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;
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);
73 }
74 }
75 return g;
76 }
77};
78
79} // namespace gtsam
80
Linear Factor Graph where all factors are Gaussians.
Factor graph of all LinearInequality factors.
Factor graph of all LinearEquality factors.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
print out graph
Definition: FactorGraph-inst.h:36
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:314
The VariableIndex class computes and stores the block column structure of a factor graph.
Definition: VariableIndex.h:43
size_t size() const
The number of variable entries. This is equal to the number of unique variable Keys.
Definition: VariableIndex.h:80
boost::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:69
Collection of all Linear Equality constraints Ax=b of a Programming problem as a Factor Graph.
Definition: EqualityFactorGraph.h:30
Collection of all Linear Inequality constraints Ax-b <= 0 of a Programming problem as a Factor Graph.
Definition: InequalityFactorGraph.h:32
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
print
Definition: InequalityFactorGraph.h:40
Struct contains factor graphs of a Quadratic Programming problem.
Definition: QP.h:31
InequalityFactorGraph inequalities
linear inequality constraints: cI(x) <= 0
Definition: QP.h:34
EqualityFactorGraph equalities
linear equality constraints: cE(x) = 0
Definition: QP.h:33
GaussianFactorGraph cost