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 QPInitSolver.h
14 * @brief This finds a feasible solution for a QP problem
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/LPInitSolver.h>
23
24namespace gtsam {
25
26/**
27 * This class finds a feasible solution for a QP problem.
28 * This uses the Matlab strategy for initialization
29 * For details, see
30 * http://www.mathworks.com/help/optim/ug/quadratic-programming-algorithms.html#brrzwpf-22
31 */
32class QPInitSolver {
33 const QP& qp_;
34public:
35 /// Constructor with a QP problem
36 QPInitSolver(const QP& qp) : qp_(qp) {}
37
38 ///@return a feasible initialization point
39 VectorValues solve() const {
40 // Make an LP with any linear cost function. It doesn't matter for
41 // initialization.
42 LP initProblem;
43 // make an unrelated key for a random variable cost
44 Key newKey = maxKey(problem: qp_) + 1;
45 initProblem.cost = LinearCost(newKey, Vector::Ones(newSize: 1));
46 initProblem.equalities = qp_.equalities;
47 initProblem.inequalities = qp_.inequalities;
48 LPInitSolver initSolver(initProblem);
49 return initSolver.solve();
50 }
51};
52
53
54}