12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Cleaned up QPSVisitor and fixed KeyVector compile issue on Ubuntu

release/4.3a0
Frank Dellaert 2019-04-24 20:10:23 -04:00
parent a450ba1e55
commit 31556ff981
2 changed files with 176 additions and 206 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

267
gtsam_unstable/linear/QPSVisitor.cpp
View File

@ -12,41 +12,52 @@
/** /**
* @file QPSVisitor.cpp * @file QPSVisitor.cpp
* @brief As the QPS parser reads a file, it call functions in QPSVistor. * @brief As the QPS parser reads a file, it call functions in QPSVistor.
* This visitor in turn stores what the parser has read in a way that can be later used to build the Factor Graph for the * This visitor in turn stores what the parser has read in a way that can be
* QP Constraints and cost. This intermediate representation is required because an expression in the QPS file doesn't * later used to build the Factor Graph for the QP Constraints and cost. This
* necessarily correspond to a single constraint or term in the cost function. * intermediate representation is required because an expression in the QPS file
* doesn't necessarily correspond to a single constraint or term in the cost
* function.
* @author Ivan Dario Jimenez * @author Ivan Dario Jimenez
* @date 3/5/16 * @date 3/5/16
*/ */
#include <gtsam_unstable/linear/QPSVisitor.h> #include <gtsam_unstable/linear/QPSVisitor.h>
#include <algorithm>
#include <iostream> #include <iostream>
#include <map>
using boost::fusion::at_c; using boost::fusion::at_c;
using namespace std; using namespace std;
using Chars = std::vector<char>;
// Get a string from a fusion vector of Chars
template <size_t I, class FusionVector>
static string fromChars(const FusionVector &vars) {
const Chars &chars = at_c<I>(vars);
return string(chars.begin(), chars.end());
}
namespace gtsam { namespace gtsam {
void QPSVisitor::setName( void QPSVisitor::setName(
boost::fusion::vector<vector<char>, vector<char>, boost::fusion::vector<Chars, Chars, Chars> const &name) {
vector<char>> const &name) { name_ = fromChars<1>(name);
name_ = string(at_c < 1 > (name).begin(), at_c < 1 > (name).end());
if (debug) { if (debug) {
cout << "Parsing file: " << name_ << endl; cout << "Parsing file: " << name_ << endl;
} }
} }
void QPSVisitor::addColumn( void QPSVisitor::addColumn(
boost::fusion::vector<vector<char>, vector<char>, boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
vector<char>, vector<char>, vector<char>, double, Chars> const &vars) {
vector<char>> const &vars) { string var_ = fromChars<1>(vars);
string row_ = fromChars<3>(vars);
string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); Matrix11 coefficient = at_c<5>(vars) * I_1x1;
string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
if (debug) { if (debug) {
cout << "Added Column for Var: " << var_ << " Row: " << row_ cout << "Added Column for Var: " << var_ << " Row: " << row_
<< " Coefficient: " << coefficient << endl; << " Coefficient: " << coefficient << endl;
} }
if (!varname_to_key.count(var_)) if (!varname_to_key.count(var_))
varname_to_key[var_] = Symbol('X', numVariables++); varname_to_key[var_] = Symbol('X', numVariables++);
@ -58,17 +69,15 @@ void QPSVisitor::addColumn(
} }
void QPSVisitor::addColumnDouble( void QPSVisitor::addColumnDouble(
boost::fusion::vector<vector<char>, vector<char>, boost::fusion::vector<Chars, Chars, Chars, Chars, double, Chars, Chars,
vector<char>, vector<char>, double, vector<char>, Chars, double> const &vars) {
vector<char>, vector<char>, double> const &vars) { string var_ = fromChars<0>(vars);
string row1_ = fromChars<2>(vars);
string var_(at_c < 0 > (vars).begin(), at_c < 0 > (vars).end()); string row2_ = fromChars<6>(vars);
string row1_(at_c < 2 > (vars).begin(), at_c < 2 > (vars).end()); Matrix11 coefficient1 = at_c<4>(vars) * I_1x1;
string row2_(at_c < 6 > (vars).begin(), at_c < 6 > (vars).end()); Matrix11 coefficient2 = at_c<8>(vars) * I_1x1;
Matrix11 coefficient1 = at_c < 4 > (vars) * I_1x1;
Matrix11 coefficient2 = at_c < 8 > (vars) * I_1x1;
if (!varname_to_key.count(var_)) if (!varname_to_key.count(var_))
varname_to_key.insert( { var_, Symbol('X', numVariables++) }); varname_to_key.insert({var_, Symbol('X', numVariables++)});
if (row1_ == obj_name) if (row1_ == obj_name)
g[varname_to_key[var_]] = coefficient1; g[varname_to_key[var_]] = coefficient1;
else else
@ -80,47 +89,39 @@ void QPSVisitor::addColumnDouble(
} }
void QPSVisitor::addRangeSingle( void QPSVisitor::addRangeSingle(
boost::fusion::vector<vector<char>, vector<char>, boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
vector<char>, vector<char>, vector<char>, double, Chars> const &vars) {
vector<char>> const & vars) { string var_ = fromChars<1>(vars);
string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); string row_ = fromChars<3>(vars);
string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); double range = at_c<5>(vars);
double range = at_c < 5 > (vars);
ranges[row_] = range; ranges[row_] = range;
if (debug) { if (debug) {
cout << "SINGLE RANGE ADDED" << endl; cout << "SINGLE RANGE ADDED" << endl;
cout << "VAR:" << var_ << " ROW: " << row_ << " RANGE: " << range cout << "VAR:" << var_ << " ROW: " << row_ << " RANGE: " << range << endl;
<< endl;
} }
} }
void QPSVisitor::addRangeDouble( void QPSVisitor::addRangeDouble(
boost::fusion::vector<vector<char>, vector<char>, boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
vector<char>, vector<char>, vector<char>, double, Chars, Chars, double> const &vars) {
vector<char>, vector<char>, vector<char>, double> const & vars) { string var_ = fromChars<1>(vars);
string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); string row1_ = fromChars<3>(vars);
string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); string row2_ = fromChars<7>(vars);
string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end()); double range1 = at_c<5>(vars);
double range1 = at_c < 5 > (vars); double range2 = at_c<9>(vars);
double range2 = at_c < 9 > (vars);
ranges[row1_] = range1; ranges[row1_] = range1;
ranges[row2_] = range2; ranges[row2_] = range2;
if (debug) { if (debug) {
cout << "DOUBLE RANGE ADDED" << endl; cout << "DOUBLE RANGE ADDED" << endl;
cout << "VAR: " << var_ << " ROW1: " << row1_ << " RANGE1: " << range1 cout << "VAR: " << var_ << " ROW1: " << row1_ << " RANGE1: " << range1
<< " ROW2: " << row2_ << " RANGE2: " << range2 << endl; << " ROW2: " << row2_ << " RANGE2: " << range2 << endl;
} }
} }
void QPSVisitor::addRHS( void QPSVisitor::addRHS(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
boost::fusion::vector<vector<char>, vector<char>, double, Chars> const &vars) {
vector<char>, vector<char>, vector<char>, double, string var_ = fromChars<1>(vars);
vector<char>> const &vars) { string row_ = fromChars<3>(vars);
double coefficient = at_c<5>(vars);
string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
double coefficient = at_c < 5 > (vars);
if (row_ == obj_name) if (row_ == obj_name)
f = -coefficient; f = -coefficient;
else else
@ -128,20 +129,18 @@ void QPSVisitor::addRHS(
if (debug) { if (debug) {
cout << "Added RHS for Var: " << var_ << " Row: " << row_ cout << "Added RHS for Var: " << var_ << " Row: " << row_
<< " Coefficient: " << coefficient << endl; << " Coefficient: " << coefficient << endl;
} }
} }
void QPSVisitor::addRHSDouble( void QPSVisitor::addRHSDouble(
boost::fusion::vector<vector<char>, vector<char>, boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
vector<char>, vector<char>, vector<char>, double, Chars, Chars, double> const &vars) {
vector<char>, vector<char>, vector<char>, double> const &vars) { string var_ = fromChars<1>(vars);
string row1_ = fromChars<3>(vars);
string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); string row2_ = fromChars<7>(vars);
string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); double coefficient1 = at_c<5>(vars);
string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end()); double coefficient2 = at_c<9>(vars);
double coefficient1 = at_c < 5 > (vars);
double coefficient2 = at_c < 9 > (vars);
if (row1_ == obj_name) if (row1_ == obj_name)
f = -coefficient1; f = -coefficient1;
else else
@ -154,34 +153,32 @@ void QPSVisitor::addRHSDouble(
if (debug) { if (debug) {
cout << "Added RHS for Var: " << var_ << " Row: " << row1_ cout << "Added RHS for Var: " << var_ << " Row: " << row1_
<< " Coefficient: " << coefficient1 << endl; << " Coefficient: " << coefficient1 << endl;
cout << " " << "Row: " << row2_ cout << " "
<< " Coefficient: " << coefficient2 << endl; << "Row: " << row2_ << " Coefficient: " << coefficient2 << endl;
} }
} }
void QPSVisitor::addRow( void QPSVisitor::addRow(
boost::fusion::vector<vector<char>, char, vector<char>, boost::fusion::vector<Chars, char, Chars, Chars, Chars> const &vars) {
vector<char>, vector<char>> const &vars) { string name_ = fromChars<3>(vars);
char type = at_c<1>(vars);
string name_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
char type = at_c < 1 > (vars);
switch (type) { switch (type) {
case 'N': case 'N':
obj_name = name_; obj_name = name_;
break; break;
case 'L': case 'L':
row_to_constraint_v[name_] = &IL; row_to_constraint_v[name_] = &IL;
break; break;
case 'G': case 'G':
row_to_constraint_v[name_] = &IG; row_to_constraint_v[name_] = &IG;
break; break;
case 'E': case 'E':
row_to_constraint_v[name_] = &E; row_to_constraint_v[name_] = &E;
break; break;
default: default:
cout << "invalid type: " << type << endl; cout << "invalid type: " << type << endl;
break; break;
} }
if (debug) { if (debug) {
cout << "Added Row Type: " << type << " Name: " << name_ << endl; cout << "Added Row Type: " << type << " Name: " << name_ << endl;
@ -189,13 +186,11 @@ void QPSVisitor::addRow(
} }
void QPSVisitor::addBound( void QPSVisitor::addBound(
boost::fusion::vector<vector<char>, vector<char>, boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars,
vector<char>, vector<char>, vector<char>, double> const &vars) {
vector<char>, vector<char>, double> const &vars) { string type_ = fromChars<1>(vars);
string var_ = fromChars<5>(vars);
string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); double number = at_c<7>(vars);
string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end());
double number = at_c < 7 > (vars);
if (type_.compare(string("UP")) == 0) if (type_.compare(string("UP")) == 0)
up[varname_to_key[var_]] = number; up[varname_to_key[var_]] = number;
else if (type_.compare(string("LO")) == 0) else if (type_.compare(string("LO")) == 0)
@ -207,65 +202,61 @@ void QPSVisitor::addBound(
if (debug) { if (debug) {
cout << "Added Bound Type: " << type_ << " Var: " << var_ cout << "Added Bound Type: " << type_ << " Var: " << var_
<< " Amount: " << number << endl; << " Amount: " << number << endl;
} }
} }
void QPSVisitor::addFreeBound( void QPSVisitor::addFreeBound(
boost::fusion::vector<vector<char>, vector<char>, boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars> const
vector<char>, vector<char>, vector<char>, &vars) {
vector<char>, vector<char>> const &vars) { string type_ = fromChars<1>(vars);
string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); string var_ = fromChars<5>(vars);
string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end()); free.push_back(varname_to_key[var_]);
Free.push_back(varname_to_key[var_]);
if (debug) { if (debug) {
cout << "Added Free Bound Type: " << type_ << " Var: " << var_ cout << "Added Free Bound Type: " << type_ << " Var: " << var_
<< " Amount: " << endl; << " Amount: " << endl;
} }
} }
void QPSVisitor::addQuadTerm( void QPSVisitor::addQuadTerm(
boost::fusion::vector<vector<char>, vector<char>, boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
vector<char>, vector<char>, vector<char>, double, Chars> const &vars) {
vector<char>> const &vars) { string var1_ = fromChars<1>(vars);
string var1_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); string var2_ = fromChars<3>(vars);
string var2_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); Matrix11 coefficient = at_c<5>(vars) * I_1x1;
Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient; H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient;
H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient; H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient;
if (debug) { if (debug) {
cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_ cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_
<< " Coefficient: " << coefficient << endl; << " Coefficient: " << coefficient << endl;
} }
} }
QP QPSVisitor::makeQP() { QP QPSVisitor::makeQP() {
// Create the keys from the variable names // Create the keys from the variable names
vector < Key > keys; KeyVector keys;
for (auto kv : varname_to_key) { for (auto kv : varname_to_key) {
keys.push_back(kv.second); keys.push_back(kv.second);
} }
// Fill the G matrices and g vectors from // Fill the G matrices and g vectors from
vector < Matrix > Gs; vector<Matrix> Gs;
vector < Vector > gs; vector<Vector> gs;
sort(keys.begin(), keys.end()); sort(keys.begin(), keys.end());
for (unsigned int i = 0; i < keys.size(); ++i) { for (size_t i = 0; i < keys.size(); ++i) {
for (unsigned int j = i; j < keys.size(); ++j) { for (size_t j = i; j < keys.size(); ++j) {
if (H.count(keys[i]) > 0 and H[keys[i]].count(keys[j]) > 0){ if (H.count(keys[i]) > 0 && H[keys[i]].count(keys[j]) > 0) {
Gs.emplace_back(H[keys[i]][keys[j]]); Gs.emplace_back(H[keys[i]][keys[j]]);
} } else {
else{
Gs.emplace_back(Z_1x1); Gs.emplace_back(Z_1x1);
} }
} }
} }
for (Key key1 : keys) { for (Key key1 : keys) {
if(g.count(key1) > 0){ if (g.count(key1) > 0) {
gs.emplace_back(-g[key1]); gs.emplace_back(-g[key1]);
} } else {
else{
gs.emplace_back(Z_1x1); gs.emplace_back(Z_1x1);
} }
} }
@ -278,8 +269,8 @@ QP QPSVisitor::makeQP() {
// Add equality and inequality constraints into the QP // Add equality and inequality constraints into the QP
size_t dual_key_num = keys.size() + 1; size_t dual_key_num = keys.size() + 1;
for (auto kv : E) { for (auto kv : E) {
map < Key, Matrix11 > keyMatrixMapPos; map<Key, Matrix11> keyMatrixMapPos;
map < Key, Matrix11 > keyMatrixMapNeg; map<Key, Matrix11> keyMatrixMapNeg;
if (ranges.count(kv.first) == 1) { if (ranges.count(kv.first) == 1) {
for (auto km : kv.second) { for (auto km : kv.second) {
keyMatrixMapPos.insert(km); keyMatrixMapPos.insert(km);
@ -289,22 +280,20 @@ QP QPSVisitor::makeQP() {
if (ranges[kv.first] > 0) { if (ranges[kv.first] > 0) {
madeQP.inequalities.push_back( madeQP.inequalities.push_back(
LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++)); LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++));
madeQP.inequalities.push_back( madeQP.inequalities.push_back(LinearInequality(
LinearInequality(keyMatrixMapPos, b[kv.first] + ranges[kv.first], keyMatrixMapPos, b[kv.first] + ranges[kv.first], dual_key_num++));
dual_key_num++));
} else if (ranges[kv.first] < 0) { } else if (ranges[kv.first] < 0) {
madeQP.inequalities.push_back( madeQP.inequalities.push_back(
LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++)); LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++));
madeQP.inequalities.push_back( madeQP.inequalities.push_back(LinearInequality(
LinearInequality(keyMatrixMapNeg, ranges[kv.first] - b[kv.first], keyMatrixMapNeg, ranges[kv.first] - b[kv.first], dual_key_num++));
dual_key_num++));
} else { } else {
cerr << "ERROR: CANNOT ADD A RANGE OF ZERO" << endl; cerr << "ERROR: CANNOT ADD A RANGE OF ZERO" << endl;
throw; throw;
} }
continue; continue;
} }
map < Key, Matrix11 > keyMatrixMap; map<Key, Matrix11> keyMatrixMap;
for (auto km : kv.second) { for (auto km : kv.second) {
keyMatrixMap.insert(km); keyMatrixMap.insert(km);
} }
@ -313,8 +302,8 @@ QP QPSVisitor::makeQP() {
} }
for (auto kv : IG) { for (auto kv : IG) {
map < Key, Matrix11 > keyMatrixMapNeg; map<Key, Matrix11> keyMatrixMapNeg;
map < Key, Matrix11 > keyMatrixMapPos; map<Key, Matrix11> keyMatrixMapPos;
for (auto km : kv.second) { for (auto km : kv.second) {
keyMatrixMapPos.insert(km); keyMatrixMapPos.insert(km);
km.second = -km.second; km.second = -km.second;
@ -323,15 +312,14 @@ QP QPSVisitor::makeQP() {
madeQP.inequalities.push_back( madeQP.inequalities.push_back(
LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++)); LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++));
if (ranges.count(kv.first) == 1) { if (ranges.count(kv.first) == 1) {
madeQP.inequalities.push_back( madeQP.inequalities.push_back(LinearInequality(
LinearInequality(keyMatrixMapPos, b[kv.first] + ranges[kv.first], keyMatrixMapPos, b[kv.first] + ranges[kv.first], dual_key_num++));
dual_key_num++));
} }
} }
for (auto kv : IL) { for (auto kv : IL) {
map < Key, Matrix11 > keyMatrixMapPos; map<Key, Matrix11> keyMatrixMapPos;
map < Key, Matrix11 > keyMatrixMapNeg; map<Key, Matrix11> keyMatrixMapNeg;
for (auto km : kv.second) { for (auto km : kv.second) {
keyMatrixMapPos.insert(km); keyMatrixMapPos.insert(km);
km.second = -km.second; km.second = -km.second;
@ -340,15 +328,13 @@ QP QPSVisitor::makeQP() {
madeQP.inequalities.push_back( madeQP.inequalities.push_back(
LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++)); LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++));
if (ranges.count(kv.first) == 1) { if (ranges.count(kv.first) == 1) {
madeQP.inequalities.push_back( madeQP.inequalities.push_back(LinearInequality(
LinearInequality(keyMatrixMapNeg, ranges[kv.first] - b[kv.first], keyMatrixMapNeg, ranges[kv.first] - b[kv.first], dual_key_num++));
dual_key_num++));
} }
} }
for (Key k : keys) { for (Key k : keys) {
if (find(Free.begin(), Free.end(), k) != Free.end()) if (find(free.begin(), free.end(), k) != free.end()) continue;
continue;
if (fx.count(k) == 1) if (fx.count(k) == 1)
madeQP.equalities.push_back( madeQP.equalities.push_back(
LinearEquality(k, I_1x1, fx[k] * I_1x1, dual_key_num++)); LinearEquality(k, I_1x1, fx[k] * I_1x1, dual_key_num++));
@ -364,5 +350,4 @@ QP QPSVisitor::makeQP() {
} }
return madeQP; return madeQP;
} }
} } // namespace gtsam

115
gtsam_unstable/linear/QPSVisitor.h
View File

@ -18,14 +18,16 @@
#pragma once #pragma once
#include <gtsam_unstable/linear/QP.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/Matrix.h> #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/include/vector.hpp>
#include <boost/fusion/sequence.hpp> #include <boost/fusion/sequence.hpp>
#include <unordered_map>
#include <string> #include <string>
#include <unordered_map>
#include <vector> #include <vector>
namespace gtsam { namespace gtsam {
@ -34,87 +36,70 @@ namespace gtsam {
* in a way that can be later used to build the full QP problem in the file. * in a way that can be later used to build the full QP problem in the file.
*/ */
class QPSVisitor { class QPSVisitor {
private: private:
typedef std::unordered_map<Key, Matrix11> coefficient_v; typedef std::unordered_map<Key, Matrix11> coefficient_v;
typedef std::unordered_map<std::string, coefficient_v> constraint_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 std::unordered_map<std::string, constraint_v*> row_to_constraint_v; // Maps QPS ROWS to Variable-Matrix pairs
constraint_v E; // Equalities constraint_v E; // Equalities
constraint_v IG;// Inequalities >= constraint_v IG; // Inequalities >=
constraint_v IL;// Inequalities <= constraint_v IL; // Inequalities <=
unsigned int numVariables; 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> 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<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<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<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 std::unordered_map<Key, std::unordered_map<Key, Matrix11> > H; // H from hessian
double f; // Constant term of quadratic cost double f; // Constant term of quadratic cost
std::string obj_name; // the objective function has a name in the QPS 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::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> 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> lo; // Lower Bound constraints on variable where MIN < X
std::unordered_map<Key, double> fx; // Equalities specified as FX in BOUNDS part of QPS std::unordered_map<Key, double> fx; // Equalities specified as FX in BOUNDS part of QPS
std::vector<Key> Free; // Variables can be specified as Free (to which no constraints apply) KeyVector free; // Variables can be specified as free (to which no constraints apply)
const bool debug = false; const bool debug = false;
public: using Chars = std::vector<char>;
QPSVisitor() : numVariables(1) {
}
void setName( public:
boost::fusion::vector<std::vector<char>, std::vector<char>, QPSVisitor() : numVariables(1) {}
std::vector<char>> const & name);
void addColumn( void setName(boost::fusion::vector<Chars, Chars, Chars> const& name);
boost::fusion::vector<std::vector<char>, std::vector<char>,
std::vector<char>, std::vector<char>, std::vector<char>, double, void addColumn(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
std::vector<char>> const & vars); double, Chars> const& vars);
void addColumnDouble( void addColumnDouble(
boost::fusion::vector<std::vector<char>, std::vector<char>, boost::fusion::vector<Chars, Chars, Chars, Chars, double, Chars, Chars,
std::vector<char>, std::vector<char>, double, std::vector<char>, Chars, double> const& vars);
std::vector<char>, std::vector<char>, double> const & vars);
void addRHS( void addRHS(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
boost::fusion::vector<std::vector<char>, std::vector<char>, Chars> const& vars);
std::vector<char>, std::vector<char>, std::vector<char>, double,
std::vector<char>> const & vars);
void addRHSDouble( void addRHSDouble(
boost::fusion::vector<std::vector<char>, std::vector<char>, boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
std::vector<char>, std::vector<char>, std::vector<char>, double, Chars, Chars, double> const& vars);
std::vector<char>, std::vector<char>, std::vector<char>, double> const & vars);
void addRangeSingle( void addRangeSingle(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
boost::fusion::vector<std::vector<char>, std::vector<char>, double, Chars> const& vars);
std::vector<char>, std::vector<char>, std::vector<char>, double,
std::vector<char>> const & vars);
void addRangeDouble( void addRangeDouble(
boost::fusion::vector<std::vector<char>, std::vector<char>, boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
std::vector<char>, std::vector<char>, std::vector<char>, double, Chars, Chars, double> const& vars);
std::vector<char>, std::vector<char>, std::vector<char>, double> const & vars);
void addRow( void addRow(
boost::fusion::vector<std::vector<char>, char, std::vector<char>, boost::fusion::vector<Chars, char, Chars, Chars, Chars> const& vars);
std::vector<char>, std::vector<char>> const & vars);
void addBound( void addBound(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, Chars,
boost::fusion::vector<std::vector<char>, std::vector<char>, Chars, double> const& vars);
std::vector<char>, std::vector<char>, std::vector<char>,
std::vector<char>, std::vector<char>, double> const & vars);
void addFreeBound( void addFreeBound(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
boost::fusion::vector<std::vector<char>, std::vector<char>, Chars, Chars> const& vars);
std::vector<char>, std::vector<char>, std::vector<char>,
std::vector<char>, std::vector<char>> const & vars);
void addQuadTerm( void addQuadTerm(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
boost::fusion::vector<std::vector<char>, std::vector<char>, double, Chars> const& vars);
std::vector<char>, std::vector<char>, std::vector<char>, double,
std::vector<char>> const & vars);
QP makeQP(); QP makeQP();
} };
;
} } // namespace gtsam