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

Style improvements and comments.

release/4.3a0
= 2018-11-04 16:16:13 -05:00
parent b851c498b7
commit df025e68eb
5 changed files with 407 additions and 404 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
gtsam_unstable/linear/QPSParser.cpp
View File

@ -19,7 +19,7 @@
#include <gtsam_unstable/linear/QPSParser.h> #include <gtsam_unstable/linear/QPSParser.h>
#include <gtsam_unstable/linear/QPSParserException.h> #include <gtsam_unstable/linear/QPSParserException.h>
#include <gtsam_unstable/linear/RawQP.h> #include <gtsam_unstable/linear/QPSVisitor.h>
#include <boost/spirit/include/qi.hpp> #include <boost/spirit/include/qi.hpp>
#include <boost/lambda/lambda.hpp> #include <boost/lambda/lambda.hpp>
@ -34,7 +34,7 @@ typedef qi::grammar<boost::spirit::basic_istream_iterator<char>> base_grammar;
struct QPSParser::MPSGrammar: base_grammar { struct QPSParser::MPSGrammar: base_grammar {
typedef std::vector<char> Chars; typedef std::vector<char> Chars;
RawQP * rqp_; QPSVisitor * rqp_;
boost::function<void(bf::vector<Chars, Chars, Chars> const&)> setName; boost::function<void(bf::vector<Chars, Chars, Chars> const&)> setName;
boost::function<void(bf::vector<Chars, char, Chars, Chars, Chars> const &)> addRow; boost::function<void(bf::vector<Chars, char, Chars, Chars, Chars> const &)> addRow;
boost::function< boost::function<
@ -62,19 +62,19 @@ struct QPSParser::MPSGrammar: base_grammar {
bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars, double> const &)> addBound; bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars, double> const &)> addBound;
boost::function< boost::function<
void(bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars> const &)> addBoundFr; void(bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars> const &)> addBoundFr;
MPSGrammar(RawQP * rqp) : MPSGrammar(QPSVisitor * rqp) :
base_grammar(start), rqp_(rqp), setName( base_grammar(start), rqp_(rqp), setName(
boost::bind(&RawQP::setName, rqp, ::_1)), addRow( boost::bind(&QPSVisitor::setName, rqp, ::_1)), addRow(
boost::bind(&RawQP::addRow, rqp, ::_1)), rhsSingle( boost::bind(&QPSVisitor::addRow, rqp, ::_1)), rhsSingle(
boost::bind(&RawQP::addRHS, rqp, ::_1)), rhsDouble( boost::bind(&QPSVisitor::addRHS, rqp, ::_1)), rhsDouble(
boost::bind(&RawQP::addRHSDouble, rqp, ::_1)), rangeSingle( boost::bind(&QPSVisitor::addRHSDouble, rqp, ::_1)), rangeSingle(
boost::bind(&RawQP::addRangeSingle, rqp, ::_1)), rangeDouble( boost::bind(&QPSVisitor::addRangeSingle, rqp, ::_1)), rangeDouble(
boost::bind(&RawQP::addRangeDouble, rqp, ::_1)), colSingle( boost::bind(&QPSVisitor::addRangeDouble, rqp, ::_1)), colSingle(
boost::bind(&RawQP::addColumn, rqp, ::_1)), colDouble( boost::bind(&QPSVisitor::addColumn, rqp, ::_1)), colDouble(
boost::bind(&RawQP::addColumnDouble, rqp, ::_1)), addQuadTerm( boost::bind(&QPSVisitor::addColumnDouble, rqp, ::_1)), addQuadTerm(
boost::bind(&RawQP::addQuadTerm, rqp, ::_1)), addBound( boost::bind(&QPSVisitor::addQuadTerm, rqp, ::_1)), addBound(
boost::bind(&RawQP::addBound, rqp, ::_1)), addBoundFr( boost::bind(&QPSVisitor::addBound, rqp, ::_1)), addBoundFr(
boost::bind(&RawQP::addBoundFr, rqp, ::_1)) { boost::bind(&QPSVisitor::addBoundFr, rqp, ::_1)) {
using namespace boost::spirit; using namespace boost::spirit;
using namespace boost::spirit::qi; using namespace boost::spirit::qi;
character = lexeme[alnum | '_' | '-' | '.']; character = lexeme[alnum | '_' | '-' | '.'];
@ -123,7 +123,7 @@ struct QPSParser::MPSGrammar: base_grammar {
}; };
QP QPSParser::Parse() { QP QPSParser::Parse() {
RawQP rawData; QPSVisitor rawData;
std::fstream stream(fileName_.c_str()); std::fstream stream(fileName_.c_str());
stream.unsetf(std::ios::skipws); stream.unsetf(std::ios::skipws);
boost::spirit::basic_istream_iterator<char> begin(stream); boost::spirit::basic_istream_iterator<char> begin(stream);

363
gtsam_unstable/linear/QPSVisitor.cpp Normal file
View File

@ -0,0 +1,363 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @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.
* @author Ivan Dario Jimenez
* @date 3/5/16
*/
#include <gtsam_unstable/linear/QPSVisitor.h>
#include <iostream>
using boost::fusion::at_c;
using namespace std;
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());
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());
Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
if (debug) {
cout << "Added Column for Var: " << var_ << " Row: " << row_
<< " Coefficient: " << coefficient << endl;
}
if (!varname_to_key.count(var_))
varname_to_key[var_] = Symbol('X', numVariables++);
if (row_ == obj_name) {
g[varname_to_key[var_]] = coefficient;
return;
}
(*row_to_constraint_v[row_])[row_][varname_to_key[var_]] = coefficient;
}
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());
Matrix11 coefficient1 = at_c < 4 > (vars) * I_1x1;
Matrix11 coefficient2 = at_c < 8 > (vars) * I_1x1;
if (!varname_to_key.count(var_))
varname_to_key.insert( { var_, Symbol('X', numVariables++) });
if (row1_ == obj_name)
g[varname_to_key[var_]] = coefficient1;
else
(*row_to_constraint_v[row1_])[row1_][varname_to_key[var_]] = coefficient1;
if (row2_ == obj_name)
g[varname_to_key[var_]] = coefficient2;
else
(*row_to_constraint_v[row2_])[row2_][varname_to_key[var_]] = coefficient2;
}
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());
double range = at_c < 5 > (vars);
ranges[row_] = range;
if (debug) {
cout << "SINGLE RANGE ADDED" << 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());
double range1 = at_c < 5 > (vars);
double range2 = at_c < 9 > (vars);
ranges[row1_] = range1;
ranges[row2_] = range2;
if (debug) {
cout << "DOUBLE RANGE ADDED" << endl;
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());
double coefficient = at_c < 5 > (vars);
if (row_ == obj_name)
f = -coefficient;
else
b[row_] = coefficient;
if (debug) {
cout << "Added RHS for Var: " << var_ << " Row: " << row_
<< " Coefficient: " << coefficient << endl;
}
}
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());
double coefficient1 = at_c < 5 > (vars);
double coefficient2 = at_c < 9 > (vars);
if (row1_ == obj_name)
f = -coefficient1;
else
b[row1_] = coefficient1;
if (row2_ == obj_name)
f = -coefficient2;
else
b[row2_] = coefficient2;
if (debug) {
cout << "Added RHS for Var: " << var_ << " Row: " << row1_
<< " Coefficient: " << coefficient1 << 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());
char type = at_c < 1 > (vars);
switch (type) {
case 'N':
obj_name = name_;
break;
case 'L':
row_to_constraint_v[name_] = &IL;
break;
case 'G':
row_to_constraint_v[name_] = &IG;
break;
case 'E':
row_to_constraint_v[name_] = &E;
break;
default:
cout << "invalid type: " << type << endl;
break;
}
if (debug) {
cout << "Added Row Type: " << type << " Name: " << name_ << endl;
}
}
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());
double number = at_c < 7 > (vars);
if (type_.compare(string("UP")) == 0)
up[varname_to_key[var_]] = number;
else if (type_.compare(string("LO")) == 0)
lo[varname_to_key[var_]] = number;
else if (type_.compare(string("FX")) == 0)
fx[varname_to_key[var_]] = number;
else
cout << "Invalid Bound Type: " << type_ << endl;
if (debug) {
cout << "Added Bound Type: " << type_ << " Var: " << var_
<< " Amount: " << number << endl;
}
}
void QPSVisitor::addBoundFr(
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_]);
if (debug) {
cout << "Added Free Bound Type: " << type_ << " Var: " << var_
<< " Amount: " << endl;
}
}
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());
Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient;
H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient;
if (debug) {
cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_
<< " Coefficient: " << coefficient << endl;
}
}
QP QPSVisitor::makeQP() {
vector < Key > keys;
vector < Matrix > Gs;
vector < Vector > gs;
for (auto kv : varname_to_key) {
keys.push_back(kv.second);
}
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){
Gs.emplace_back(H[keys[i]][keys[j]]);
}
else{
Gs.emplace_back(Z_1x1);
}
}
}
for (Key key1 : keys) {
if(g.count(key1) > 0){
gs.emplace_back(-g[key1]);
}
else{
gs.emplace_back(Z_1x1);
}
}
size_t dual_key_num = keys.size() + 1;
QP madeQP;
auto obj = HessianFactor(keys, Gs, gs, 2 * f);
madeQP.cost.push_back(obj);
for (auto kv : E) {
map < Key, Matrix11 > keyMatrixMapPos;
map < Key, Matrix11 > keyMatrixMapNeg;
if (ranges.count(kv.first) == 1) {
for (auto km : kv.second) {
keyMatrixMapPos.insert(km);
km.second = -km.second;
keyMatrixMapNeg.insert(km);
}
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++));
} 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++));
} else {
cerr << "ERROR: CANNOT ADD A RANGE OF ZERO" << endl;
throw;
}
continue;
}
map < Key, Matrix11 > keyMatrixMap;
for (auto km : kv.second) {
keyMatrixMap.insert(km);
}
madeQP.equalities.push_back(
LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++));
}
for (auto kv : IG) {
map < Key, Matrix11 > keyMatrixMapNeg;
map < Key, Matrix11 > keyMatrixMapPos;
for (auto km : kv.second) {
keyMatrixMapPos.insert(km);
km.second = -km.second;
keyMatrixMapNeg.insert(km);
}
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++));
}
}
for (auto kv : IL) {
map < Key, Matrix11 > keyMatrixMapPos;
map < Key, Matrix11 > keyMatrixMapNeg;
for (auto km : kv.second) {
keyMatrixMapPos.insert(km);
km.second = -km.second;
keyMatrixMapNeg.insert(km);
}
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++));
}
}
for (Key k : keys) {
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++));
if (up.count(k) == 1)
madeQP.inequalities.push_back(
LinearInequality(k, I_1x1, up[k], dual_key_num++));
if (lo.count(k) == 1)
madeQP.inequalities.push_back(
LinearInequality(k, -I_1x1, -lo[k], dual_key_num++));
else
madeQP.inequalities.push_back(
LinearInequality(k, -I_1x1, 0, dual_key_num++));
}
return madeQP;
}
}

