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

[QP/LP] Error Identified and test sample test case generated but fails.

release/4.3a0
ivan 2016-04-25 19:00:22 -04:00
parent 5b208b459a
commit 3fc9d98707
5 changed files with 368 additions and 214 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
gtsam_unstable/linear/LPSolver.cpp
View File

@ -54,7 +54,8 @@ LPState LPSolver::iterate(const LPState &state) const {
// to find the direction to move
VectorValues newValues = solveWithCurrentWorkingSet(state.values,
state.workingSet);
// GTSAM_PRINT(newValues);
// GTSAM_PRINT(state.values);
// If we CAN'T move further
// LP: projection on the constraints' nullspace is zero: we are at a vertex
if (newValues.equals(state.values, 1e-7)) {
@ -66,6 +67,8 @@ LPState LPSolver::iterate(const LPState &state) const {
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
newValues);
VectorValues duals = dualGraph->optimize();
// GTSAM_PRINT(*dualGraph);
// GTSAM_PRINT(duals);
// LP: see which inequality constraint has wrong pulling direction, i.e., dual < 0
int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
// If all inequality constraints are satisfied: We have the solution!!
@ -92,6 +95,7 @@ LPState LPSolver::iterate(const LPState &state) const {
double alpha;
int factorIx;
VectorValues p = newValues - state.values;
// GTSAM_PRINT(p);
boost::tie(alpha, factorIx) = // using 16.41
computeStepSize(state.workingSet, state.values, p);
// also add to the working set the one that complains the most
@ -108,14 +112,31 @@ LPState LPSolver::iterate(const LPState &state) const {
GaussianFactorGraph::shared_ptr LPSolver::createLeastSquareFactors(
const LinearCost &cost, const VectorValues &xk) const {
GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph());
KeyVector keys = cost.keys();
//Something in this function breaks when working with funcions that do not include all
//Variables in the cost function. When adding the missing variables as shown we still don't converge
//to the right answer as we would expect from the iterations.
// GTSAM_PRINT(xk);
// GTSAM_PRINT(cost);
for (LinearCost::const_iterator it = cost.begin(); it != cost.end(); ++it) {
size_t dim = cost.getDim(it);
Vector b = xk.at(*it) - cost.getA(it).transpose(); // b = xk-g
graph->push_back(JacobianFactor(*it, eye(dim), b));
}
KeySet allKeys = lp_.inequalities.keys();
allKeys.merge(lp_.equalities.keys());
allKeys.merge(KeySet(lp_.cost.keys()));
if (cost.keys().size() != allKeys.size()) {
KeySet difference;
std::set_difference(allKeys.begin(), allKeys.end(), lp_.cost.begin(),
lp_.cost.end(), std::inserter(difference, difference.end()));
for (Key k : difference) {
graph->push_back(JacobianFactor(k, eye(keysDim_.at(k)), xk.at(k)));
}
}
// GTSAM_PRINT(*graph);
return graph;
}
@ -123,11 +144,11 @@ VectorValues LPSolver::solveWithCurrentWorkingSet(const VectorValues &xk,
const InequalityFactorGraph &workingSet) const {
GaussianFactorGraph workingGraph = baseGraph_; // || X - Xk + g ||^2
workingGraph.push_back(*createLeastSquareFactors(lp_.cost, xk));
for (const LinearInequality::shared_ptr &factor : workingSet) {
if (factor->active())
workingGraph.push_back(factor);
}
// GTSAM_PRINT(workingGraph);
return workingGraph.optimize();
}
@ -186,9 +207,11 @@ std::pair<VectorValues, VectorValues> LPSolver::optimize(
LPState state(initialValues, duals, workingSet, false, 0);
/// main loop of the solver
while (!state.converged)
while (!state.converged) {
if(state.iterations > 10000) // Temporary break to avoid infine loops
break;
state = iterate(state);
}
return make_pair(state.values, state.duals);
}
}

253
gtsam_unstable/linear/RawQP.cpp
View File

@ -1,2 +1,253 @@
#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) * ones(1, 1);
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) * ones(1, 1);
Matrix11 coefficient2 = at_c < 8 > (vars) * ones(1, 1);
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) * ones(1, 1);
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) {
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::vector < std::pair<Key, Matrix11> > KeyMatPair;
for (auto km : kv.second) {
KeyMatPair.push_back(km);
}
madeQP.equalities.push_back(
LinearEquality(KeyMatPair, b[kv.first] * ones(1, 1), dual_key_num++));
}
for (auto kv : IG) {
std::vector < std::pair<Key, Matrix11> > KeyMatPair;
for (auto km : kv.second) {
km.second = -km.second;
KeyMatPair.push_back(km);
}
madeQP.inequalities.push_back(
LinearInequality(KeyMatPair, -b[kv.first], dual_key_num++));
}
for (auto kv : IL) {
std::vector < std::pair<Key, Matrix11> > KeyMatPair;
for (auto km : kv.second) {
KeyMatPair.push_back(km);
}
madeQP.inequalities.push_back(
LinearInequality(KeyMatPair, 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, ones(1, 1), up[k], dual_key_num++));
if (lo.count(k) == 1)
madeQP.inequalities.push_back(
LinearInequality(k, -ones(1, 1), lo[k], dual_key_num++));
else
madeQP.inequalities.push_back(
LinearInequality(k, -ones(1, 1), 0, dual_key_num++));
}
return madeQP;
}
}

