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


gtsam
Author(s):
autogenerated on Sat May 8 2021 02:43:44