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

TESTED QP Solver

release/4.3a0
= 2016-06-18 10:39:59 -04:00
parent 0092c27551
commit 2c7f1902af
5 changed files with 8 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
gtsam_unstable/linear/ActiveSetSolver-inl.h
View File

@ -189,13 +189,6 @@ This::buildWorkingGraph(const InequalityFactorGraph& workingSet,
return workingGraph; return workingGraph;
} }
//******************************************************************************
Template VectorValues
This::evaluateCostFunction(const VectorValues &xk) const {
GaussianFactorGraph costGraph;
costGraph.push_back(POLICY::buildCostFunction(problem_, xk));
return costGraph.optimize();
}
//****************************************************************************** //******************************************************************************
Template typename This::State This::iterate( Template typename This::State This::iterate(
const typename This::State& state) const { const typename This::State& state) const {

2
gtsam_unstable/linear/ActiveSetSolver.h
View File

@ -168,8 +168,6 @@ public: /// Just for testing...
const InequalityFactorGraph& workingSet, const InequalityFactorGraph& workingSet,
const VectorValues& xk = VectorValues()) const; const VectorValues& xk = VectorValues()) const;
VectorValues evaluateCostFunction(const VectorValues& xk) const;
/// Iterate 1 step, return a new state with a new workingSet and values /// Iterate 1 step, return a new state with a new workingSet and values
State iterate(const State& state) const; State iterate(const State& state) const;

2
gtsam_unstable/linear/InequalityFactorGraph.h
View File

@ -53,7 +53,7 @@ public:
double error(const VectorValues& x) const { double error(const VectorValues& x) const {
for (const sharedFactor& factor : *this) { for (const sharedFactor& factor : *this) {
if (factor) if (factor)
if (factor->error(x) > 0) if (factor->error(x) > 1e-7)
return std::numeric_limits<double>::infinity(); return std::numeric_limits<double>::infinity();
} }
return 0.0; return 0.0;

2
gtsam_unstable/linear/RawQP.cpp
View File

@ -216,7 +216,7 @@ QP RawQP::makeQP() {
} }
} }
for (Key key1 : keys) { for (Key key1 : keys) {
gs.push_back(g[key1]); gs.push_back(-g[key1]);
} }
int dual_key_num = keys.size() + 1; int dual_key_num = keys.size() + 1;
QP madeQP; QP madeQP;

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

@ -217,8 +217,8 @@ pair<QP, QP> testParser(QPSParser parser) {
Key X1(Symbol('X', 1)), X2(Symbol('X', 2)); Key X1(Symbol('X', 1)), X2(Symbol('X', 2));
// 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, 4.0));
// 2x + y >= 2 // 2x + y >= 2
// -x + 2y <= 6 // -x + 2y <= 6
expectedqp.inequalities.push_back( expectedqp.inequalities.push_back(
@ -263,12 +263,12 @@ TEST(QPSolver, QPExampleTest){
VectorValues expectedSolution; VectorValues expectedSolution;
expectedSolution.insert(Symbol('X',1),0.7625*I_1x1); expectedSolution.insert(Symbol('X',1),0.7625*I_1x1);
expectedSolution.insert(Symbol('X',2),0.4750*I_1x1); expectedSolution.insert(Symbol('X',2),0.4750*I_1x1);
VectorValues actualCost = solver.evaluateCostFunction(actualSolution); double error_expected = problem.cost.error(expectedSolution);
VectorValues expectedCost = solver.evaluateCostFunction(expectedCost); double error_actual = problem.cost.error(actualSolution);
GTSAM_PRINT(actualCost);
CHECK(assert_equal(expectedCost, actualCost))
CHECK(assert_equal(expectedSolution, actualSolution, 1e-7)) CHECK(assert_equal(expectedSolution, actualSolution, 1e-7))
CHECK(assert_equal(error_expected, error_actual))
} }
/* ************************************************************************* */ /* ************************************************************************* */
// 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() {