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

Merged in feature/LPSolver (pull request #277) 

Long awaited fixe of some QPSolver tests
Feature/lpsolver
release/4.3a0
Ivan Jimenez 2019-04-27 21:43:17 +00:00 committed by Frank Dellaert
parent 94225862f8 5f4cedec8a
commit a5844cbdcd
14 changed files with 767 additions and 460 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
examples/Data/HS21.QPS Normal file
View File

@ -0,0 +1,20 @@
NAME HS21
ROWS
N OBJ.FUNC
G R------1
COLUMNS
C------1 R------1 0.100000e+02
C------2 R------1 -.100000e+01
RHS
RHS OBJ.FUNC 0.100000e+03
RHS R------1 0.100000e+02
RANGES
BOUNDS
LO BOUNDS C------1 0.200000e+01
UP BOUNDS C------1 0.500000e+02
LO BOUNDS C------2 -.500000e+02
UP BOUNDS C------2 0.500000e+02
QUADOBJ
C------1 C------1 0.200000e-01
C------2 C------2 0.200000e+01
ENDATA

55
examples/Data/HS268.QPS Normal file
View File

@ -0,0 +1,55 @@
NAME HS268
ROWS
N OBJ.FUNC
G R------1
G R------2
G R------3
G R------4
G R------5
COLUMNS
C------1 OBJ.FUNC 0.183400e+05 R------1 -.100000e+01
C------1 R------2 0.100000e+02 R------3 -.800000e+01
C------1 R------4 0.800000e+01 R------5 -.400000e+01
C------2 OBJ.FUNC -.341980e+05 R------1 -.100000e+01
C------2 R------2 0.100000e+02 R------3 0.100000e+01
C------2 R------4 -.100000e+01 R------5 -.200000e+01
C------3 OBJ.FUNC 0.454200e+04 R------1 -.100000e+01
C------3 R------2 -.300000e+01 R------3 -.200000e+01
C------3 R------4 0.200000e+01 R------5 0.300000e+01
C------4 OBJ.FUNC 0.867200e+04 R------1 -.100000e+01
C------4 R------2 0.500000e+01 R------3 -.500000e+01
C------4 R------4 0.500000e+01 R------5 -.500000e+01
C------5 OBJ.FUNC 0.860000e+02 R------1 -.100000e+01
C------5 R------2 0.400000e+01 R------3 0.300000e+01
C------5 R------4 -.300000e+01 R------5 0.100000e+01
RHS
RHS OBJ.FUNC -.144630e+05
RHS R------1 -.500000e+01
RHS R------2 0.200000e+02
RHS R------3 -.400000e+02
RHS R------4 0.110000e+02
RHS R------5 -.300000e+02
RANGES
BOUNDS
FR BOUNDS C------1
FR BOUNDS C------2
FR BOUNDS C------3
FR BOUNDS C------4
FR BOUNDS C------5
QUADOBJ
C------1 C------1 0.203940e+05
C------1 C------2 -.249080e+05
C------1 C------3 -.202600e+04
C------1 C------4 0.389600e+04
C------1 C------5 0.658000e+03
C------2 C------2 0.418180e+05
C------2 C------3 -.346600e+04
C------2 C------4 -.982800e+04
C------2 C------5 -.372000e+03
C------3 C------3 0.351000e+04
C------3 C------4 0.217800e+04
C------3 C------5 -.348000e+03
C------4 C------4 0.303000e+04
C------4 C------5 -.440000e+02
C------5 C------5 0.540000e+02
ENDATA

20
examples/Data/HS35.QPS Normal file
View File

@ -0,0 +1,20 @@
NAME HS35
ROWS
N OBJ.FUNC
G R------1
COLUMNS
C------1 OBJ.FUNC -.800000e+01 R------1 -.100000e+01
C------2 OBJ.FUNC -.600000e+01 R------1 -.100000e+01
C------3 OBJ.FUNC -.400000e+01 R------1 -.200000e+01
RHS
RHS OBJ.FUNC -.900000e+01
RHS R------1 -.300000e+01
RANGES
BOUNDS
QUADOBJ
C------1 C------1 0.400000e+01
C------1 C------2 0.200000e+01
C------1 C------3 0.200000e+01
C------2 C------2 0.400000e+01
C------3 C------3 0.200000e+01
ENDATA

21
examples/Data/HS35MOD.QPS Normal file
View File

@ -0,0 +1,21 @@
NAME HS35MOD
ROWS
N OBJ.FUNC
G R------1
COLUMNS
C------1 OBJ.FUNC -.800000e+01 R------1 -.100000e+01
C------2 OBJ.FUNC -.600000e+01 R------1 -.100000e+01
C------3 OBJ.FUNC -.400000e+01 R------1 -.200000e+01
RHS
RHS OBJ.FUNC -.900000e+01
RHS R------1 -.300000e+01
RANGES
BOUNDS
FX BOUNDS C------2 0.500000e+00
QUADOBJ
C------1 C------1 0.400000e+01
C------1 C------2 0.200000e+01
C------1 C------3 0.200000e+01
C------2 C------2 0.400000e+01
C------3 C------3 0.200000e+01
ENDATA

33
examples/Data/HS51.QPS Normal file
View File

@ -0,0 +1,33 @@
NAME HS51
ROWS
N OBJ.FUNC
E R------1
E R------2
E R------3
COLUMNS
C------1 R------1 0.100000e+01
C------2 OBJ.FUNC -.400000e+01 R------1 0.300000e+01
C------2 R------3 0.100000e+01
C------3 OBJ.FUNC -.400000e+01 R------2 0.100000e+01
C------4 OBJ.FUNC -.200000e+01 R------2 0.100000e+01
C------5 OBJ.FUNC -.200000e+01 R------2 -.200000e+01
C------5 R------3 -.100000e+01
RHS
RHS OBJ.FUNC -.600000e+01
RHS R------1 0.400000e+01
RANGES
BOUNDS
FR BOUNDS C------1
FR BOUNDS C------2
FR BOUNDS C------3
FR BOUNDS C------4
FR BOUNDS C------5
QUADOBJ
C------1 C------1 0.200000e+01
C------1 C------2 -.200000e+01
C------2 C------2 0.400000e+01
C------2 C------3 0.200000e+01
C------3 C------3 0.200000e+01
C------4 C------4 0.200000e+01
C------5 C------5 0.200000e+01
ENDATA

32
examples/Data/HS52.QPS Normal file
View File

@ -0,0 +1,32 @@
NAME HS52
ROWS
N OBJ.FUNC
E R------1
E R------2
E R------3
COLUMNS
C------1 R------1 0.100000e+01
C------2 OBJ.FUNC -.400000e+01 R------1 0.300000e+01
C------2 R------3 0.100000e+01
C------3 OBJ.FUNC -.400000e+01 R------2 0.100000e+01
C------4 OBJ.FUNC -.200000e+01 R------2 0.100000e+01
C------5 OBJ.FUNC -.200000e+01 R------2 -.200000e+01
C------5 R------3 -.100000e+01
RHS
RHS OBJ.FUNC -.600000e+01
RANGES
BOUNDS
FR BOUNDS C------1
FR BOUNDS C------2
FR BOUNDS C------3
FR BOUNDS C------4
FR BOUNDS C------5
QUADOBJ
C------1 C------1 0.320000e+02
C------1 C------2 -.800000e+01
C------2 C------2 0.400000e+01
C------2 C------3 0.200000e+01
C------3 C------3 0.200000e+01
C------4 C------4 0.200000e+01
C------5 C------5 0.200000e+01
ENDATA

19
examples/Data/QPTEST.QPS Normal file
View File

@ -0,0 +1,19 @@
NAME QP example
ROWS
N obj
G r1
L r2
COLUMNS
c1 r1 2.0 r2 -1.0
c1 obj 1.5
c2 r1 1.0 r2 2.0
c2 obj -2.0
RHS
rhs1 r1 2.0 r2 6.0
BOUNDS
UP bnd1 c1 20.0
QUADOBJ
c1 c1 8.0
c1 c2 2.0
c2 c2 10.0
ENDATA

28
gtsam/linear/JacobianFactor.cpp
View File

@ -102,10 +102,9 @@ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
} }
/* ************************************************************************* */ /* ************************************************************************* */
JacobianFactor::JacobianFactor(const HessianFactor& factor) : JacobianFactor::JacobianFactor(const HessianFactor& factor)
Base(factor), Ab_( : Base(factor),
VerticalBlockMatrix::LikeActiveViewOf(factor.info(), Ab_(VerticalBlockMatrix::LikeActiveViewOf(factor.info(), factor.rows())) {
factor.rows())) {
// Copy Hessian into our matrix and then do in-place Cholesky // Copy Hessian into our matrix and then do in-place Cholesky
Ab_.full() = factor.info().selfadjointView(); Ab_.full() = factor.info().selfadjointView();
@ -114,16 +113,19 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) :
bool success; bool success;
boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix()); boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix());
// Check for indefinite system // Check that Cholesky succeeded OR it managed to factor the full Hessian.
if (!success) // THe latter case occurs with non-positive definite matrices arising from QP.
if (success || maxrank == factor.rows() - 1) {
// Zero out lower triangle
Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
Matrix::Zero(maxrank, Ab_.matrix().cols());
// FIXME: replace with triangular system
Ab_.rowEnd() = maxrank;
model_ = SharedDiagonal(); // is equivalent to Unit::Create(maxrank)
} else {
// indefinite system
throw IndeterminantLinearSystemException(factor.keys().front()); throw IndeterminantLinearSystemException(factor.keys().front());
}
// Zero out lower triangle
Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
Matrix::Zero(maxrank, Ab_.matrix().cols());
// FIXME: replace with triangular system
Ab_.rowEnd() = maxrank;
model_ = SharedDiagonal(); // should be same as Unit::Create(maxrank);
} }
/* ************************************************************************* */ /* ************************************************************************* */

