gtsam 4.1.1
Go to the documentation of this file.
1/* ----------------------------------------------------------------------------
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)
8 * See LICENSE for the license information
10 * -------------------------------------------------------------------------- */
24#include <limits>
25#include <algorithm>
27namespace gtsam {
30struct LPPolicy {
33 static constexpr double maxAlpha = std::numeric_limits<double>::infinity();
48 const VectorValues& xk) {
50 for (LinearCost::const_iterator it = lp.cost.begin(); it != lp.cost.end();
51 ++it) {
52 size_t dim = lp.cost.getDim(it);
53 Vector b =*it) - lp.cost.getA(it).transpose(); // b = xk-g
54 graph.emplace_shared<JacobianFactor>(*it, Matrix::Identity(dim, dim), b);
55 }
57 KeySet allKeys = lp.inequalities.keys();
58 allKeys.merge(lp.equalities.keys());
59 allKeys.merge(KeySet(lp.cost.keys()));
60 // Add corresponding factors for all variables that are not explicitly in
61 // the cost function. Gradients of the cost function wrt to these variables
62 // are zero (g=0), so b=xk
63 if (lp.cost.keys().size() != allKeys.size()) {
64 KeySet difference;
65 std::set_difference(allKeys.begin(), allKeys.end(), lp.cost.begin(),
66 lp.cost.end(),
67 std::inserter(difference, difference.end()));
68 for (Key k : difference) {
69 size_t dim = lp.constrainedKeyDimMap().at(k);
70 graph.emplace_shared<JacobianFactor>(k, Matrix::Identity(dim, dim),;
71 }
72 }
73 return graph;
74 }
77using LPSolver = ActiveSetSolver<LP, LPPolicy, LPInitSolver>;
Active set method for solving LP, QP problems.
This finds a feasible solution for an LP problem.
Struct used to hold a Linear Programming Problem.
Global functions in a separate testing namespace.
Definition: chartTesting.h:28
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:69
void merge(const FastSet &other)
insert another set: handy for MATLAB access
Definition: FastSet.h:121
KeySet keys() const
Potentially slow function to return all keys involved, sorted, as a set.
Definition: FactorGraph-inst.h:74
const KeyVector & keys() const
Access the factor's involved variable keys.
Definition: Factor.h:125
const_iterator begin() const
Iterator at beginning of involved variable keys.
Definition: Factor.h:128
KeyVector::const_iterator const_iterator
Const iterator over keys.
Definition: Factor.h:68
const_iterator end() const
Iterator at end of involved variable keys.
Definition: Factor.h:131
A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
Definition: GaussianFactorGraph.h:69
A Gaussian factor in the squared-error form.
Definition: JacobianFactor.h:91
DenseIndex getDim(const_iterator variable) const override
Return the dimension of the variable pointed to by the given key iterator todo: Remove this in favor ...
Definition: JacobianFactor.h:274
constABlock getA(const_iterator variable) const
Get a view of the A matrix for the variable pointed to by the given key iterator.
Definition: JacobianFactor.h:298
This class represents a collection of vector-valued variables associated each with a unique integer i...
Definition: VectorValues.h:74
Vector & at(Key j)
Read/write access to the vector value with key j, throws std::out_of_range if j does not exist,...
Definition: VectorValues.h:137
Data structure of a Linear Program.
Definition: LP.h:51
InequalityFactorGraph inequalities
Linear inequality constraints: cI(x) <= 0.
Definition: LP.h:56
LinearCost cost
Linear cost factor.
Definition: LP.h:54
EqualityFactorGraph equalities
Linear equality constraints: cE(x) = 0.
Definition: LP.h:55
Policy for ActivetSetSolver to solve Linear Programming.
Definition: LPSolver.h:30
static constexpr double maxAlpha
Maximum alpha for line search x'=xk + alpha*p, where p is the cost gradient For LP,...
Definition: LPSolver.h:33
static GaussianFactorGraph buildCostFunction(const LP &lp, const VectorValues &xk)
Create the factor ||x-xk - (-g)||^2 where xk is the current feasible solution on the constraint surfa...
Definition: LPSolver.h:47