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 LPSolver.h
14 * @brief Policy of ActiveSetSolver to solve Linear Programming Problems
15 * @author Duy Nguyen Ta
16 * @author Ivan Dario Jimenez
17 * @date 6/16/16
18 */
19
20#pragma once
21
22#include <gtsam_unstable/linear/LP.h>
23#include <gtsam_unstable/linear/ActiveSetSolver.h>
24#include <gtsam_unstable/linear/LPInitSolver.h>
25
26#include <limits>
27#include <algorithm>
28
29namespace gtsam {
30
31/// Policy for ActivetSetSolver to solve Linear Programming \sa LP problems
32struct LPPolicy {
33 /// Maximum alpha for line search x'=xk + alpha*p, where p is the cost gradient
34 /// For LP, maxAlpha = Infinity
35 static constexpr double maxAlpha = std::numeric_limits<double>::infinity();
36
37 /**
38 * Create the factor ||x-xk - (-g)||^2 where xk is the current feasible solution
39 * on the constraint surface and g is the gradient of the linear cost,
40 * i.e. -g is the direction we wish to follow to decrease the cost.
41 *
42 * Essentially, we try to match the direction d = x-xk with -g as much as possible
43 * subject to the condition that x needs to be on the constraint surface, i.e., d is
44 * along the surface's subspace.
45 *
46 * The least-square solution of this quadratic subject to a set of linear constraints
47 * is the projection of the gradient onto the constraints' subspace
48 */
49 static GaussianFactorGraph buildCostFunction(const LP& lp,
50 const VectorValues& xk) {
51 GaussianFactorGraph graph;
52 for (LinearCost::const_iterator it = lp.cost.begin(); it != lp.cost.end();
53 ++it) {
54 size_t dim = lp.cost.getDim(variable: it);
55 Vector b = xk.at(j: *it) - lp.cost.getA(variable: it).transpose(); // b = xk-g
56 graph.emplace_shared<JacobianFactor>(args: *it, args: Matrix::Identity(rows: dim, cols: dim), args&: b);
57 }
58
59 KeySet allKeys = lp.inequalities.keys();
60 allKeys.merge(other: lp.equalities.keys());
61 allKeys.merge(other: 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(first1: allKeys.begin(), last1: allKeys.end(), first2: lp.cost.begin(),
68 last2: lp.cost.end(),
69 result: std::inserter(x&: difference, i: difference.end()));
70 for (Key k : difference) {
71 size_t dim = lp.constrainedKeyDimMap().at(k: k);
72 graph.emplace_shared<JacobianFactor>(args&: k, args: Matrix::Identity(rows: dim, cols: dim), args: xk.at(j: k));
73 }
74 }
75 return graph;
76 }
77};
78
79using LPSolver = ActiveSetSolver<LP, LPPolicy, LPInitSolver>;
80
81}
82