224
gtsam_unstable/linear/RawQP.h
View File

@ -4,250 +4,86 @@
#include <gtsam/base/Matrix.h>
#include <gtsam/inference/Key.h>
#include <iostream>
#include <string>
#include <vector>
#include <utility>
#include <unordered_map>
#include <gtsam/inference/Symbol.h>
#include <boost/any.hpp>
#include <boost/fusion/sequence.hpp>
namespace gtsam {
class RawQP {
typedef std::vector<std::pair<Key, Matrix> > coefficient_v;
private:
typedef std::unordered_map<Key, Matrix11> coefficient_v;
typedef std::unordered_map<std::string, coefficient_v> constraint_v;
std::vector<std::pair<Key, Matrix> > g;
std::unordered_map<std::string, std::unordered_map<std::string, coefficient_v> > row_to_map;
std::unordered_map<std::string, Key> varname_to_key;
std::unordered_map<std::string, coefficient_v> IL;
std::unordered_map<std::string, coefficient_v> IG;
std::unordered_map<std::string, coefficient_v> E;
std::unordered_map<std::string, double> b;
std::unordered_map<std::string, std::unordered_map<std::string, double> > H;
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;
std::vector<Key> Free;
const bool debug = true;
public:
RawQP() :
g(), row_to_map(), varname_to_key(), IL(), IG(), E(), b(), H(), varNumber(
1) {
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) {
std::vector<char>> const & name);
name_ = std::string(boost::fusion::at_c < 1 > (name).begin(),
boost::fusion::at_c < 1 > (name).end());
if (debug) {
std::cout << "Parsing file: " << name_ << std::endl;
}
}
// void addColumn(std::vector<char> var, std::vector<char> row,
// double coefficient) {
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) {
std::string var_(boost::fusion::at_c < 1 > (vars).begin(),
boost::fusion::at_c < 1 > (vars).end()), row_(
boost::fusion::at_c < 3 > (vars).begin(),
boost::fusion::at_c < 3 > (vars).end());
double coefficient = boost::fusion::at_c < 5 > (vars);
if (!varname_to_key.count(var_))
varname_to_key.insert( { var_, Symbol('X', varNumber++) });
std::vector<char>> const & vars);
if (row_ == obj_name) {
g.push_back(
std::make_pair(varname_to_key[var_], coefficient * ones(1, 1)));
return;
}
row_to_map[row_][row_].push_back(
{ varname_to_key[var_], coefficient * ones(1, 1) });
if (debug) {
std::cout << "Added Column for Var: " << var_ << " Row: " << row_
<< " Coefficient: " << coefficient << std::endl;
}
}
// void addColumn(std::vector<char> var, std::vector<char> row1,
// double coefficient1, std::vector<char> row2, double coefficient2) {
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) {
std::string var_(boost::fusion::at_c < 0 > (vars).begin(),
boost::fusion::at_c < 0 > (vars).end()), row1_(
boost::fusion::at_c < 2 > (vars).begin(),
boost::fusion::at_c < 2 > (vars).end()), row2_(
boost::fusion::at_c < 6 > (vars).begin(),
boost::fusion::at_c < 6 > (vars).end());
double coefficient1 = boost::fusion::at_c < 4 > (vars);
double coefficient2 = boost::fusion::at_c < 8 > (vars);
if (!varname_to_key.count(var_))
varname_to_key.insert( { var_, Symbol('X', varNumber++) });
if (row1_ == obj_name)
row_to_map[row1_][row2_].push_back(
{ varname_to_key[var_], coefficient1 * ones(1, 1) });
else
row_to_map[row1_][row1_].push_back(
{ varname_to_key[var_], coefficient1 * ones(1, 1) });
if (row2_ == obj_name)
row_to_map[row2_][row2_].push_back(
{ varname_to_key[var_], coefficient2 * ones(1, 1) });
else
row_to_map[row2_][row2_].push_back(
{ varname_to_key[var_], coefficient2 * ones(1, 1) });
if (debug) {
std::cout << "Added Column for Var: " << var_ << " Row: " << row1_
<< " Coefficient: " << coefficient1 << std::endl;
std::cout << " " << "Row: " << row2_
<< " Coefficient: " << coefficient2 << std::endl;
}
}
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) {
std::string var_(boost::fusion::at_c < 1 > (vars).begin(),
boost::fusion::at_c < 1 > (vars).end()), row_(
boost::fusion::at_c < 3 > (vars).begin(),
boost::fusion::at_c < 3 > (vars).end());
double coefficient = boost::fusion::at_c < 5 > (vars);
if (row_ == obj_name)
f = -coefficient;
else
b.insert( { row_, coefficient });
std::vector<char>> const & vars);
if (debug) {
std::cout << "Added RHS for Var: " << var_ << " Row: " << row_
<< " Coefficient: " << coefficient << std::endl;
}
}
// void addRHS(std::vector<char> var, std::vector<char> row1,
// double coefficient1, std::vector<char> row2, double coefficient2) {
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) {
std::string var_(boost::fusion::at_c < 1 > (vars).begin(),
boost::fusion::at_c < 1 > (vars).end()), row1_(
boost::fusion::at_c < 3 > (vars).begin(),
boost::fusion::at_c < 3 > (vars).end()), row2_(
boost::fusion::at_c < 7 > (vars).begin(),
boost::fusion::at_c < 7 > (vars).end());
double coefficient1 = boost::fusion::at_c < 5 > (vars);
double coefficient2 = boost::fusion::at_c < 9 > (vars);
if (row1_ == obj_name)
f = -coefficient1;
else
b.insert( { row1_, coefficient1 });
if (row2_ == obj_name)
f = -coefficient2;
else
b.insert( { 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;
}
}
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) {
std::string name_(boost::fusion::at_c < 3 > (vars).begin(),
boost::fusion::at_c < 3 > (vars).end());
char type = boost::fusion::at_c < 1 > (vars);
switch (type) {
case 'N':
obj_name = name_;
break;
case 'L':
row_to_map.insert( { name_, IL });
IL.insert( { name_, coefficient_v() });
break;
case 'G':
row_to_map.insert( { name_, IG });
IG.insert( { name_, coefficient_v() });
break;
case 'E':
row_to_map.insert( { name_, E });
E.insert( { name_, coefficient_v() });
break;
default:
std::cout << "invalid type: " << type << std::endl;
break;
}
if (debug) {
std::cout << "Added Row Type: " << type << " Name: " << name_
<< std::endl;
}
}
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) {
std::string type_(boost::fusion::at_c < 1 > (vars).begin(),
boost::fusion::at_c < 1 > (vars).end()), var_(
boost::fusion::at_c < 5 > (vars).begin(),
boost::fusion::at_c < 5 > (vars).end());
double number = boost::fusion::at_c < 7 > (vars);
std::vector<char>, std::vector<char>, double> const & vars);
if (debug) {
std::cout << "Added Bound Type: " << type_ << " Var: " << var_
<< " Amount: " << number << std::endl;
}
}
// void addBound(std::vector<char> type, std::vector<char> var) {
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) {
std::string type_(boost::fusion::at_c < 1 > (vars).begin(),
boost::fusion::at_c < 1 > (vars).end()), var_(
boost::fusion::at_c < 5 > (vars).begin(),
boost::fusion::at_c < 5 > (vars).end());
if (debug) {
std::cout << "Added Free Bound Type: " << type_ << " Var: " << var_
<< " Amount: " << std::endl;
}
}
std::vector<char>, std::vector<char>> const & vars);
// void addQuadTerm(std::vector<char> var1, std::vector<char> var2,
// double coefficient) {
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) {
std::string var1_(boost::fusion::at_c < 1 > (vars).begin(),
boost::fusion::at_c < 1 > (vars).end()), var2_(
boost::fusion::at_c < 3 > (vars).begin(),
boost::fusion::at_c < 3 > (vars).end());
double coefficient = boost::fusion::at_c < 5 > (vars);
if (debug) {
std::cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_
<< " Coefficient: " << coefficient << std::endl;
}
}
std::vector<char>> const & vars);
QP makeQP() {
return QP();
}
};
QP makeQP();
}
;
}

