QPSParser.cpp
Go to the documentation of this file.
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 
18 #define BOOST_SPIRIT_USE_PHOENIX_V3 1
19 
20 #include <gtsam/base/Matrix.h>
21 #include <gtsam/inference/Key.h>
22 #include <gtsam/inference/Symbol.h>
26 
27 #include <boost/fusion/include/vector.hpp>
28 #include <boost/fusion/sequence.hpp>
29 #include <boost/lambda/lambda.hpp>
30 #include <boost/phoenix/bind.hpp>
31 #include <boost/spirit/include/classic.hpp>
32 #include <boost/spirit/include/qi.hpp>
33 
34 #include <algorithm>
35 #include <iostream>
36 #include <map>
37 #include <string>
38 #include <unordered_map>
39 #include <vector>
40 
41 using boost::fusion::at_c;
42 using namespace std::placeholders;
43 using namespace std;
44 
45 namespace bf = boost::fusion;
46 namespace qi = boost::spirit::qi;
47 
48 using Chars = std::vector<char>;
49 
50 // Get a string from a fusion vector of Chars
51 template <size_t I, class FusionVector>
52 static string fromChars(const FusionVector &vars) {
53  const Chars &chars = at_c<I>(vars);
54  return string(chars.begin(), chars.end());
55 }
56 
57 namespace gtsam {
58 
64 class QPSVisitor {
65  private:
66  typedef std::unordered_map<Key, Matrix11> coefficient_v;
67  typedef std::unordered_map<std::string, coefficient_v> constraint_v;
68 
69  std::unordered_map<std::string, constraint_v *>
70  row_to_constraint_v; // Maps QPS ROWS to Variable-Matrix pairs
71  constraint_v E; // Equalities
72  constraint_v IG; // Inequalities >=
73  constraint_v IL; // Inequalities <=
74  unsigned int numVariables;
75  std::unordered_map<std::string, double>
76  b; // maps from constraint name to b value for Ax = b equality
77  // constraints
78  std::unordered_map<std::string, double>
79  ranges; // Inequalities can be specified as ranges on a variable
80  std::unordered_map<Key, Vector1> g; // linear term of quadratic cost
81  std::unordered_map<std::string, Key>
82  varname_to_key; // Variable QPS string name to key
83  std::unordered_map<Key, std::unordered_map<Key, Matrix11>>
84  H; // H from hessian
85  double f = 0; // Constant term of quadratic cost
86  std::string obj_name; // the objective function has a name in the QPS
87  std::string name_; // the quadratic program has a name in the QPS
88  std::unordered_map<Key, double>
89  up; // Upper Bound constraints on variable where X < MAX
90  std::unordered_map<Key, double>
91  lo; // Lower Bound constraints on variable where MIN < X
92  std::unordered_map<Key, double>
93  fx; // Equalities specified as FX in BOUNDS part of QPS
94  KeyVector free; // Variables can be specified as free (to which no
95  // constraints apply)
96  const bool debug = false;
97 
98  public:
99  QPSVisitor() : numVariables(1) {}
100 
101  void setName(boost::fusion::vector<Chars, Chars, Chars> const &name) {
102  name_ = fromChars<1>(name);
103  if (debug) {
104  cout << "Parsing file: " << name_ << endl;
105  }
106  }
107 
108  void addColumn(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
109  double, Chars> const &vars) {
110  string var_ = fromChars<1>(vars);
111  string row_ = fromChars<3>(vars);
112  Matrix11 coefficient = at_c<5>(vars) * I_1x1;
113  if (debug) {
114  cout << "Added Column for Var: " << var_ << " Row: " << row_
115  << " Coefficient: " << coefficient << endl;
116  }
117  if (!varname_to_key.count(var_))
118  varname_to_key[var_] = Symbol('X', numVariables++);
119  if (row_ == obj_name) {
120  g[varname_to_key[var_]] = coefficient;
121  return;
122  }
123  (*row_to_constraint_v[row_])[row_][varname_to_key[var_]] = coefficient;
124  }
125 
127  boost::fusion::vector<Chars, Chars, Chars, Chars, double, Chars, Chars,
128  Chars, double> const &vars) {
129  string var_ = fromChars<0>(vars);
130  string row1_ = fromChars<2>(vars);
131  string row2_ = fromChars<6>(vars);
132  Matrix11 coefficient1 = at_c<4>(vars) * I_1x1;
133  Matrix11 coefficient2 = at_c<8>(vars) * I_1x1;
134  if (!varname_to_key.count(var_))
135  varname_to_key.insert({var_, Symbol('X', numVariables++)});
136  if (row1_ == obj_name)
137  g[varname_to_key[var_]] = coefficient1;
138  else
139  (*row_to_constraint_v[row1_])[row1_][varname_to_key[var_]] = coefficient1;
140  if (row2_ == obj_name)
141  g[varname_to_key[var_]] = coefficient2;
142  else
143  (*row_to_constraint_v[row2_])[row2_][varname_to_key[var_]] = coefficient2;
144  }
145 
146  void addRangeSingle(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
147  double, Chars> const &vars) {
148  string var_ = fromChars<1>(vars);
149  string row_ = fromChars<3>(vars);
150  double range = at_c<5>(vars);
151  ranges[row_] = range;
152  if (debug) {
153  cout << "SINGLE RANGE ADDED" << endl;
154  cout << "VAR:" << var_ << " ROW: " << row_ << " RANGE: " << range << endl;
155  }
156  }
158  boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
159  Chars, Chars, double> const &vars) {
160  string var_ = fromChars<1>(vars);
161  string row1_ = fromChars<3>(vars);
162  string row2_ = fromChars<7>(vars);
163  double range1 = at_c<5>(vars);
164  double range2 = at_c<9>(vars);
165  ranges[row1_] = range1;
166  ranges[row2_] = range2;
167  if (debug) {
168  cout << "DOUBLE RANGE ADDED" << endl;
169  cout << "VAR: " << var_ << " ROW1: " << row1_ << " RANGE1: " << range1
170  << " ROW2: " << row2_ << " RANGE2: " << range2 << endl;
171  }
172  }
173 
174  void addRHS(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
175  Chars> const &vars) {
176  string var_ = fromChars<1>(vars);
177  string row_ = fromChars<3>(vars);
178  double coefficient = at_c<5>(vars);
179  if (row_ == obj_name) {
180  f = -coefficient;
181  } else {
182  b[row_] = coefficient;
183  }
184 
185  if (debug) {
186  cout << "Added RHS for Var: " << var_ << " Row: " << row_
187  << " Coefficient: " << coefficient << endl;
188  }
189  }
190 
192  boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
193  Chars, Chars, double> const &vars) {
194  string var_ = fromChars<1>(vars);
195  string row1_ = fromChars<3>(vars);
196  string row2_ = fromChars<7>(vars);
197  double coefficient1 = at_c<5>(vars);
198  double coefficient2 = at_c<9>(vars);
199  if (row1_ == obj_name) {
200  f = -coefficient1;
201  } else {
202  b[row1_] = coefficient1;
203  }
204 
205  if (row2_ == obj_name) {
206  f = -coefficient2;
207  } else {
208  b[row2_] = coefficient2;
209  }
210 
211  if (debug) {
212  cout << "Added RHS for Var: " << var_ << " Row: " << row1_
213  << " Coefficient: " << coefficient1 << endl;
214  cout << " "
215  << "Row: " << row2_ << " Coefficient: " << coefficient2 << endl;
216  }
217  }
218 
219  void addRow(
220  boost::fusion::vector<Chars, char, Chars, Chars, Chars> const &vars) {
221  string name_ = fromChars<3>(vars);
222  char type = at_c<1>(vars);
223  switch (type) {
224  case 'N':
225  obj_name = name_;
226  break;
227  case 'L':
228  row_to_constraint_v[name_] = &IL;
229  break;
230  case 'G':
231  row_to_constraint_v[name_] = &IG;
232  break;
233  case 'E':
234  row_to_constraint_v[name_] = &E;
235  break;
236  default:
237  cout << "invalid type: " << type << endl;
238  break;
239  }
240  if (debug) {
241  cout << "Added Row Type: " << type << " Name: " << name_ << endl;
242  }
243  }
244 
245  void addBound(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, Chars,
246  Chars, double> const &vars) {
247  string type_ = fromChars<1>(vars);
248  string var_ = fromChars<5>(vars);
249  double number = at_c<7>(vars);
250  if (type_.compare(string("UP")) == 0)
251  up[varname_to_key[var_]] = number;
252  else if (type_.compare(string("LO")) == 0)
253  lo[varname_to_key[var_]] = number;
254  else if (type_.compare(string("FX")) == 0)
255  fx[varname_to_key[var_]] = number;
256  else
257  cout << "Invalid Bound Type: " << type_ << endl;
258 
259  if (debug) {
260  cout << "Added Bound Type: " << type_ << " Var: " << var_
261  << " Amount: " << number << endl;
262  }
263  }
264 
265  void addFreeBound(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
266  Chars, Chars> const &vars) {
267  string type_ = fromChars<1>(vars);
268  string var_ = fromChars<5>(vars);
269  free.push_back(varname_to_key[var_]);
270  if (debug) {
271  cout << "Added Free Bound Type: " << type_ << " Var: " << var_
272  << " Amount: " << endl;
273  }
274  }
275 
276  void addQuadTerm(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
277  double, Chars> const &vars) {
278  string var1_ = fromChars<1>(vars);
279  string var2_ = fromChars<3>(vars);
280  Matrix11 coefficient = at_c<5>(vars) * I_1x1;
281 
282  H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient;
283  H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient;
284  if (debug) {
285  cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_
286  << " Coefficient: " << coefficient << endl;
287  }
288  }
289 
290  QP makeQP() {
291  // Create the keys from the variable names
292  KeyVector keys;
293  for (auto kv : varname_to_key) {
294  keys.push_back(kv.second);
295  }
296 
297  // Fill the G matrices and g vectors from
298  vector<Matrix> Gs;
299  vector<Vector> gs;
300  sort(keys.begin(), keys.end());
301  for (size_t i = 0; i < keys.size(); ++i) {
302  for (size_t j = i; j < keys.size(); ++j) {
303  if (H.count(keys[i]) > 0 && H[keys[i]].count(keys[j]) > 0) {
304  Gs.emplace_back(H[keys[i]][keys[j]]);
305  } else {
306  Gs.emplace_back(Z_1x1);
307  }
308  }
309  }
310  for (Key key1 : keys) {
311  if (g.count(key1) > 0) {
312  gs.emplace_back(-g[key1]);
313  } else {
314  gs.emplace_back(Z_1x1);
315  }
316  }
317 
318  // Construct the quadratic program
319  QP madeQP;
320  auto obj = HessianFactor(keys, Gs, gs, 2 * f);
321  madeQP.cost.push_back(obj);
322 
323  // Add equality and inequality constraints into the QP
324  size_t dual_key_num = keys.size() + 1;
325  for (auto kv : E) {
326  map<Key, Matrix11> keyMatrixMapPos;
327  map<Key, Matrix11> keyMatrixMapNeg;
328  if (ranges.count(kv.first) == 1) {
329  for (auto km : kv.second) {
330  keyMatrixMapPos.insert(km);
331  km.second = -km.second;
332  keyMatrixMapNeg.insert(km);
333  }
334  if (ranges[kv.first] > 0) {
335  madeQP.inequalities.push_back(
336  LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++));
338  keyMatrixMapPos, b[kv.first] + ranges[kv.first], dual_key_num++));
339  } else if (ranges[kv.first] < 0) {
340  madeQP.inequalities.push_back(
341  LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++));
343  keyMatrixMapNeg, ranges[kv.first] - b[kv.first], dual_key_num++));
344  } else {
345  cerr << "ERROR: CANNOT ADD A RANGE OF ZERO" << endl;
346  throw;
347  }
348  continue;
349  }
350  map<Key, Matrix11> keyMatrixMap;
351  for (auto km : kv.second) {
352  keyMatrixMap.insert(km);
353  }
354  madeQP.equalities.push_back(
355  LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++));
356  }
357 
358  for (auto kv : IG) {
359  map<Key, Matrix11> keyMatrixMapNeg;
360  map<Key, Matrix11> keyMatrixMapPos;
361  for (auto km : kv.second) {
362  keyMatrixMapPos.insert(km);
363  km.second = -km.second;
364  keyMatrixMapNeg.insert(km);
365  }
366  madeQP.inequalities.push_back(
367  LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++));
368  if (ranges.count(kv.first) == 1) {
370  keyMatrixMapPos, b[kv.first] + ranges[kv.first], dual_key_num++));
371  }
372  }
373 
374  for (auto kv : IL) {
375  map<Key, Matrix11> keyMatrixMapPos;
376  map<Key, Matrix11> keyMatrixMapNeg;
377  for (auto km : kv.second) {
378  keyMatrixMapPos.insert(km);
379  km.second = -km.second;
380  keyMatrixMapNeg.insert(km);
381  }
382  madeQP.inequalities.push_back(
383  LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++));
384  if (ranges.count(kv.first) == 1) {
386  keyMatrixMapNeg, ranges[kv.first] - b[kv.first], dual_key_num++));
387  }
388  }
389 
390  for (Key k : keys) {
391  if (find(free.begin(), free.end(), k) != free.end()) continue;
392  if (fx.count(k) == 1)
393  madeQP.equalities.push_back(
394  LinearEquality(k, I_1x1, fx[k] * I_1x1, dual_key_num++));
395  if (up.count(k) == 1)
396  madeQP.inequalities.push_back(
397  LinearInequality(k, I_1x1, up[k], dual_key_num++));
398  if (lo.count(k) == 1)
399  madeQP.inequalities.push_back(
400  LinearInequality(k, -I_1x1, -lo[k], dual_key_num++));
401  else
402  madeQP.inequalities.push_back(
403  LinearInequality(k, -I_1x1, 0, dual_key_num++));
404  }
405  return madeQP;
406  }
407 };
408 
409 typedef qi::grammar<boost::spirit::basic_istream_iterator<char>> base_grammar;
410 
411 struct QPSParser::MPSGrammar : base_grammar {
412  typedef std::vector<char> Chars;
414  std::function<void(bf::vector<Chars, Chars, Chars> const &)> setName;
415  std::function<void(bf::vector<Chars, char, Chars, Chars, Chars> const &)>
416  addRow;
417  std::function<void(
418  bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)>
420  std::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, double,
421  Chars, Chars, Chars, double>)>
423  std::function<void(
424  bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)>
426  std::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, double,
427  Chars, Chars, Chars, double>)>
429  std::function<void(
430  bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars>)>
432  std::function<void(bf::vector<Chars, Chars, Chars, Chars, double, Chars,
433  Chars, Chars, double> const &)>
435  std::function<void(
436  bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)>
438  std::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, Chars,
439  Chars, double> const &)>
441  std::function<void(
442  bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars> const &)>
445  : base_grammar(start),
446  rqp_(rqp),
447  setName(std::bind(&QPSVisitor::setName, rqp, std::placeholders::_1)),
448  addRow(std::bind(&QPSVisitor::addRow, rqp, std::placeholders::_1)),
449  rhsSingle(std::bind(&QPSVisitor::addRHS, rqp, std::placeholders::_1)),
450  rhsDouble(std::bind(&QPSVisitor::addRHSDouble, rqp, std::placeholders::_1)),
451  rangeSingle(std::bind(&QPSVisitor::addRangeSingle, rqp, std::placeholders::_1)),
452  rangeDouble(std::bind(&QPSVisitor::addRangeDouble, rqp, std::placeholders::_1)),
453  colSingle(std::bind(&QPSVisitor::addColumn, rqp, std::placeholders::_1)),
454  colDouble(std::bind(&QPSVisitor::addColumnDouble, rqp, std::placeholders::_1)),
455  addQuadTerm(std::bind(&QPSVisitor::addQuadTerm, rqp, std::placeholders::_1)),
456  addBound(std::bind(&QPSVisitor::addBound, rqp, std::placeholders::_1)),
457  addFreeBound(std::bind(&QPSVisitor::addFreeBound, rqp, std::placeholders::_1)) {
458  using namespace boost::spirit;
459  using namespace boost::spirit::qi;
460  character = lexeme[alnum | '_' | '-' | '.'];
461  title = lexeme[character >> *(blank | character)];
462  word = lexeme[+character];
463  name = lexeme[lit("NAME") >> *blank >> title >> +space][setName];
464  row = lexeme[*blank >> character >> +blank >> word >> *blank][addRow];
465  rhs_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >>
466  *blank][rhsSingle];
467  rhs_double =
468  lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ >>
469  +blank >> word >> +blank >> double_)[rhsDouble] >>
470  *blank];
471  range_single = lexeme[*blank >> word >> +blank >> word >> +blank >>
472  double_ >> *blank][rangeSingle];
473  range_double =
474  lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ >>
475  +blank >> word >> +blank >> double_)[rangeDouble] >>
476  *blank];
477  col_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >>
478  *blank][colSingle];
479  col_double =
480  lexeme[*blank >> (word >> +blank >> word >> +blank >> double_ >>
481  +blank >> word >> +blank >> double_)[colDouble] >>
482  *blank];
483  quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >>
484  *blank][addQuadTerm];
485  bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >>
486  +blank >> double_)[addBound] >>
487  *blank];
488  bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word >>
489  *blank][addFreeBound];
490  rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)];
491  rhs = lexeme[lit("RHS") >> *blank >> eol >>
492  +((rhs_double | rhs_single) >> eol)];
493  cols = lexeme[lit("COLUMNS") >> *blank >> eol >>
494  +((col_double | col_single) >> eol)];
495  quad = lexeme[lit("QUADOBJ") >> *blank >> eol >> +(quad_l >> eol)];
496  bounds = lexeme[lit("BOUNDS") >> +space >> *((bound | bound_fr) >> eol)];
497  ranges = lexeme[lit("RANGES") >> +space >>
498  *((range_double | range_single) >> eol)];
499  end = lexeme[lit("ENDATA") >> *space];
500  start =
501  lexeme[name >> rows >> cols >> rhs >> -ranges >> bounds >> quad >> end];
502  }
503 
504  qi::rule<boost::spirit::basic_istream_iterator<char>, char()> character;
505  qi::rule<boost::spirit::basic_istream_iterator<char>, Chars()> word, title;
506  qi::rule<boost::spirit::basic_istream_iterator<char>> row, end, col_single,
507  col_double, rhs_single, rhs_double, range_single, range_double, ranges,
508  bound, bound_fr, bounds, quad, quad_l, rows, cols, rhs, name, start;
509 };
510 
511 QP QPSParser::Parse() {
512  QPSVisitor rawData;
513  std::fstream stream(fileName_.c_str());
514  stream.unsetf(std::ios::skipws);
515  boost::spirit::basic_istream_iterator<char> begin(stream);
516  boost::spirit::basic_istream_iterator<char> last;
517 
518  if (!parse(begin, last, MPSGrammar(&rawData)) || begin != last) {
519  throw QPSParserException();
520  }
521 
522  return rawData.makeQP();
523 }
524 
525 } // namespace gtsam
qi::rule< boost::spirit::basic_istream_iterator< char > > start
Definition: QPSParser.cpp:506
std::function< void(bf::vector< Chars, Chars, Chars, Chars, double, Chars, Chars, Chars, double > const &)> colDouble
Definition: QPSParser.cpp:434
std::function< void(bf::vector< Chars, Chars, Chars, Chars, Chars, Chars, Chars, double > const &)> addBound
Definition: QPSParser.cpp:440
void addFreeBound(boost::fusion::vector< Chars, Chars, Chars, Chars, Chars, Chars, Chars > const &vars)
Definition: QPSParser.cpp:265
Scalar * b
Definition: benchVecAdd.cpp:17
std::function< void(bf::vector< Chars, Chars, Chars, Chars, Chars, double, Chars >)> colSingle
Definition: QPSParser.cpp:431
void addRow(boost::fusion::vector< Chars, char, Chars, Chars, Chars > const &vars)
Definition: QPSParser.cpp:219
qi::grammar< boost::spirit::basic_istream_iterator< char > > base_grammar
Definition: QPSParser.cpp:409
IsDerived< DERIVEDFACTOR > push_back(std::shared_ptr< DERIVEDFACTOR > factor)
Add a factor directly using a shared_ptr.
Definition: FactorGraph.h:190
constraint_v IL
Definition: QPSParser.cpp:73
KeyVector free
Definition: QPSParser.cpp:94
void addBound(boost::fusion::vector< Chars, Chars, Chars, Chars, Chars, Chars, Chars, double > const &vars)
Definition: QPSParser.cpp:245
std::unordered_map< Key, Matrix11 > coefficient_v
Definition: QPSParser.cpp:66
std::function< void(bf::vector< Chars, Chars, Chars, Chars, Chars, Chars, Chars > const &)> addFreeBound
Definition: QPSParser.cpp:443
static double range2(const Camera &camera, const Camera2 &camera2)
Definition: BFloat16.h:88
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate set rrange [*:*] noreverse nowriteback set trange [*:*] noreverse nowriteback set urange [*:*] noreverse nowriteback set vrange [*:*] noreverse nowriteback set xlabel matrix size set x2label set timefmt d m y n H
void addQuadTerm(boost::fusion::vector< Chars, Chars, Chars, Chars, Chars, double, Chars > const &vars)
Definition: QPSParser.cpp:276
static const symbolic::SymbolExpr< internal::symbolic_last_tag > last
void addRangeDouble(boost::fusion::vector< Chars, Chars, Chars, Chars, Chars, double, Chars, Chars, Chars, double > const &vars)
Definition: QPSParser.cpp:157
static constexpr bool debug
Exception thrown if there is an error parsing a QPS file.
GaussianFactorGraph cost
Quadratic cost factors.
Definition: QP.h:32
std::string name_
Definition: QPSParser.cpp:87
unsigned int numVariables
Definition: QPSParser.cpp:74
MPSGrammar(QPSVisitor *rqp)
Definition: QPSParser.cpp:444
const Symbol key1('v', 1)
constraint_v IG
Definition: QPSParser.cpp:72
m row(1)
constraint_v E
Definition: QPSParser.cpp:71
static string fromChars(const FusionVector &vars)
Definition: QPSParser.cpp:52
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
void addRHSDouble(boost::fusion::vector< Chars, Chars, Chars, Chars, Chars, double, Chars, Chars, Chars, double > const &vars)
Definition: QPSParser.cpp:191
std::unordered_map< std::string, double > b
Definition: QPSParser.cpp:76
std::unordered_map< std::string, double > ranges
Definition: QPSParser.cpp:79
std::unordered_map< Key, double > fx
Definition: QPSParser.cpp:93
std::function< void(bf::vector< Chars, Chars, Chars, Chars, Chars, double, Chars, Chars, Chars, double >)> rangeDouble
Definition: QPSParser.cpp:428
std::function< void(bf::vector< Chars, Chars, Chars, Chars, Chars, double, Chars > const &)> rhsSingle
Definition: QPSParser.cpp:419
traits
Definition: chartTesting.h:28
Factor graphs of a Quadratic Programming problem.
void addRHS(boost::fusion::vector< Chars, Chars, Chars, Chars, Chars, double, Chars > const &vars)
Definition: QPSParser.cpp:174
InequalityFactorGraph inequalities
linear inequality constraints: cI(x) <= 0
Definition: QP.h:34
QPS parser implementation.
qi::rule< boost::spirit::basic_istream_iterator< char >, Chars()> word
Definition: QPSParser.cpp:505
DiscreteKey E(5, 2)
void addColumnDouble(boost::fusion::vector< Chars, Chars, Chars, Chars, double, Chars, Chars, Chars, double > const &vars)
Definition: QPSParser.cpp:126
std::unordered_map< std::string, Key > varname_to_key
Definition: QPSParser.cpp:82
EqualityFactorGraph equalities
linear equality constraints: cE(x) = 0
Definition: QP.h:33
Double_ range(const Point2_ &p, const Point2_ &q)
static double range1(const Camera &camera, const Pose3 &pose)
static EIGEN_DEPRECATED const end_t end
void addRangeSingle(boost::fusion::vector< Chars, Chars, Chars, Chars, Chars, double, Chars > const &vars)
Definition: QPSParser.cpp:146
std::vector< char > Chars
Definition: QPSParser.cpp:412
std::unordered_map< Key, std::unordered_map< Key, Matrix11 > > H
Definition: QPSParser.cpp:84
std::function< void(bf::vector< Chars, Chars, Chars, Chars, Chars, double, Chars > const &)> rangeSingle
Definition: QPSParser.cpp:425
A Gaussian factor using the canonical parameters (information form)
std::function< void(bf::vector< Chars, Chars, Chars, Chars, Chars, double, Chars > const &)> addQuadTerm
Definition: QPSParser.cpp:437
qi::rule< boost::spirit::basic_istream_iterator< char >, char()> character
Definition: QPSParser.cpp:504
Annotation for function names.
Definition: attr.h:48
std::unordered_map< Key, double > lo
Definition: QPSParser.cpp:91
std::unordered_map< std::string, coefficient_v > constraint_v
Definition: QPSParser.cpp:67
const double fx
Definition: QP.h:31
const KeyVector keys
FastVector< Key > KeyVector
Define collection type once and for all - also used in wrappers.
Definition: Key.h:86
void addColumn(boost::fusion::vector< Chars, Chars, Chars, Chars, Chars, double, Chars > const &vars)
Definition: QPSParser.cpp:108
def parse(input_path, output_path, quiet=False, generate_xml_flag=True)
Definition: parse_xml.py:10
std::unordered_map< Key, double > up
Definition: QPSParser.cpp:89
std::uint64_t Key
Integer nonlinear key type.
Definition: types.h:102
std::function< void(bf::vector< Chars, Chars, Chars, Chars, Chars, double, Chars, Chars, Chars, double >)> rhsDouble
Definition: QPSParser.cpp:422
std::ptrdiff_t j
std::string obj_name
Definition: QPSParser.cpp:86
std::unordered_map< Key, Vector1 > g
Definition: QPSParser.cpp:80
std::unordered_map< std::string, constraint_v * > row_to_constraint_v
Definition: QPSParser.cpp:70
std::vector< char > Chars
Definition: QPSParser.cpp:48
void setName(boost::fusion::vector< Chars, Chars, Chars > const &name)
Definition: QPSParser.cpp:101


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:35:26