56
gtsam_unstable/linear/RawQP.h → gtsam_unstable/linear/QPSVisitor.h
View File

@ -10,7 +10,7 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* @file RawQP.h * @file QPSVisitor.h
* @brief * @brief
* @author Ivan Dario Jimenez * @author Ivan Dario Jimenez
* @date 3/5/16 * @date 3/5/16
@ -19,48 +19,46 @@
#pragma once #pragma once
#include <gtsam_unstable/linear/QP.h> #include <gtsam_unstable/linear/QP.h>
#include <gtsam/base/Matrix.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/base/Matrix.h>
#include <boost/fusion/include/vector.hpp>
#include <boost/fusion/sequence.hpp>
#include <unordered_map>
#include <string> #include <string>
#include <vector> #include <vector>
#include <unordered_map>
#include <gtsam/inference/Symbol.h>
#include <boost/fusion/sequence.hpp>
#include <boost/fusion/include/vector.hpp>
namespace gtsam { namespace gtsam {
/** /**
* This class is responsible for collecting a QP problem as the parser parses a QPS file * As the parser reads a file, it call functions in this visitor. This visitor in turn stores what the parser has read
* and then generating a QP problem. * in a way that can be later used to build the full QP problem in the file.
*/ */
class RawQP { 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; std::unordered_map<std::string, constraint_v*> row_to_constraint_v; // Maps QPS ROWS to Variable-Matrix pairs
constraint_v E; constraint_v E; // Equalities
constraint_v IG; constraint_v IG;// Inequalities >=
constraint_v IL; constraint_v IL;// Inequalities <=
unsigned int varNumber; unsigned int numVariables;
std::unordered_map<std::string, double> b; 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; std::unordered_map<std::string, double> ranges; // Inequalities can be specified as ranges on a variable
std::unordered_map<Key, Vector1> g; std::unordered_map<Key, Vector1> g; // linear term of quadratic cost
std::unordered_map<std::string, Key> varname_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; std::unordered_map<Key, std::unordered_map<Key, Matrix11> > H; // H from hessian
double f; double f; // Constant term of quadratic cost
std::string obj_name; std::string obj_name; // the objective function has a name in the QPS
std::string name_; std::string name_; // the quadratic program has a name in the QPS
std::unordered_map<Key, double> up; std::unordered_map<Key, double> up; // Upper Bound constraints on variable where X < MAX
std::unordered_map<Key, double> lo; std::unordered_map<Key, double> lo; // Lower Bound constraints on variable where MIN < X
std::unordered_map<Key, double> fx; std::unordered_map<Key, double> fx; // Equalities specified as FX in BOUNDS part of QPS
std::vector<Key> Free; std::vector<Key> Free; // Variables can be specified as Free (to which no constraints apply)
const bool debug = false; const bool debug = false;
public: public:
RawQP() : QPSVisitor() : numVariables(1) {
row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), ranges(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), fx(), Free() {
} }
void setName( void setName(

359
gtsam_unstable/linear/RawQP.cpp
View File

@ -1,359 +0,0 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file RawQP.cpp
* @brief
* @author Ivan Dario Jimenez
* @date 3/5/16
*/
#include <gtsam_unstable/linear/RawQP.h>
#include <iostream>
using boost::fusion::at_c;
namespace gtsam {
void RawQP::setName(
boost::fusion::vector<std::vector<char>, std::vector<char>,
std::vector<char>> const &name) {
name_ = std::string(at_c < 1 > (name).begin(), at_c < 1 > (name).end());
if (debug) {
std::cout << "Parsing file: " << name_ << std::endl;
}
}
void RawQP::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) {
std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
if (debug) {
std::cout << "Added Column for Var: " << var_ << " Row: " << row_
<< " Coefficient: " << coefficient << std::endl;
}
if (!varname_to_key.count(var_))
varname_to_key[var_] = Symbol('X', varNumber++);
if (row_ == obj_name) {
g[varname_to_key[var_]] = coefficient;
return;
}
(*row_to_constraint_v[row_])[row_][varname_to_key[var_]] = coefficient;
}
void RawQP::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) {
std::string var_(at_c < 0 > (vars).begin(), at_c < 0 > (vars).end());
std::string row1_(at_c < 2 > (vars).begin(), at_c < 2 > (vars).end());
std::string row2_(at_c < 6 > (vars).begin(), at_c < 6 > (vars).end());
Matrix11 coefficient1 = at_c < 4 > (vars) * I_1x1;
Matrix11 coefficient2 = at_c < 8 > (vars) * I_1x1;
if (!varname_to_key.count(var_))
varname_to_key.insert( { var_, Symbol('X', varNumber++) });
if (row1_ == obj_name)
g[varname_to_key[var_]] = coefficient1;
else
(*row_to_constraint_v[row1_])[row1_][varname_to_key[var_]] = coefficient1;
if (row2_ == obj_name)
g[varname_to_key[var_]] = coefficient2;
else
(*row_to_constraint_v[row2_])[row2_][varname_to_key[var_]] = coefficient2;
}
void RawQP::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) {
std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
double range = at_c < 5 > (vars);
ranges[row_] = range;
if (debug) {
std::cout << "SINGLE RANGE ADDED" << std::endl;
std::cout << "VAR:" << var_ << " ROW: " << row_ << " RANGE: " << range
<< std::endl;
}
}
void RawQP::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) {
std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
std::string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
std::string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end());
double range1 = at_c < 5 > (vars);
double range2 = at_c < 9 > (vars);
ranges[row1_] = range1;
ranges[row2_] = range2;
if (debug) {
std::cout << "DOUBLE RANGE ADDED" << std::endl;
std::cout << "VAR: " << var_ << " ROW1: " << row1_ << " RANGE1: " << range1
<< " ROW2: " << row2_ << " RANGE2: " << range2 << std::endl;
}
}
void RawQP::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) {
std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
double coefficient = at_c < 5 > (vars);
if (row_ == obj_name)
f = -coefficient;
else
b[row_] = coefficient;
if (debug) {
std::cout << "Added RHS for Var: " << var_ << " Row: " << row_
<< " Coefficient: " << coefficient << std::endl;
}
}
void RawQP::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) {
std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
std::string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
std::string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end());
double coefficient1 = at_c < 5 > (vars);
double coefficient2 = at_c < 9 > (vars);
if (row1_ == obj_name)
f = -coefficient1;
else
b[row1_] = coefficient1;
if (row2_ == obj_name)
f = -coefficient2;
else
b[row2_] = coefficient2;
if (debug) {
std::cout << "Added RHS for Var: " << var_ << " Row: " << row1_
<< " Coefficient: " << coefficient1 << std::endl;
std::cout << " " << "Row: " << row2_
<< " Coefficient: " << coefficient2 << std::endl;
}
}
void RawQP::addRow(
boost::fusion::vector<std::vector<char>, char, std::vector<char>,
std::vector<char>, std::vector<char>> const &vars) {
std::string name_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
char type = at_c < 1 > (vars);
switch (type) {
case 'N':
obj_name = name_;
break;
case 'L':
row_to_constraint_v[name_] = &IL;
break;
case 'G':
row_to_constraint_v[name_] = &IG;
break;
case 'E':
row_to_constraint_v[name_] = &E;
break;
default:
std::cout << "invalid type: " << type << std::endl;
break;
}
if (debug) {
std::cout << "Added Row Type: " << type << " Name: " << name_ << std::endl;
}
}
void RawQP::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) {
std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end());
double number = at_c < 7 > (vars);
if (type_.compare(std::string("UP")) == 0)
up[varname_to_key[var_]] = number;
else if (type_.compare(std::string("LO")) == 0)
lo[varname_to_key[var_]] = number;
else if (type_.compare(std::string("FX")) == 0)
fx[varname_to_key[var_]] = number;
else
std::cout << "Invalid Bound Type: " << type_ << std::endl;
if (debug) {
std::cout << "Added Bound Type: " << type_ << " Var: " << var_
<< " Amount: " << number << std::endl;
}
}
void RawQP::addBoundFr(
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) {
std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end());
Free.push_back(varname_to_key[var_]);
if (debug) {
std::cout << "Added Free Bound Type: " << type_ << " Var: " << var_
<< " Amount: " << std::endl;
}
}
void RawQP::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) {
std::string var1_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end());
std::string var2_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end());
Matrix11 coefficient = at_c < 5 > (vars) * I_1x1;
H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient;
H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient;
if (debug) {
std::cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_
<< " Coefficient: " << coefficient << std::endl;
}
}
QP RawQP::makeQP() {
std::vector < Key > keys;
std::vector < Matrix > Gs;
std::vector < Vector > gs;
for (auto kv : varname_to_key) {
keys.push_back(kv.second);
}
std::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){
Gs.emplace_back(H[keys[i]][keys[j]]);
}
else{
Gs.emplace_back(Z_1x1);
}
}
}
for (Key key1 : keys) {
if(g.count(key1) > 0){
gs.emplace_back(-g[key1]);
}
else{
gs.emplace_back(Z_1x1);
}
}
size_t dual_key_num = keys.size() + 1;
QP madeQP;
auto obj = HessianFactor(keys, Gs, gs, 2 * f);
madeQP.cost.push_back(obj);
for (auto kv : E) {
std::map < Key, Matrix11 > keyMatrixMapPos;
std::map < Key, Matrix11 > keyMatrixMapNeg;
if (ranges.count(kv.first) == 1) {
for (auto km : kv.second) {
keyMatrixMapPos.insert(km);
km.second = -km.second;
keyMatrixMapNeg.insert(km);
}
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++));
} 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++));
} else {
std::cerr << "ERROR: CANNOT ADD A RANGE OF ZERO" << std::endl;
throw;
}
continue;
}
std::map < Key, Matrix11 > keyMatrixMap;
for (auto km : kv.second) {
keyMatrixMap.insert(km);
}
madeQP.equalities.push_back(
LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++));
}
for (auto kv : IG) {
std::map < Key, Matrix11 > keyMatrixMapNeg;
std::map < Key, Matrix11 > keyMatrixMapPos;
for (auto km : kv.second) {
keyMatrixMapPos.insert(km);
km.second = -km.second;
keyMatrixMapNeg.insert(km);
}
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++));
}
}
for (auto kv : IL) {
std::map < Key, Matrix11 > keyMatrixMapPos;
std::map < Key, Matrix11 > keyMatrixMapNeg;
for (auto km : kv.second) {
keyMatrixMapPos.insert(km);
km.second = -km.second;
keyMatrixMapNeg.insert(km);
}
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++));
}
}
for (Key k : keys) {
if (std::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++));
if (up.count(k) == 1)
madeQP.inequalities.push_back(
LinearInequality(k, I_1x1, up[k], dual_key_num++));
if (lo.count(k) == 1)
madeQP.inequalities.push_back(
LinearInequality(k, -I_1x1, -lo[k], dual_key_num++));
else
madeQP.inequalities.push_back(
LinearInequality(k, -I_1x1, 0, dual_key_num++));
}
return madeQP;
}
}

3
gtsam_unstable/linear/tests/testQPSolver.cpp
View File

@ -13,7 +13,8 @@
* @file testQPSolver.cpp * @file testQPSolver.cpp
* @brief Test simple QP solver for a linear inequality constraint * @brief Test simple QP solver for a linear inequality constraint
* @date Apr 10, 2014 * @date Apr 10, 2014
* @author Duy-Nguyen Ta, Ivan Dario Jimenez * @author Duy-Nguyen Ta
* @author Ivan Dario Jimenez
*/ */
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>