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 QP.h
14 * @brief Factor graphs of a Quadratic Programming problem
15 * @date Dec 8, 2014
16 * @author Duy-Nguyen Ta
17 */
18
19#pragma once
20
21#include <gtsam/linear/GaussianFactorGraph.h>
22#include <gtsam_unstable/linear/EqualityFactorGraph.h>
23#include <gtsam_unstable/linear/InequalityFactorGraph.h>
24#include <gtsam/slam/dataset.h>
25
26namespace gtsam {
27
28/**
29 * Struct contains factor graphs of a Quadratic Programming problem
30 */
31struct QP {
32 GaussianFactorGraph cost; //!< Quadratic cost factors
33 EqualityFactorGraph equalities; //!< linear equality constraints: cE(x) = 0
34 InequalityFactorGraph inequalities; //!< linear inequality constraints: cI(x) <= 0
35
36private:
37 mutable VariableIndex cachedCostVariableIndex_;
38
39public:
40 /** default constructor */
41 QP() :
42 cost(), equalities(), inequalities() {
43 }
44
45 /** constructor */
46 QP(const GaussianFactorGraph& _cost,
47 const EqualityFactorGraph& _linearEqualities,
48 const InequalityFactorGraph& _linearInequalities) :
49 cost(_cost), equalities(_linearEqualities), inequalities(
50 _linearInequalities) {
51 }
52
53 /** print */
54 void print(const std::string& s = "") {
55 std::cout << s << std::endl;
56 cost.print(s: "Quadratic cost factors: ");
57 equalities.print(s: "Linear equality factors: ");
58 inequalities.print(str: "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(size: delta.at(j: key).size());
69 if (costVariableIndex().find(key) != costVariableIndex().end()) {
70 for (size_t factorIx : costVariableIndex()[key]) {
71 GaussianFactor::shared_ptr factor = cost.at(i: factorIx);
72 g += factor->gradient(key, x: delta);
73 }
74 }
75 return g;
76 }
77};
78
79} // namespace gtsam
80
81