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)
63  cachedCostVariableIndex_ = VariableIndex(cost);
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 
const gtsam::Symbol key('X', 0)
QP()
Definition: QP.h:41
const_iterator find(Key key) const
Find the iterator for the requested variable entry.
size_t size() const
The number of variable entries. This is equal to the number of unique variable Keys.
Definition: VariableIndex.h:78
Vector costGradient(Key key, const VectorValues &delta) const
Definition: QP.h:67
Factor graph of all LinearInequality factors.
Vector & at(Key j)
Definition: VectorValues.h:139
virtual void print(const std::string &s="FactorGraph", const KeyFormatter &formatter=DefaultKeyFormatter) const
Print out graph to std::cout, with optional key formatter.
const_iterator end() const
Iterator to the first variable entry.
Factor graph of all LinearEquality factors.
void g(const string &key, int i)
Definition: testBTree.cpp:41
GaussianFactorGraph cost
Quadratic cost factors.
Definition: QP.h:32
Eigen::VectorXd Vector
Definition: Vector.h:38
RealScalar s
Linear Factor Graph where all factors are Gaussians.
VariableIndex cachedCostVariableIndex_
Definition: QP.h:37
std::shared_ptr< This > shared_ptr
shared_ptr to this class
traits
Definition: chartTesting.h:28
InequalityFactorGraph inequalities
linear inequality constraints: cI(x) <= 0
Definition: QP.h:34
void print(const std::string &str="", const KeyFormatter &keyFormatter=DefaultKeyFormatter) const override
EqualityFactorGraph equalities
linear equality constraints: cE(x) = 0
Definition: QP.h:33
const sharedFactor at(size_t i) const
Definition: FactorGraph.h:343
QP(const GaussianFactorGraph &_cost, const EqualityFactorGraph &_linearEqualities, const InequalityFactorGraph &_linearInequalities)
Definition: QP.h:46
void print(const std::string &s="")
Definition: QP.h:54
Definition: QP.h:31
const VariableIndex & costVariableIndex() const
Definition: QP.h:61
utility functions for loading datasets
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:26