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:
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 
62  if (cachedCostVariableIndex_.size() == 0)
65  }
66 
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 
GaussianFactorGraph.h
Linear Factor Graph where all factors are Gaussians.
s
RealScalar s
Definition: level1_cplx_impl.h:126
EqualityFactorGraph.h
Factor graph of all LinearEquality factors.
gtsam::QP::QP
QP(const GaussianFactorGraph &_cost, const EqualityFactorGraph &_linearEqualities, const InequalityFactorGraph &_linearInequalities)
Definition: QP.h:46
gtsam::FactorGraph::print
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
Definition: FactorGraph-inst.h:37
gtsam::EqualityFactorGraph
Definition: EqualityFactorGraph.h:30
InequalityFactorGraph.h
Factor graph of all LinearInequality factors.
gtsam::QP::cachedCostVariableIndex_
VariableIndex cachedCostVariableIndex_
Definition: QP.h:37
gtsam::QP::costGradient
Vector costGradient(Key key, const VectorValues &delta) const
Definition: QP.h:67
gtsam::QP::costVariableIndex
const VariableIndex & costVariableIndex() const
Definition: QP.h:61
gtsam::Vector
Eigen::VectorXd Vector
Definition: Vector.h:38
gtsam::QP::inequalities
InequalityFactorGraph inequalities
linear inequality constraints: cI(x) <= 0
Definition: QP.h:34
gtsam::FactorGraph::at
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:306
gtsam::GaussianFactorGraph
Definition: GaussianFactorGraph.h:73
gtsam.examples.PlanarManipulatorExample.delta
def delta(g0, g1)
Definition: PlanarManipulatorExample.py:45
dataset.h
utility functions for loading datasets
gtsam::VectorValues
Definition: VectorValues.h:74
gtsam::QP::QP
QP()
Definition: QP.h:41
gtsam::GaussianFactor::shared_ptr
std::shared_ptr< This > shared_ptr
shared_ptr to this class
Definition: GaussianFactor.h:42
gtsam::QP::equalities
EqualityFactorGraph equalities
linear equality constraints: cE(x) = 0
Definition: QP.h:33
gtsam::QP
Definition: QP.h:31
gtsam::InequalityFactorGraph::print
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
Definition: InequalityFactorGraph.h:40
gtsam::VariableIndex
Definition: VariableIndex.h:41
g
void g(const string &key, int i)
Definition: testBTree.cpp:41
gtsam::QP::print
void print(const std::string &s="")
Definition: QP.h:54
key
const gtsam::Symbol key('X', 0)
gtsam::InequalityFactorGraph
Definition: InequalityFactorGraph.h:32
gtsam
traits
Definition: SFMdata.h:40
gtsam::VariableIndex::size
size_t size() const
The number of variable entries. This is equal to the number of unique variable Keys.
Definition: VariableIndex.h:78
Eigen::placeholders::end
static const EIGEN_DEPRECATED end_t end
Definition: IndexedViewHelper.h:181
gtsam::Key
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:97
gtsam::QP::cost
GaussianFactorGraph cost
Quadratic cost factors.
Definition: QP.h:32


gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:34:49