Cleaned up QPSVisitor and fixed KeyVector compile issue on Ubuntu
Frank Dellaert
parent
a450ba1e55
commit
31556ff981
|
|
@ -12,37 +12,48 @@
|
|||
/**
|
||||
* @file QPSVisitor.cpp
|
||||
* @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
|
||||
* QP Constraints and cost. This 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.
|
||||
* 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 QP Constraints and cost. This
|
||||
* 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
|
||||
* @date 3/5/16
|
||||
*/
|
||||
|
||||
#include <gtsam_unstable/linear/QPSVisitor.h>
|
||||
|
||||
#include <algorithm>
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
|
||||
using boost::fusion::at_c;
|
||||
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 {
|
||||
|
||||
void QPSVisitor::setName(
|
||||
boost::fusion::vector<vector<char>, vector<char>,
|
||||
vector<char>> const &name) {
|
||||
name_ = string(at_c < 1 > (name).begin(), at_c < 1 > (name).end());
|
||||
boost::fusion::vector<Chars, Chars, Chars> const &name) {
|
||||
name_ = fromChars<1>(name);
|
||||
if (debug) {
|
||||
cout << "Parsing file: " << name_ << endl;
|
||||
}
|
||||
}
|
||||
|
||||
void QPSVisitor::addColumn(
|
||||
boost::fusion::vector<vector<char>, vector<char>,
|
||||
vector<char>, vector<char>, vector<char>, double,
|
||||
vector<char>> const &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());
|
||||
boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
|
||||
Chars> const &vars) {
|
||||
string var_ = fromChars<1>(vars);
|
||||
string row_ = fromChars<3>(vars);
|
||||
Matrix11 coefficient = at_c<5>(vars) * I_1x1;
|
||||
if (debug) {
|
||||
cout << "Added Column for Var: " << var_ << " Row: " << row_
|
||||
|
|
@ -58,13 +69,11 @@ void QPSVisitor::addColumn(
|
|||
}
|
||||
|
||||
void QPSVisitor::addColumnDouble(
|
||||
boost::fusion::vector<vector<char>, vector<char>,
|
||||
vector<char>, vector<char>, double, vector<char>,
|
||||
vector<char>, vector<char>, double> const &vars) {
|
||||
|
||||
string var_(at_c < 0 > (vars).begin(), at_c < 0 > (vars).end());
|
||||
string row1_(at_c < 2 > (vars).begin(), at_c < 2 > (vars).end());
|
||||
string row2_(at_c < 6 > (vars).begin(), at_c < 6 > (vars).end());
|
||||
boost::fusion::vector<Chars, Chars, Chars, Chars, double, Chars, Chars,
|
||||
Chars, double> const &vars) {
|
||||
string var_ = fromChars<0>(vars);
|
||||
string row1_ = fromChars<2>(vars);
|
||||
string row2_ = fromChars<6>(vars);
|
||||
Matrix11 coefficient1 = at_c<4>(vars) * I_1x1;
|
||||
Matrix11 coefficient2 = at_c<8>(vars) * I_1x1;
|
||||
if (!varname_to_key.count(var_))
|
||||
|
|
@ -80,27 +89,23 @@ void QPSVisitor::addColumnDouble(
|
|||
}
|
||||
|
||||
void QPSVisitor::addRangeSingle(
|
||||
boost::fusion::vector<vector<char>, vector<char>,
|
||||
vector<char>, vector<char>, vector<char>, double,
|
||||
vector<char>> const & 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());
|
||||
boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
|
||||
Chars> const &vars) {
|
||||
string var_ = fromChars<1>(vars);
|
||||
string row_ = fromChars<3>(vars);
|
||||
double range = at_c<5>(vars);
|
||||
ranges[row_] = range;
|
||||
if (debug) {
|
||||
cout << "SINGLE RANGE ADDED" << endl;
|
||||
cout << "VAR:" << var_ << " ROW: " << row_ << " RANGE: " << range
|
||||
<< endl;
|
||||
cout << "VAR:" << var_ << " ROW: " << row_ << " RANGE: " << range << endl;
|
||||
}
|
||||
|
||||
}
|
||||
void QPSVisitor::addRangeDouble(
|
||||
boost::fusion::vector<vector<char>, vector<char>,
|
||||
vector<char>, vector<char>, vector<char>, double,
|
||||
vector<char>, vector<char>, vector<char>, double> const & vars) {
|
||||
string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
|
||||
string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
|
||||
string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end());
|
||||
boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
|
||||
Chars, Chars, double> const &vars) {
|
||||
string var_ = fromChars<1>(vars);
|
||||
string row1_ = fromChars<3>(vars);
|
||||
string row2_ = fromChars<7>(vars);
|
||||
double range1 = at_c<5>(vars);
|
||||
double range2 = at_c<9>(vars);
|
||||
ranges[row1_] = range1;
|
||||
|
|
@ -110,16 +115,12 @@ void QPSVisitor::addRangeDouble(
|
|||
cout << "VAR: " << var_ << " ROW1: " << row1_ << " RANGE1: " << range1
|
||||
<< " ROW2: " << row2_ << " RANGE2: " << range2 << endl;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void QPSVisitor::addRHS(
|
||||
boost::fusion::vector<vector<char>, vector<char>,
|
||||
vector<char>, vector<char>, vector<char>, double,
|
||||
vector<char>> const &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());
|
||||
void QPSVisitor::addRHS(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
|
||||
double, Chars> const &vars) {
|
||||
string var_ = fromChars<1>(vars);
|
||||
string row_ = fromChars<3>(vars);
|
||||
double coefficient = at_c<5>(vars);
|
||||
if (row_ == obj_name)
|
||||
f = -coefficient;
|
||||
|
|
@ -133,13 +134,11 @@ void QPSVisitor::addRHS(
|
|||
}
|
||||
|
||||
void QPSVisitor::addRHSDouble(
|
||||
boost::fusion::vector<vector<char>, vector<char>,
|
||||
vector<char>, vector<char>, vector<char>, double,
|
||||
vector<char>, vector<char>, vector<char>, double> const &vars) {
|
||||
|
||||
string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
|
||||
string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
|
||||
string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end());
|
||||
boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
|
||||
Chars, Chars, double> const &vars) {
|
||||
string var_ = fromChars<1>(vars);
|
||||
string row1_ = fromChars<3>(vars);
|
||||
string row2_ = fromChars<7>(vars);
|
||||
double coefficient1 = at_c<5>(vars);
|
||||
double coefficient2 = at_c<9>(vars);
|
||||
if (row1_ == obj_name)
|
||||
|
|
@ -155,16 +154,14 @@ void QPSVisitor::addRHSDouble(
|
|||
if (debug) {
|
||||
cout << "Added RHS for Var: " << var_ << " Row: " << row1_
|
||||
<< " Coefficient: " << coefficient1 << endl;
|
||||
cout << " " << "Row: " << row2_
|
||||
<< " Coefficient: " << coefficient2 << endl;
|
||||
cout << " "
|
||||
<< "Row: " << row2_ << " Coefficient: " << coefficient2 << endl;
|
||||
}
|
||||
}
|
||||
|
||||
void QPSVisitor::addRow(
|
||||
boost::fusion::vector<vector<char>, char, vector<char>,
|
||||
vector<char>, vector<char>> const &vars) {
|
||||
|
||||
string name_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
|
||||
boost::fusion::vector<Chars, char, Chars, Chars, Chars> const &vars) {
|
||||
string name_ = fromChars<3>(vars);
|
||||
char type = at_c<1>(vars);
|
||||
switch (type) {
|
||||
case 'N':
|
||||
|
|
@ -189,12 +186,10 @@ void QPSVisitor::addRow(
|
|||
}
|
||||
|
||||
void QPSVisitor::addBound(
|
||||
boost::fusion::vector<vector<char>, vector<char>,
|
||||
vector<char>, vector<char>, vector<char>,
|
||||
vector<char>, vector<char>, double> const &vars) {
|
||||
|
||||
string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
|
||||
string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end());
|
||||
boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars,
|
||||
double> const &vars) {
|
||||
string type_ = fromChars<1>(vars);
|
||||
string var_ = fromChars<5>(vars);
|
||||
double number = at_c<7>(vars);
|
||||
if (type_.compare(string("UP")) == 0)
|
||||
up[varname_to_key[var_]] = number;
|
||||
|
|
@ -212,12 +207,11 @@ void QPSVisitor::addBound(
|
|||
}
|
||||
|
||||
void QPSVisitor::addFreeBound(
|
||||
boost::fusion::vector<vector<char>, vector<char>,
|
||||
vector<char>, vector<char>, vector<char>,
|
||||
vector<char>, vector<char>> const &vars) {
|
||||
string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
|
||||
string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end());
|
||||
Free.push_back(varname_to_key[var_]);
|
||||
boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars> const
|
||||
&vars) {
|
||||
string type_ = fromChars<1>(vars);
|
||||
string var_ = fromChars<5>(vars);
|
||||
free.push_back(varname_to_key[var_]);
|
||||
if (debug) {
|
||||
cout << "Added Free Bound Type: " << type_ << " Var: " << var_
|
||||
<< " Amount: " << endl;
|
||||
|
|
@ -225,11 +219,10 @@ void QPSVisitor::addFreeBound(
|
|||
}
|
||||
|
||||
void QPSVisitor::addQuadTerm(
|
||||
boost::fusion::vector<vector<char>, vector<char>,
|
||||
vector<char>, vector<char>, vector<char>, double,
|
||||
vector<char>> const &vars) {
|
||||
string var1_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
|
||||
string var2_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
|
||||
boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
|
||||
Chars> const &vars) {
|
||||
string var1_ = fromChars<1>(vars);
|
||||
string var2_ = fromChars<3>(vars);
|
||||
Matrix11 coefficient = at_c<5>(vars) * I_1x1;
|
||||
|
||||
H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient;
|
||||
|
|
@ -242,7 +235,7 @@ void QPSVisitor::addQuadTerm(
|
|||
|
||||
QP QPSVisitor::makeQP() {
|
||||
// Create the keys from the variable names
|
||||
vector < Key > keys;
|
||||
KeyVector keys;
|
||||
for (auto kv : varname_to_key) {
|
||||
keys.push_back(kv.second);
|
||||
}
|
||||
|
|
@ -251,12 +244,11 @@ QP QPSVisitor::makeQP() {
|
|||
vector<Matrix> Gs;
|
||||
vector<Vector> gs;
|
||||
sort(keys.begin(), keys.end());
|
||||
for (unsigned int i = 0; i < keys.size(); ++i) {
|
||||
for (unsigned int j = i; j < keys.size(); ++j) {
|
||||
if (H.count(keys[i]) > 0 and H[keys[i]].count(keys[j]) > 0){
|
||||
for (size_t i = 0; i < keys.size(); ++i) {
|
||||
for (size_t j = i; j < keys.size(); ++j) {
|
||||
if (H.count(keys[i]) > 0 && H[keys[i]].count(keys[j]) > 0) {
|
||||
Gs.emplace_back(H[keys[i]][keys[j]]);
|
||||
}
|
||||
else{
|
||||
} else {
|
||||
Gs.emplace_back(Z_1x1);
|
||||
}
|
||||
}
|
||||
|
|
@ -264,8 +256,7 @@ QP QPSVisitor::makeQP() {
|
|||
for (Key key1 : keys) {
|
||||
if (g.count(key1) > 0) {
|
||||
gs.emplace_back(-g[key1]);
|
||||
}
|
||||
else{
|
||||
} else {
|
||||
gs.emplace_back(Z_1x1);
|
||||
}
|
||||
}
|
||||
|
|
@ -289,15 +280,13 @@ QP QPSVisitor::makeQP() {
|
|||
if (ranges[kv.first] > 0) {
|
||||
madeQP.inequalities.push_back(
|
||||
LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++));
|
||||
madeQP.inequalities.push_back(
|
||||
LinearInequality(keyMatrixMapPos, b[kv.first] + ranges[kv.first],
|
||||
dual_key_num++));
|
||||
madeQP.inequalities.push_back(LinearInequality(
|
||||
keyMatrixMapPos, b[kv.first] + ranges[kv.first], dual_key_num++));
|
||||
} else if (ranges[kv.first] < 0) {
|
||||
madeQP.inequalities.push_back(
|
||||
LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++));
|
||||
madeQP.inequalities.push_back(
|
||||
LinearInequality(keyMatrixMapNeg, ranges[kv.first] - b[kv.first],
|
||||
dual_key_num++));
|
||||
madeQP.inequalities.push_back(LinearInequality(
|
||||
keyMatrixMapNeg, ranges[kv.first] - b[kv.first], dual_key_num++));
|
||||
} else {
|
||||
cerr << "ERROR: CANNOT ADD A RANGE OF ZERO" << endl;
|
||||
throw;
|
||||
|
|
@ -323,9 +312,8 @@ QP QPSVisitor::makeQP() {
|
|||
madeQP.inequalities.push_back(
|
||||
LinearInequality(keyMatrixMapNeg, -b[kv.first], dual_key_num++));
|
||||
if (ranges.count(kv.first) == 1) {
|
||||
madeQP.inequalities.push_back(
|
||||
LinearInequality(keyMatrixMapPos, b[kv.first] + ranges[kv.first],
|
||||
dual_key_num++));
|
||||
madeQP.inequalities.push_back(LinearInequality(
|
||||
keyMatrixMapPos, b[kv.first] + ranges[kv.first], dual_key_num++));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -340,15 +328,13 @@ QP QPSVisitor::makeQP() {
|
|||
madeQP.inequalities.push_back(
|
||||
LinearInequality(keyMatrixMapPos, b[kv.first], dual_key_num++));
|
||||
if (ranges.count(kv.first) == 1) {
|
||||
madeQP.inequalities.push_back(
|
||||
LinearInequality(keyMatrixMapNeg, ranges[kv.first] - b[kv.first],
|
||||
dual_key_num++));
|
||||
madeQP.inequalities.push_back(LinearInequality(
|
||||
keyMatrixMapNeg, ranges[kv.first] - b[kv.first], dual_key_num++));
|
||||
}
|
||||
}
|
||||
|
||||
for (Key k : keys) {
|
||||
if (find(Free.begin(), Free.end(), k) != Free.end())
|
||||
continue;
|
||||
if (find(free.begin(), free.end(), k) != free.end()) continue;
|
||||
if (fx.count(k) == 1)
|
||||
madeQP.equalities.push_back(
|
||||
LinearEquality(k, I_1x1, fx[k] * I_1x1, dual_key_num++));
|
||||
|
|
@ -364,5 +350,4 @@ QP QPSVisitor::makeQP() {
|
|||
}
|
||||
return madeQP;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -18,14 +18,16 @@
|
|||
|
||||
#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/inference/Key.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam_unstable/linear/QP.h>
|
||||
|
||||
#include <boost/fusion/include/vector.hpp>
|
||||
#include <boost/fusion/sequence.hpp>
|
||||
#include <unordered_map>
|
||||
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -54,67 +56,50 @@ private:
|
|||
std::unordered_map<Key, double> up; // Upper Bound constraints on variable where X < MAX
|
||||
std::unordered_map<Key, double> lo; // Lower Bound constraints on variable where MIN < X
|
||||
std::unordered_map<Key, double> fx; // Equalities specified as FX in BOUNDS part of QPS
|
||||
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;
|
||||
|
||||
using Chars = std::vector<char>;
|
||||
|
||||
public:
|
||||
QPSVisitor() : numVariables(1) {
|
||||
}
|
||||
QPSVisitor() : numVariables(1) {}
|
||||
|
||||
void setName(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>> const & name);
|
||||
void setName(boost::fusion::vector<Chars, Chars, Chars> const& name);
|
||||
|
||||
void addColumn(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>> const & vars);
|
||||
void addColumn(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
|
||||
double, Chars> const& vars);
|
||||
|
||||
void addColumnDouble(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, double, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, double> const & vars);
|
||||
boost::fusion::vector<Chars, Chars, Chars, Chars, double, Chars, Chars,
|
||||
Chars, double> const& vars);
|
||||
|
||||
void addRHS(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>> const & vars);
|
||||
void addRHS(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double,
|
||||
Chars> const& vars);
|
||||
|
||||
void addRHSDouble(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double> const & vars);
|
||||
boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
|
||||
Chars, Chars, double> const& vars);
|
||||
|
||||
void addRangeSingle(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>> const & vars);
|
||||
void addRangeSingle(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
|
||||
double, Chars> const& vars);
|
||||
|
||||
void addRangeDouble(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double> const & vars);
|
||||
boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, double, Chars,
|
||||
Chars, Chars, double> const& vars);
|
||||
|
||||
void addRow(
|
||||
boost::fusion::vector<std::vector<char>, char, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>> const & vars);
|
||||
boost::fusion::vector<Chars, char, Chars, Chars, Chars> const& vars);
|
||||
|
||||
void addBound(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, double> const & vars);
|
||||
void addBound(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars, Chars,
|
||||
Chars, double> const& vars);
|
||||
|
||||
void addFreeBound(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>> const & vars);
|
||||
void addFreeBound(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
|
||||
Chars, Chars> const& vars);
|
||||
|
||||
void addQuadTerm(
|
||||
boost::fusion::vector<std::vector<char>, std::vector<char>,
|
||||
std::vector<char>, std::vector<char>, std::vector<char>, double,
|
||||
std::vector<char>> const & vars);
|
||||
void addQuadTerm(boost::fusion::vector<Chars, Chars, Chars, Chars, Chars,
|
||||
double, Chars> const& vars);
|
||||
|
||||
QP makeQP();
|
||||
}
|
||||
;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
Loading…
Reference in New Issue