106 lines
4.0 KiB
C++
106 lines
4.0 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file QPSVisitor.h
|
|
* @brief
|
|
* @author Ivan Dario Jimenez
|
|
* @date 3/5/16
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <gtsam/base/Matrix.h>
|
|
#include <gtsam/inference/Key.h>
|
|
#include <gtsam/inference/Symbol.h>
|
|
#include <gtsam_unstable/linear/QP.h>
|
|
|
|
#include <boost/fusion/include/vector.hpp>
|
|
#include <boost/fusion/sequence.hpp>
|
|
|
|
#include <string>
|
|
#include <unordered_map>
|
|
#include <vector>
|
|
|
|
namespace gtsam {
|
|
/**
|
|
* As the parser reads a file, it call functions in this visitor. This visitor in turn stores what the parser has read
|
|
* in a way that can be later used to build the full QP problem in the file.
|
|
*/
|
|
class QPSVisitor {
|
|
private:
|
|
typedef std::unordered_map<Key, Matrix11> coefficient_v;
|
|
typedef std::unordered_map<std::string, coefficient_v> constraint_v;
|
|
|
|
std::unordered_map<std::string, constraint_v*> row_to_constraint_v; // Maps QPS ROWS to Variable-Matrix pairs
|
|
constraint_v E; // Equalities
|
|
constraint_v IG; // Inequalities >=
|
|
constraint_v IL; // Inequalities <=
|
|
unsigned int numVariables;
|
|
std::unordered_map<std::string, double> b; // maps from constraint name to b value for Ax = b equality constraints
|
|
std::unordered_map<std::string, double> ranges; // Inequalities can be specified as ranges on a variable
|
|
std::unordered_map<Key, Vector1> g; // linear term of quadratic cost
|
|
std::unordered_map<std::string, Key> varname_to_key; // Variable QPS string name to key
|
|
std::unordered_map<Key, std::unordered_map<Key, Matrix11> > H; // H from hessian
|
|
double f; // Constant term of quadratic cost
|
|
std::string obj_name; // the objective function has a name in the QPS
|
|
std::string name_; // the quadratic program has a name in the QPS
|
|
std::unordered_map<Key, double> up; // Upper Bound constraints on variable where X < MAX
|
|
std::unordered_map<Key, double> lo; // Lower Bound constraints on variable where MIN < X
|
|
std::unordered_map<Key, double> fx; // Equalities specified as FX in BOUNDS part of QPS
|
|
KeyVector free; // Variables can be specified as free (to which no constraints apply)
|
|
const bool debug = false;
|
|
|
|
using Chars = std::vector<char>;
|
|
|
|
public:
|
|
QPSVisitor() : numVariables(1) {}
|
|
|
|
void setName(boost::fusion::vector<Chars, Chars, Chars> const& name);
|
|
|
|
void addColumn(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
|
|
double, Chars> const& vars);
|
|
|
|
void addColumnDouble(
|
|
boost::fusion::vector<Chars, Chars, Chars, Chars, double, Chars, Chars,
|
|
Chars, double> const& vars);
|
|
|
|
void addRHS(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
|
|
Chars> const& vars);
|
|
|
|
void addRHSDouble(
|
|
boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
|
|
Chars, Chars, double> const& vars);
|
|
|
|
void addRangeSingle(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
|
|
double, Chars> const& vars);
|
|
|
|
void addRangeDouble(
|
|
boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
|
|
Chars, Chars, double> const& vars);
|
|
|
|
void addRow(
|
|
boost::fusion::vector<Chars, char, Chars, Chars, Chars> const& vars);
|
|
|
|
void addBound(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, Chars,
|
|
Chars, double> const& vars);
|
|
|
|
void addFreeBound(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
|
|
Chars, Chars> const& vars);
|
|
|
|
void addQuadTerm(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
|
|
double, Chars> const& vars);
|
|
|
|
QP makeQP();
|
|
};
|
|
|
|
} // namespace gtsam
|