51
gtsam_unstable/linear/tests/testLPSolver.cpp
View File

@ -54,15 +54,51 @@ LP simpleLP1() {
return lp;
}
LP infeasibleLP() {
LP lp;
lp.cost = LinearCost(1, Vector3(-1, -1, -2));
}
/* ************************************************************************* */
namespace gtsam {
TEST(LPInitSolverMatlab, infinite_loop_multi_var) {
LP initchecker;
Key X = symbol('X',1);
Key Y = symbol('Y',1);
Key Z = symbol('Z',1);
initchecker.cost = LinearCost(Z, ones(1)); //min alpha
// initchecker.cost = LinearCost(Z, ones(1), X, zero(1), Y, zero(1)); //min alpha
initchecker.inequalities.push_back(LinearInequality(X,-2.0*ones(1), Y, -1.0*ones(1), Z, -1.0*ones(1),-2,1));//-2x-y-alpha <= -2
initchecker.inequalities.push_back(LinearInequality(X, -1.0*ones(1), Y, 2.0*ones(1), Z, -1.0*ones(1), 6, 2));// -x+2y-alpha <= 6
initchecker.inequalities.push_back(LinearInequality(X, -1.0*ones(1), Z, -1.0*ones(1), 0,3));// -x - alpha <= 0
initchecker.inequalities.push_back(LinearInequality(X, 1.0*ones(1), Z, -1.0*ones(1), 20, 4));//x - alpha <= 20
initchecker.inequalities.push_back(LinearInequality(Y, -1.0*ones(1), Z, -1.0*ones(1), 0, 5));// -y - alpha <= 0
LPSolver solver(initchecker);
VectorValues starter;
starter.insert(X, zero(1));
starter.insert(Y, zero(1));
starter.insert(Z, 2*ones(1));
VectorValues results, duals;
boost::tie(results, duals) = solver.optimize(starter);
VectorValues expected;
expected.insert(X, Vector::Constant(1, 13.5));
expected.insert(Y, Vector::Constant(1,6.5));
expected.insert(Z, Vector::Constant(1,-6.5));
CHECK(assert_equal(results, expected, 1e-7));
}
TEST(LPInitSolverMatlab, infinite_loop_single_var) {
LP initchecker;
initchecker.cost = LinearCost(1,Vector3(0,0,1)); //min alpha
initchecker.inequalities.push_back(LinearInequality(1, Vector3(-2,-1,-1),-2,1));//-2x-y-alpha <= -2
initchecker.inequalities.push_back(LinearInequality(1, Vector3(-1,2,-1), 6, 2));// -x+2y-alpha <= 6
initchecker.inequalities.push_back(LinearInequality(1, Vector3(-1,0,-1), 0,3));// -x - alpha <= 0
initchecker.inequalities.push_back(LinearInequality(1, Vector3(1,0,-1), 20, 4));//x - alpha <= 20
initchecker.inequalities.push_back(LinearInequality(1, Vector3(0,-1,-1),0, 5));// -y - alpha <= 0
LPSolver solver(initchecker);
VectorValues starter;
starter.insert(1,Vector3(0,0,2));
VectorValues results, duals;
boost::tie(results, duals) = solver.optimize(starter);
VectorValues expected;
expected.insert(1, Vector3(13.5, 6.5, -6.5));
CHECK(assert_equal(results, expected, 1e-7));
}
TEST(LPInitSolverMatlab, initialization) {
LP lp = simpleLP1();
LPSolver lpSolver(lp);
@ -93,7 +129,6 @@ TEST(LPInitSolverMatlab, initialization) {
expectedInitLP.inequalities.push_back(
LinearInequality(1, Vector2( -1, 1), 2, Vector::Constant(1, -1), 1, 5));// -x1 + x2 - y <= 1
CHECK(assert_equal(expectedInitLP, *initLP, 1e-10));
LPSolver lpSolveInit(*initLP);
VectorValues xy0(x0);
xy0.insert(yKey, Vector::Constant(1, y0));

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

@ -213,9 +213,8 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
CHECK(assert_equal(expectedSolution, solution, 1e-100));
}
void testParser(QPSParser parser) {
pair<QP, QP> testParser(QPSParser parser) {
QP exampleqp = parser.Parse();
// QP expectedqp = createExampleQP();
QP expectedqp;
// min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
@ -234,7 +233,8 @@ void testParser(QPSParser parser) {
expectedqp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), 0, 3));
// x<= 20
expectedqp.inequalities.push_back(LinearInequality(X(1), ones(1, 1), 20, 4));
return std::make_pair(expectedqp, exampleqp);
// CHECK(assert_equal(expectedqp.cost, exampleqp.cost, 1e-7));
// CHECK(expectedqp.cost.equals(exampleqp.cost, 1e-7));
// CHECK(expectedqp.inequalities.equals(exampleqp.inequalities, 1e-7));
// CHECK(expectedqp.equalities.equals(exampleqp.equalities, 1e-7));
@ -242,9 +242,18 @@ void testParser(QPSParser parser) {
TEST(QPSolver, QPExampleData) {
testParser(QPSParser("QPExample.QPS"));
testParser(QPSParser("AUG2D.QPS"));
testParser(QPSParser("CONT-050.QPS"));
auto expected_actual = testParser(QPSParser("QPExample.QPS"));
VectorValues actualSolution, expectedSolution;
expected_actual.first.print("EXPECTED GRAPH:");
expected_actual.second.print("ACTUAL GRAPH");
boost::tie(expectedSolution, boost::tuples::ignore) = QPSolver(expected_actual.first).optimize();
std::cout << "Expected Execution Works" << std::endl;
boost::tie(actualSolution, boost::tuples::ignore) = QPSolver(expected_actual.second).optimize();
std::cout << "Actual Execution Works" << std::endl;
CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
// testParser(QPSParser("AUG2D.QPS"));
// testParser(QPSParser("CONT-050.QPS"));
}
/* ************************************************************************* */