LPSolver.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 
20 #pragma once
21 
25 
26 #include <limits>
27 #include <algorithm>
28 
29 namespace gtsam {
30 
32 struct LPPolicy {
35  static constexpr double maxAlpha = std::numeric_limits<double>::infinity();
36 
50  const VectorValues& xk) {
52  for (LinearCost::const_iterator it = lp.cost.begin(); it != lp.cost.end();
53  ++it) {
54  size_t dim = lp.cost.getDim(it);
55  Vector b = xk.at(*it) - lp.cost.getA(it).transpose(); // b = xk-g
56  graph.emplace_shared<JacobianFactor>(*it, Matrix::Identity(dim, dim), b);
57  }
58 
59  KeySet allKeys = lp.inequalities.keys();
60  allKeys.merge(lp.equalities.keys());
61  allKeys.merge(KeySet(lp.cost.keys()));
62  // Add corresponding factors for all variables that are not explicitly in
63  // the cost function. Gradients of the cost function wrt to these variables
64  // are zero (g=0), so b=xk
65  if (lp.cost.keys().size() != allKeys.size()) {
66  KeySet difference;
67  std::set_difference(allKeys.begin(), allKeys.end(), lp.cost.begin(),
68  lp.cost.end(),
69  std::inserter(difference, difference.end()));
70  for (Key k : difference) {
71  size_t dim = lp.constrainedKeyDimMap().at(k);
72  graph.emplace_shared<JacobianFactor>(k, Matrix::Identity(dim, dim), xk.at(k));
73  }
74  }
75  return graph;
76  }
77 };
78 
80 
81 }
EqualityFactorGraph equalities
Linear equality constraints: cE(x) = 0.
Definition: LP.h:55
KeySet keys() const
InequalityFactorGraph inequalities
Linear inequality constraints: cI(x) <= 0.
Definition: LP.h:56
IsDerived< DERIVEDFACTOR > emplace_shared(Args &&... args)
Emplace a shared pointer to factor of given type.
Definition: FactorGraph.h:196
static constexpr double maxAlpha
Definition: LPSolver.h:35
Active set method for solving LP, QP problems.
void merge(const FastSet &other)
Definition: FastSet.h:121
Vector & at(Key j)
Definition: VectorValues.h:139
NonlinearFactorGraph graph
const_iterator end() const
Definition: Factor.h:148
Definition: LP.h:51
Eigen::VectorXd Vector
Definition: Vector.h:38
Policy for ActivetSetSolver to solve Linear Programming.
Definition: LPSolver.h:32
LinearCost cost
Linear cost factor.
Definition: LP.h:54
DenseIndex getDim(const_iterator variable) const override
const G & b
Definition: Group.h:86
static GaussianFactorGraph buildCostFunction(const LP &lp, const VectorValues &xk)
Definition: LPSolver.h:49
traits
Definition: chartTesting.h:28
Struct used to hold a Linear Programming Problem.
const KeyVector & keys() const
Access the factor&#39;s involved variable keys.
Definition: Factor.h:142
const KeyDimMap & constrainedKeyDimMap() const
Definition: LP.h:80
FastSet< Key > KeySet
Definition: Key.h:90
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:82
const_iterator begin() const
Definition: Factor.h:145
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
constABlock getA(const_iterator variable) const
This finds a feasible solution for an LP problem.


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