25
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -146,7 +146,7 @@ TEST(JacobianFactor, constructors_and_accessors)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(JabobianFactor, Hessian_conversion) { TEST(JabobianFactor, Hessian_conversion) {
HessianFactor hessian(0, (Matrix(4,4) << HessianFactor hessian(0, (Matrix(4, 4) <<
1.57, 2.695, -1.1, -2.35, 1.57, 2.695, -1.1, -2.35,
2.695, 11.3125, -0.65, -10.225, 2.695, 11.3125, -0.65, -10.225,
-1.1, -0.65, 1, 0.5, -1.1, -0.65, 1, 0.5,
@ -154,7 +154,7 @@ TEST(JabobianFactor, Hessian_conversion) {
(Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(),
73.1725); 73.1725);
JacobianFactor expected(0, (Matrix(2,4) << JacobianFactor expected(0, (Matrix(2, 4) <<
1.2530, 2.1508, -0.8779, -1.8755, 1.2530, 2.1508, -0.8779, -1.8755,
0, 2.5858, 0.4789, -2.3943).finished(), 0, 2.5858, 0.4789, -2.3943).finished(),
Vector2(-6.2929, -5.7941)); Vector2(-6.2929, -5.7941));
@ -162,6 +162,27 @@ TEST(JabobianFactor, Hessian_conversion) {
EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3)); EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3));
} }
/* ************************************************************************* */
TEST(JabobianFactor, Hessian_conversion2) {
JacobianFactor jf(0, (Matrix(3, 3) <<
1, 2, 3,
0, 2, 3,
0, 0, 3).finished(),
Vector3(1, 2, 2));
HessianFactor hessian(jf);
EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9));
}
/* ************************************************************************* */
TEST(JabobianFactor, Hessian_conversion3) {
JacobianFactor jf(0, (Matrix(2, 4) <<
1, 2, 3, 0,
0, 3, 2, 1).finished(),
Vector2(1, 2));
HessianFactor hessian(jf);
EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9));
}
/* ************************************************************************* */ /* ************************************************************************* */
namespace simple_graph { namespace simple_graph {

523
gtsam_unstable/linear/QPSParser.cpp
View File

@ -17,56 +17,440 @@
#define BOOST_SPIRIT_USE_PHOENIX_V3 1 #define BOOST_SPIRIT_USE_PHOENIX_V3 1
#include <gtsam/base/Matrix.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/linear/QP.h>
#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 <boost/spirit/include/qi.hpp> #include <boost/fusion/include/vector.hpp>
#include <boost/fusion/sequence.hpp>
#include <boost/lambda/lambda.hpp> #include <boost/lambda/lambda.hpp>
#include <boost/phoenix/bind.hpp> #include <boost/phoenix/bind.hpp>
#include <boost/spirit/include/classic.hpp> #include <boost/spirit/include/classic.hpp>
#include <boost/spirit/include/qi.hpp>
#include <algorithm>
#include <iostream>
#include <map>
#include <string>
#include <unordered_map>
#include <vector>
using boost::fusion::at_c;
using namespace std;
namespace bf = boost::fusion; namespace bf = boost::fusion;
namespace qi = boost::spirit::qi; namespace qi = boost::spirit::qi;
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 { namespace gtsam {
/**
* As the parser reads a file, it call functions in this visitor. This visitor
* in turn stores what the parser has read in a way that can be later used to
* build the full QP problem in the file.
*/
class QPSVisitor {
private:
typedef std::unordered_map<Key, Matrix11> coefficient_v;
typedef std::unordered_map<std::string, coefficient_v> constraint_v;
std::unordered_map<std::string, constraint_v *>
row_to_constraint_v; // Maps QPS ROWS to Variable-Matrix pairs
constraint_v E; // Equalities
constraint_v IG; // Inequalities >=
constraint_v IL; // Inequalities <=
unsigned int numVariables;
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; // Inequalities can be specified as ranges on a variable
std::unordered_map<Key, Vector1> g; // linear term of quadratic cost
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; // H from hessian
double f; // Constant term of quadratic cost
std::string obj_name; // the objective function has a name in the QPS
std::string name_; // the quadratic program has a name in the QPS
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
KeyVector free; // Variables can be specified as free (to which no
// constraints apply)
const bool debug = false;
public:
QPSVisitor() : numVariables(1) {}
void setName(boost::fusion::vector<Chars, Chars, Chars> const &name) {
name_ = fromChars<1>(name);
if (debug) {
cout << "Parsing file: " << name_ << endl;
}
}
void addColumn(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_
<< " 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 addColumnDouble(
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_))
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 addRangeSingle(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;
}
}
void addRangeDouble(
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;
ranges[row2_] = range2;
if (debug) {
cout << "DOUBLE RANGE ADDED" << endl;
cout << "VAR: " << var_ << " ROW1: " << row1_ << " RANGE1: " << range1
<< " ROW2: " << row2_ << " RANGE2: " << range2 << endl;
}
}
void 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;
else
b[row_] = coefficient;
if (debug) {
cout << "Added RHS for Var: " << var_ << " Row: " << row_
<< " Coefficient: " << coefficient << endl;
}
}
void addRHSDouble(
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)
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 addRow(
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':
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 addBound(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;
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 addFreeBound(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;
}
}
void addQuadTerm(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;
H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient;
if (debug) {
cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_
<< " Coefficient: " << coefficient << endl;
}
}
QP makeQP() {
// Create the keys from the variable names
KeyVector keys;
for (auto kv : varname_to_key) {
keys.push_back(kv.second);
}
// Fill the G matrices and g vectors from
vector<Matrix> Gs;
vector<Vector> gs;
sort(keys.begin(), keys.end());
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 {
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);
}
}
// Construct the quadratic program
QP madeQP;
auto obj = HessianFactor(keys, Gs, gs, 2 * f);
madeQP.cost.push_back(obj);
// Add equality and inequality constraints into the QP
size_t dual_key_num = keys.size() + 1;
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;
}
};
typedef qi::grammar<boost::spirit::basic_istream_iterator<char>> base_grammar; 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 &)>
boost::function< addRow;
void(bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)> rhsSingle; boost::function<void(
boost::function< bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)>
void( rhsSingle;
bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars, Chars, boost::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, double,
Chars, double>)> rhsDouble; Chars, Chars, Chars, double>)>
boost::function< rhsDouble;
void(bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars>)> colSingle; boost::function<void(
boost::function< bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)>
void( rangeSingle;
bf::vector<Chars, Chars, Chars, Chars, double, Chars, Chars, Chars, boost::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, double,
double> const &)> colDouble; Chars, Chars, Chars, double>)>
boost::function< rangeDouble;
void(bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)> addQuadTerm; boost::function<void(
boost::function< bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars>)>
void( colSingle;
bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars, double> const &)> addBound; boost::function<void(bf::vector<Chars, Chars, Chars, Chars, double, Chars,
boost::function< Chars, Chars, double> const &)>
void(bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars> const &)> addBoundFr; colDouble;
MPSGrammar(RawQP * rqp) : boost::function<void(
base_grammar(start), rqp_(rqp), setName( bf::vector<Chars, Chars, Chars, Chars, Chars, double, Chars> const &)>
boost::bind(&RawQP::setName, rqp, ::_1)), addRow( addQuadTerm;
boost::bind(&RawQP::addRow, rqp, ::_1)), rhsSingle( boost::function<void(bf::vector<Chars, Chars, Chars, Chars, Chars, Chars,
boost::bind(&RawQP::addRHS, rqp, ::_1)), rhsDouble( Chars, double> const &)>
boost::bind(&RawQP::addRHSDouble, rqp, ::_1)), colSingle( addBound;
boost::bind(&RawQP::addColumn, rqp, ::_1)), colDouble( boost::function<void(
boost::bind(&RawQP::addColumnDouble, rqp, ::_1)), addQuadTerm( bf::vector<Chars, Chars, Chars, Chars, Chars, Chars, Chars> const &)>
boost::bind(&RawQP::addQuadTerm, rqp, ::_1)), addBound( addFreeBound;
boost::bind(&RawQP::addBound, rqp, ::_1)), addBoundFr( MPSGrammar(QPSVisitor *rqp)
boost::bind(&RawQP::addBoundFr, rqp, ::_1)) { : base_grammar(start),
rqp_(rqp),
setName(boost::bind(&QPSVisitor::setName, rqp, ::_1)),
addRow(boost::bind(&QPSVisitor::addRow, rqp, ::_1)),
rhsSingle(boost::bind(&QPSVisitor::addRHS, rqp, ::_1)),
rhsDouble(boost::bind(&QPSVisitor::addRHSDouble, rqp, ::_1)),
rangeSingle(boost::bind(&QPSVisitor::addRangeSingle, rqp, ::_1)),
rangeDouble(boost::bind(&QPSVisitor::addRangeDouble, rqp, ::_1)),
colSingle(boost::bind(&QPSVisitor::addColumn, rqp, ::_1)),
colDouble(boost::bind(&QPSVisitor::addColumnDouble, rqp, ::_1)),
addQuadTerm(boost::bind(&QPSVisitor::addQuadTerm, rqp, ::_1)),
addBound(boost::bind(&QPSVisitor::addBound, rqp, ::_1)),
addFreeBound(boost::bind(&QPSVisitor::addFreeBound, 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 | '_' | '-' | '.'];
@ -74,43 +458,54 @@ struct QPSParser::MPSGrammar: base_grammar {
word = lexeme[+character]; word = lexeme[+character];
name = lexeme[lit("NAME") >> *blank >> title >> +space][setName]; name = lexeme[lit("NAME") >> *blank >> title >> +space][setName];
row = lexeme[*blank >> character >> +blank >> word >> *blank][addRow]; row = lexeme[*blank >> character >> +blank >> word >> *blank][addRow];
rhs_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ rhs_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >>
>> *blank][rhsSingle]; *blank][rhsSingle];
rhs_double = lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ rhs_double =
>> +blank >> word >> +blank >> double_)[rhsDouble] >> *blank]; lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ >>
col_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ +blank >> word >> +blank >> double_)[rhsDouble] >>
>> *blank][colSingle]; *blank];
col_double = lexeme[*blank range_single = lexeme[*blank >> word >> +blank >> word >> +blank >>
>> (word >> +blank >> word >> +blank >> double_ >> +blank >> word double_ >> *blank][rangeSingle];
>> +blank >> double_)[colDouble] >> *blank]; range_double =
quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ >>
>> *blank][addQuadTerm]; +blank >> word >> +blank >> double_)[rangeDouble] >>
bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >> +blank *blank];
>> double_)[addBound] >> *blank]; col_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >>
bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word *blank][colSingle];
>> *blank][addBoundFr]; col_double =
lexeme[*blank >> (word >> +blank >> word >> +blank >> double_ >>
+blank >> word >> +blank >> double_)[colDouble] >>
*blank];
quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ >>
*blank][addQuadTerm];
bound = lexeme[(*blank >> word >> +blank >> word >> +blank >> word >>
+blank >> double_)[addBound] >>
*blank];
bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word >>
*blank][addFreeBound];
rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)]; rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)];
rhs = lexeme[lit("RHS") >> *blank >> eol rhs = lexeme[lit("RHS") >> *blank >> eol >>
>> +((rhs_double | rhs_single) >> eol)]; +((rhs_double | rhs_single) >> eol)];
cols = lexeme[lit("COLUMNS") >> *blank >> eol cols = lexeme[lit("COLUMNS") >> *blank >> eol >>
>> +((col_double | col_single) >> eol)]; +((col_double | col_single) >> eol)];
quad = lexeme[lit("QUADOBJ") >> *blank >> eol >> +(quad_l >> eol)]; quad = lexeme[lit("QUADOBJ") >> *blank >> eol >> +(quad_l >> eol)];
bounds = lexeme[lit("BOUNDS") >> +space >> +((bound | bound_fr) >> eol)]; bounds = lexeme[lit("BOUNDS") >> +space >> *((bound | bound_fr) >> eol)];
ranges = lexeme[lit("RANGES") >> +space]; ranges = lexeme[lit("RANGES") >> +space >>
*((range_double | range_single) >> eol)];
end = lexeme[lit("ENDATA") >> *space]; end = lexeme[lit("ENDATA") >> *space];
start = lexeme[name >> rows >> cols >> rhs >> -ranges >> bounds >> quad start =
>> end]; lexeme[name >> rows >> cols >> rhs >> -ranges >> bounds >> quad >> end];
} }
qi::rule<boost::spirit::basic_istream_iterator<char>, char()> character; qi::rule<boost::spirit::basic_istream_iterator<char>, char()> character;
qi::rule<boost::spirit::basic_istream_iterator<char>, Chars()> word, title; qi::rule<boost::spirit::basic_istream_iterator<char>, Chars()> word, title;
qi::rule<boost::spirit::basic_istream_iterator<char> > row, end, col_single, qi::rule<boost::spirit::basic_istream_iterator<char>> row, end, col_single,
col_double, rhs_single, rhs_double, ranges, bound, bound_fr, bounds, quad, col_double, rhs_single, rhs_double, range_single, range_double, ranges,
quad_l, rows, cols, rhs, name, start; bound, bound_fr, bounds, quad, quad_l, rows, cols, rhs, name, start;
}; };
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);
@ -123,4 +518,4 @@ QP QPSParser::Parse() {
return rawData.makeQP(); return rawData.makeQP();
} }
} } // namespace gtsam

