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