11
gtsam_unstable/linear/QPSolver.h
View File

@ -32,9 +32,14 @@ struct QPPolicy {
static constexpr double maxAlpha = 1.0; static constexpr double maxAlpha = 1.0;
/// Simply the cost of the QP problem /// Simply the cost of the QP problem
static const GaussianFactorGraph& buildCostFunction( static const GaussianFactorGraph buildCostFunction(const QP& qp,
const QP& qp, const VectorValues& xk = VectorValues()) { const VectorValues& xk = VectorValues()) {
return qp.cost; GaussianFactorGraph no_constant_factor;
for (auto factor : qp.cost) {
HessianFactor hf = static_cast<HessianFactor>(*factor);
no_constant_factor.push_back(hf);
}
return no_constant_factor;
} }
}; };

271
gtsam_unstable/linear/RawQP.cpp
View File

@ -1,271 +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 (!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;
if (debug) {
std::cout << "Added Column for Var: " << var_ << " Row: " << row_
<< " Coefficient: " << coefficient << std::endl;
}
}
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::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
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() {
KeyVector 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) {
Gs.push_back(H[keys[i]][keys[j]]);
}
}
for (Key key1 : keys) {
gs.push_back(-g[key1]);
}
int dual_key_num = keys.size() + 1;
QP madeQP;
auto obj = HessianFactor(keys, Gs, gs, f);
madeQP.cost.push_back(obj);
for (auto kv : E) {
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> keyMatrixMap;
for (auto km : kv.second) {
km.second = -km.second;
keyMatrixMap.insert(km);
}
madeQP.inequalities.push_back(
LinearInequality(keyMatrixMap, -b[kv.first], dual_key_num++));
}
for (auto kv : IL) {
std::map<Key, Matrix11> keyMatrixMap;
for (auto km : kv.second) {
keyMatrixMap.insert(km);
}
madeQP.inequalities.push_back(
LinearInequality(keyMatrixMap, b[kv.first], dual_key_num++));
}
for (Key k : keys) {
if (std::find(Free.begin(), Free.end(), k) != Free.end())
continue;
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;
}
}

106
gtsam_unstable/linear/RawQP.h
View File

@ -1,106 +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.h
* @brief
* @author Ivan Dario Jimenez
* @date 3/5/16
*/
#pragma once
#include <gtsam_unstable/linear/QP.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/inference/Key.h>
#include <string>
#include <vector>
#include <unordered_map>
#include <gtsam/inference/Symbol.h>
#include <boost/fusion/sequence.hpp>
#include <boost/fusion/include/vector.hpp>
namespace gtsam {
class RawQP {
private:
typedef std::unordered_map<Key, Matrix11> coefficient_v;
typedef std::unordered_map<std::string, coefficient_v> constraint_v;
std::unordered_map<std::string, constraint_v*> row_to_constraint_v;
constraint_v E;
constraint_v IG;
constraint_v IL;
unsigned int varNumber;
std::unordered_map<std::string, double> b;
std::unordered_map<Key, Vector1> g;
std::unordered_map<std::string, Key> varname_to_key;
std::unordered_map<Key, std::unordered_map<Key, Matrix11> > H;
double f;
std::string obj_name;
std::string name_;
std::unordered_map<Key, double> up;
std::unordered_map<Key, double> lo;
KeyVector Free;
const bool debug = false;
public:
RawQP() :
row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), Free() {
}
void setName(
boost::fusion::vector<std::vector<char>, std::vector<char>,
std::vector<char>> 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 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);
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 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);
void addRow(
boost::fusion::vector<std::vector<char>, char, std::vector<char>,
std::vector<char>, std::vector<char>> 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 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);
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);
QP makeQP();
}
;
}

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

@ -14,6 +14,7 @@
* @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 * @author Duy-Nguyen Ta
* @author Ivan Dario Jimenez
*/ */
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
@ -218,7 +219,7 @@ pair<QP, QP> testParser(QPSParser parser) {
// min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
expectedqp.cost.push_back( expectedqp.cost.push_back(
HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1, HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1,
2.0 * kOne, 4.0)); 2.0 * kOne, 8.0));
// 2x + y >= 2 // 2x + y >= 2
// -x + 2y <= 6 // -x + 2y <= 6
expectedqp.inequalities.push_back( expectedqp.inequalities.push_back(
@ -269,6 +270,66 @@ TEST(QPSolver, QPExampleTest){
CHECK(assert_equal(error_expected, error_actual)) CHECK(assert_equal(error_expected, error_actual))
} }
TEST(QPSolver, HS21) {
QP problem = QPSParser("HS21.QPS").Parse();
VectorValues actualSolution;
VectorValues expectedSolution;
expectedSolution.insert(Symbol('X',1), 2.0*I_1x1);
expectedSolution.insert(Symbol('X',2), 0.0*I_1x1);
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(-99.9599999, error_actual, 1e-7))
CHECK(assert_equal(expectedSolution, actualSolution))
}
TEST(QPSolver, HS35) {
QP problem = QPSParser("HS35.QPS").Parse();
VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(1.11111111e-01,error_actual, 1e-7))
}
TEST(QPSolver, HS35MOD) {
QP problem = QPSParser("HS35MOD.QPS").Parse();
VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(2.50000001e-01,error_actual, 1e-7))
}
TEST(QPSolver, HS51) {
QP problem = QPSParser("HS51.QPS").Parse();
VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(8.88178420e-16,error_actual, 1e-7))
}
TEST(QPSolver, HS52) {
QP problem = QPSParser("HS52.QPS").Parse();
VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(5.32664756,error_actual, 1e-7))
}
TEST(QPSolver, HS268) { // This test needs an extra order of magnitude of tolerance than the rest
QP problem = QPSParser("HS268.QPS").Parse();
VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(5.73107049e-07,error_actual, 1e-6))
}
TEST(QPSolver, QPTEST) { // REQUIRES Jacobian Fix
QP problem = QPSParser("QPTEST.QPS").Parse();
VectorValues actualSolution;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(problem).optimize();
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(0.437187500e01,error_actual, 1e-7))
}
/* ************************************************************************* */ /* ************************************************************************* */
// Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html // Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html
QP createTestMatlabQPEx() { QP createTestMatlabQPEx() {