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

Merge pull request #546 from borglab/fix/LP_QP_stype 

LP/QP Style Update
release/4.3a0
Frank Dellaert 2020-10-02 18:01:53 -04:00 committed by GitHub
parent 3f3e286a4d a8ea6f2bd2
commit 188efad2c5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
7 changed files with 221 additions and 259 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -149,7 +149,7 @@ Template JacobianFactor::shared_ptr This::createDualFactor(
// to compute the least-square approximation of dual variables
return boost::make_shared<JacobianFactor>(Aterms, b);
} else {
return boost::make_shared<JacobianFactor>();
return nullptr;
}
}
@ -165,14 +165,13 @@ Template JacobianFactor::shared_ptr This::createDualFactor(
* if lambda = 0 you are on the constraint
* if lambda > 0 you are violating the constraint.
*/
Template GaussianFactorGraph::shared_ptr This::buildDualGraph(
Template GaussianFactorGraph This::buildDualGraph(
const InequalityFactorGraph& workingSet, const VectorValues& delta) const {
GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph());
GaussianFactorGraph dualGraph;
for (Key key : constrainedKeys_) {
// Each constrained key becomes a factor in the dual graph
JacobianFactor::shared_ptr dualFactor =
createDualFactor(key, workingSet, delta);
if (!dualFactor->empty()) dualGraph->push_back(dualFactor);
auto dualFactor = createDualFactor(key, workingSet, delta);
if (dualFactor) dualGraph.push_back(dualFactor);
}
return dualGraph;
}
@ -193,19 +192,16 @@ This::buildWorkingGraph(const InequalityFactorGraph& workingSet,
Template typename This::State This::iterate(
const typename This::State& state) const {
// Algorithm 16.3 from Nocedal06book.
// Solve with the current working set eqn 16.39, but instead of solving for p
// solve for x
GaussianFactorGraph workingGraph =
buildWorkingGraph(state.workingSet, state.values);
// Solve with the current working set eqn 16.39, but solve for x not p
auto workingGraph = buildWorkingGraph(state.workingSet, state.values);
VectorValues newValues = workingGraph.optimize();
// If we CAN'T move further
// if p_k = 0 is the original condition, modified by Duy to say that the state
// update is zero.
if (newValues.equals(state.values, 1e-7)) {
// Compute lambda from the dual graph
GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet,
newValues);
VectorValues duals = dualGraph->optimize();
auto dualGraph = buildDualGraph(state.workingSet, newValues);
VectorValues duals = dualGraph.optimize();
int leavingFactor = identifyLeavingConstraint(state.workingSet, duals);
// If all inequality constraints are satisfied: We have the solution!!
if (leavingFactor < 0) {

4
gtsam_unstable/linear/ActiveSetSolver.h
View File

@ -154,8 +154,8 @@ protected:
public: /// Just for testing...
/// Builds a dual graph from the current working set.
GaussianFactorGraph::shared_ptr buildDualGraph(
const InequalityFactorGraph& workingSet, const VectorValues& delta) const;
GaussianFactorGraph buildDualGraph(const InequalityFactorGraph &workingSet,
const VectorValues &delta) const;
/**
* Build a working graph of cost, equality and active inequality constraints

5
gtsam_unstable/linear/EqualityFactorGraph.h
View File

@ -31,6 +31,11 @@ class EqualityFactorGraph: public FactorGraph<LinearEquality> {
public:
typedef boost::shared_ptr<EqualityFactorGraph> shared_ptr;
/// Add a linear inequality, forwards arguments to LinearInequality.
template <class... Args> void add(Args &&... args) {
emplace_shared<LinearEquality>(std::forward<Args>(args)...);
}
/// Compute error of a guess.
double error(const VectorValues& x) const {
double total_error = 0.;

5
gtsam_unstable/linear/InequalityFactorGraph.h
View File

@ -47,6 +47,11 @@ public:
return Base::equals(other, tol);
}
/// Add a linear inequality, forwards arguments to LinearInequality.
template <class... Args> void add(Args &&... args) {
emplace_shared<LinearInequality>(std::forward<Args>(args)...);
}
/**
* Compute error of a guess.
* Infinity error if it violates an inequality; zero otherwise. */

3
gtsam_unstable/linear/LPInitSolver.h
View File

@ -21,7 +21,6 @@
#include <gtsam_unstable/linear/LP.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <CppUnitLite/Test.h>
namespace gtsam {
/**
@ -83,7 +82,7 @@ private:
const InequalityFactorGraph& inequalities) const;
// friend class for unit-testing private methods
FRIEND_TEST(LPInitSolver, initialization);
friend class LPInitSolverInitializationTest;
};
}

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

@ -16,21 +16,23 @@
* @author Duy-Nguyen Ta
*/
#include <gtsam_unstable/linear/LPInitSolver.h>
#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam_unstable/linear/EqualityFactorGraph.h>
#include <gtsam_unstable/linear/InequalityFactorGraph.h>
#include <gtsam_unstable/linear/InfeasibleInitialValues.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/foreach.hpp>
#include <boost/range/adaptor/map.hpp>
#include <gtsam_unstable/linear/LPSolver.h>
#include <gtsam_unstable/linear/LPInitSolver.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
@ -48,36 +50,26 @@ static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1);
LP simpleLP1() {
LP lp;
lp.cost = LinearCost(1, Vector2(-1., -1.)); // min -x1-x2 (max x1+x2)
lp.inequalities.push_back(
LinearInequality(1, Vector2(-1, 0), 0, 1)); // x1 >= 0
lp.inequalities.push_back(
LinearInequality(1, Vector2(0, -1), 0, 2)); // x2 >= 0
lp.inequalities.push_back(
LinearInequality(1, Vector2(1, 2), 4, 3)); // x1 + 2*x2 <= 4
lp.inequalities.push_back(
LinearInequality(1, Vector2(4, 2), 12, 4)); // 4x1 + 2x2 <= 12
lp.inequalities.push_back(
LinearInequality(1, Vector2(-1, 1), 1, 5)); // -x1 + x2 <= 1
lp.inequalities.add(1, Vector2(-1, 0), 0, 1); // x1 >= 0
lp.inequalities.add(1, Vector2(0, -1), 0, 2); // x2 >= 0
lp.inequalities.add(1, Vector2(1, 2), 4, 3); // x1 + 2*x2 <= 4
lp.inequalities.add(1, Vector2(4, 2), 12, 4); // 4x1 + 2x2 <= 12
lp.inequalities.add(1, Vector2(-1, 1), 1, 5); // -x1 + x2 <= 1
return lp;
}
/* ************************************************************************* */
namespace gtsam {
TEST(LPInitSolver, 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);
TEST(LPInitSolver, InfiniteLoopSingleVar) {
LP lp;
lp.cost = LinearCost(1, Vector3(0, 0, 1)); // min alpha
lp.inequalities.add(1, Vector3(-2, -1, -1), -2, 1); //-2x-y-a <= -2
lp.inequalities.add(1, Vector3(-1, 2, -1), 6, 2); // -x+2y-a <= 6
lp.inequalities.add(1, Vector3(-1, 0, -1), 0, 3); // -x - a <= 0
lp.inequalities.add(1, Vector3(1, 0, -1), 20, 4); // x - a <= 20
lp.inequalities.add(1, Vector3(0, -1, -1), 0, 5); // -y - a <= 0
LPSolver solver(lp);
VectorValues starter;
starter.insert(1, Vector3(0, 0, 2));
VectorValues results, duals;
@ -87,25 +79,23 @@ TEST(LPInitSolver, infinite_loop_single_var) {
CHECK(assert_equal(results, expected, 1e-7));
}
TEST(LPInitSolver, infinite_loop_multi_var) {
LP initchecker;
TEST(LPInitSolver, InfiniteLoopMultiVar) {
LP lp;
Key X = symbol('X', 1);
Key Y = symbol('Y', 1);
Key Z = symbol('Z', 1);
initchecker.cost = LinearCost(Z, kOne); // min alpha
initchecker.inequalities.push_back(
LinearInequality(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2,
1)); //-2x-y-alpha <= -2
initchecker.inequalities.push_back(
LinearInequality(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6,
2)); // -x+2y-alpha <= 6
initchecker.inequalities.push_back(LinearInequality(
X, -1.0 * kOne, Z, -1.0 * kOne, 0, 3)); // -x - alpha <= 0
initchecker.inequalities.push_back(LinearInequality(
X, 1.0 * kOne, Z, -1.0 * kOne, 20, 4)); // x - alpha <= 20
initchecker.inequalities.push_back(LinearInequality(
Y, -1.0 * kOne, Z, -1.0 * kOne, 0, 5)); // -y - alpha <= 0
LPSolver solver(initchecker);
lp.cost = LinearCost(Z, kOne); // min alpha
lp.inequalities.add(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2,
1); //-2x-y-alpha <= -2
lp.inequalities.add(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6,
2); // -x+2y-alpha <= 6
lp.inequalities.add(X, -1.0 * kOne, Z, -1.0 * kOne, 0,
3); // -x - alpha <= 0
lp.inequalities.add(X, 1.0 * kOne, Z, -1.0 * kOne, 20,
4); // x - alpha <= 20
lp.inequalities.add(Y, -1.0 * kOne, Z, -1.0 * kOne, 0,
5); // -y - alpha <= 0
LPSolver solver(lp);
VectorValues starter;
starter.insert(X, kZero);
starter.insert(Y, kZero);
@ -119,7 +109,7 @@ TEST(LPInitSolver, infinite_loop_multi_var) {
CHECK(assert_equal(results, expected, 1e-7));
}
TEST(LPInitSolver, initialization) {
TEST(LPInitSolver, Initialization) {
LP lp = simpleLP1();
LPInitSolver initSolver(lp);
@ -138,19 +128,19 @@ TEST(LPInitSolver, initialization) {
LP::shared_ptr initLP = initSolver.buildInitialLP(yKey);
LP expectedInitLP;
expectedInitLP.cost = LinearCost(yKey, kOne);
expectedInitLP.inequalities.push_back(LinearInequality(
1, Vector2(-1, 0), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0
expectedInitLP.inequalities.push_back(LinearInequality(
1, Vector2(0, -1), 2, Vector::Constant(1, -1), 0, 2)); // -x2 - y <= 0
expectedInitLP.inequalities.push_back(
LinearInequality(1, Vector2(1, 2), 2, Vector::Constant(1, -1), 4,
3)); // x1 + 2*x2 - y <= 4
expectedInitLP.inequalities.push_back(
LinearInequality(1, Vector2(4, 2), 2, Vector::Constant(1, -1), 12,
4)); // 4x1 + 2x2 - y <= 12
expectedInitLP.inequalities.push_back(
LinearInequality(1, Vector2(-1, 1), 2, Vector::Constant(1, -1), 1,
5)); // -x1 + x2 - y <= 1
expectedInitLP.inequalities.add(1, Vector2(-1, 0), 2, Vector::Constant(1, -1),
0, 1); // -x1 - y <= 0
expectedInitLP.inequalities.add(1, Vector2(0, -1), 2, Vector::Constant(1, -1),
0, 2); // -x2 - y <= 0
expectedInitLP.inequalities.add(1, Vector2(1, 2), 2, Vector::Constant(1, -1),
4,
3); // x1 + 2*x2 - y <= 4
expectedInitLP.inequalities.add(1, Vector2(4, 2), 2, Vector::Constant(1, -1),
12,
4); // 4x1 + 2x2 - y <= 12
expectedInitLP.inequalities.add(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);
@ -164,7 +154,7 @@ TEST(LPInitSolver, initialization) {
VectorValues x = initSolver.solve();
CHECK(lp.isFeasible(x));
}
}
} // namespace gtsam
/* ************************************************************************* */
/**
@ -173,28 +163,24 @@ TEST(LPInitSolver, initialization) {
* x - y = 5
* x + 2y = 6
*/
TEST(LPSolver, overConstrainedLinearSystem) {
TEST(LPSolver, OverConstrainedLinearSystem) {
GaussianFactorGraph graph;
Matrix A1 = Vector3(1, 1, 1);
Matrix A2 = Vector3(1, -1, 2);
Vector b = Vector3(1, 5, 6);
JacobianFactor factor(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
graph.push_back(factor);
graph.add(1, A1, 2, A2, b, noiseModel::Constrained::All(3));
VectorValues x = graph.optimize();
// This check confirms that gtsam linear constraint solver can't handle
// over-constrained system
CHECK(factor.error(x) != 0.0);
CHECK(graph[0]->error(x) != 0.0);
}
TEST(LPSolver, overConstrainedLinearSystem2) {
GaussianFactorGraph graph;
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, I_1x1, kOne,
noiseModel::Constrained::All(1));
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, -I_1x1, 5 * kOne,
noiseModel::Constrained::All(1));
graph.emplace_shared<JacobianFactor>(1, I_1x1, 2, 2 * I_1x1, 6 * kOne,
noiseModel::Constrained::All(1));
graph.add(1, I_1x1, 2, I_1x1, kOne, noiseModel::Constrained::All(1));
graph.add(1, I_1x1, 2, -I_1x1, 5 * kOne, noiseModel::Constrained::All(1));
graph.add(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, noiseModel::Constrained::All(1));
VectorValues x = graph.optimize();
// This check confirms that gtsam linear constraint solver can't handle
// over-constrained system
@ -202,7 +188,7 @@ TEST(LPSolver, overConstrainedLinearSystem2) {
}
/* ************************************************************************* */
TEST(LPSolver, simpleTest1) {
TEST(LPSolver, SimpleTest1) {
LP lp = simpleLP1();
LPSolver lpSolver(lp);
VectorValues init;
@ -222,7 +208,7 @@ TEST(LPSolver, simpleTest1) {
}
/* ************************************************************************* */
TEST(LPSolver, testWithoutInitialValues) {
TEST(LPSolver, TestWithoutInitialValues) {
LP lp = simpleLP1();
LPSolver lpSolver(lp);
VectorValues result, duals, expectedResult;

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

@ -17,10 +17,12 @@
* @author Ivan Dario Jimenez
*/
#include <gtsam_unstable/linear/QPSParser.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam/base/Testable.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/linear/QPSolver.h>
#include <gtsam_unstable/linear/QPSParser.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
@ -40,15 +42,15 @@ QP createTestCase() {
// Hence, we have G11=2, G12 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10
//TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of 10 because the form of the equation
// Should be 0.5x'Gx + gx + f : Nocedal 449
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, 2.0 * I_1x1,
Z_1x1, 10.0));
qp.cost.push_back(HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1,
2.0 * I_1x1, Z_1x1, 10.0));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2
qp.inequalities.add(X(1), I_1x1, X(2), I_1x1, 2,
0); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2
qp.inequalities.add(X(1), -I_1x1, 0, 1); // -x1 <= 0
qp.inequalities.add(X(2), -I_1x1, 0, 2); // -x2 <= 0
qp.inequalities.add(X(1), I_1x1, 1.5, 3); // x1 <= 3/2
return qp;
}
@ -94,16 +96,15 @@ QP createEqualityConstrainedTest() {
// Note the Hessian encodes:
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// Hence, we have G11=2, G12 = 0, g1 = 0, G22 = 2, g2 = 0, f = 0
qp.cost.push_back(
HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1,
0.0));
qp.cost.push_back(HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1,
2.0 * I_1x1, Z_1x1, 0.0));
// Equality constraints
// x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector
Matrix A1 = I_1x1;
Matrix A2 = I_1x1;
Vector b = -kOne;
qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0));
qp.equalities.add(X(1), A1, X(2), A2, b, 0);
return qp;
}
@ -118,9 +119,8 @@ TEST(QPSolver, dual) {
QPSolver solver(qp);
GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph(
qp.inequalities, initialValues);
VectorValues dual = dualGraph->optimize();
auto dualGraph = solver.buildDualGraph(qp.inequalities, initialValues);
VectorValues dual = dualGraph.optimize();
VectorValues expectedDual;
expectedDual.insert(0, (Vector(1) << 2.0).finished());
CHECK(assert_equal(expectedDual, dual, 1e-10));
@ -135,19 +135,19 @@ TEST(QPSolver, indentifyActiveConstraints) {
currentSolution.insert(X(1), Z_1x1);
currentSolution.insert(X(2), Z_1x1);
InequalityFactorGraph workingSet = solver.identifyActiveConstraints(
qp.inequalities, currentSolution);
auto workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
CHECK(!workingSet.at(0)->active()); // inactive
CHECK(workingSet.at(1)->active());// active
CHECK(workingSet.at(2)->active());// active
CHECK(!workingSet.at(3)->active());// inactive
CHECK(workingSet.at(1)->active()); // active
CHECK(workingSet.at(2)->active()); // active
CHECK(!workingSet.at(3)->active()); // inactive
VectorValues solution = solver.buildWorkingGraph(workingSet).optimize();
VectorValues expectedSolution;
expectedSolution.insert(X(1), kZero);
expectedSolution.insert(X(2), kZero);
CHECK(assert_equal(expectedSolution, solution, 1e-100));
VectorValues expected;
expected.insert(X(1), kZero);
expected.insert(X(2), kZero);
CHECK(assert_equal(expected, solution, 1e-100));
}
/* ************************************************************************* */
@ -159,24 +159,24 @@ TEST(QPSolver, iterate) {
currentSolution.insert(X(1), Z_1x1);
currentSolution.insert(X(2), Z_1x1);
std::vector<VectorValues> expectedSolutions(4), expectedDuals(4);
expectedSolutions[0].insert(X(1), kZero);
expectedSolutions[0].insert(X(2), kZero);
std::vector<VectorValues> expected(4), expectedDuals(4);
expected[0].insert(X(1), kZero);
expected[0].insert(X(2), kZero);
expectedDuals[0].insert(1, (Vector(1) << 3).finished());
expectedDuals[0].insert(2, kZero);
expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[1].insert(X(2), kZero);
expected[1].insert(X(1), (Vector(1) << 1.5).finished());
expected[1].insert(X(2), kZero);
expectedDuals[1].insert(3, (Vector(1) << 1.5).finished());
expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[2].insert(X(2), (Vector(1) << 0.75).finished());
expected[2].insert(X(1), (Vector(1) << 1.5).finished());
expected[2].insert(X(2), (Vector(1) << 0.75).finished());
expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished());
expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished());
expected[3].insert(X(1), (Vector(1) << 1.5).finished());
expected[3].insert(X(2), (Vector(1) << 0.5).finished());
InequalityFactorGraph workingSet = solver.identifyActiveConstraints(
qp.inequalities, currentSolution);
auto workingSet =
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
QPSolver::State state(currentSolution, VectorValues(), workingSet, false,
100);
@ -188,12 +188,12 @@ TEST(QPSolver, iterate) {
// Forst10book do not follow exactly what we implemented from Nocedal06book.
// Specifically, we do not re-identify active constraints and
// do not recompute dual variables after every step!!!
// CHECK(assert_equal(expectedSolutions[it], state.values, 1e-10));
// CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10));
// CHECK(assert_equal(expected[it], state.values, 1e-10));
// CHECK(assert_equal(expectedDuals[it], state.duals, 1e-10));
it++;
}
CHECK(assert_equal(expectedSolutions[3], state.values, 1e-10));
CHECK(assert_equal(expected[3], state.values, 1e-10));
}
/* ************************************************************************* */
@ -204,182 +204,161 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
VectorValues initialValues;
initialValues.insert(X(1), Z_1x1);
initialValues.insert(X(2), Z_1x1);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 1.5).finished());
expectedSolution.insert(X(2), (Vector(1) << 0.5).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-100));
VectorValues solution = solver.optimize(initialValues).first;
VectorValues expected;
expected.insert(X(1), (Vector(1) << 1.5).finished());
expected.insert(X(2), (Vector(1) << 0.5).finished());
CHECK(assert_equal(expected, solution, 1e-100));
}
pair<QP, QP> testParser(QPSParser parser) {
QP exampleqp = parser.Parse();
QP expectedqp;
QP expected;
Key X1(Symbol('X', 1)), X2(Symbol('X', 2));
// min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2
expectedqp.cost.push_back(
HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1,
2.0 * kOne, 8.0));
// 2x + y >= 2
// -x + 2y <= 6
expectedqp.inequalities.push_back(
LinearInequality(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0));
expectedqp.inequalities.push_back(
LinearInequality(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1));
// x<= 20
expectedqp.inequalities.push_back(LinearInequality(X1, I_1x1, 20, 4));
//x >= 0
expectedqp.inequalities.push_back(LinearInequality(X1, -I_1x1, 0, 2));
// y > = 0
expectedqp.inequalities.push_back(LinearInequality(X2, -I_1x1, 0, 3));
return std::make_pair(expectedqp, exampleqp);
}
;
expected.cost.push_back(HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1,
-1.5 * kOne, 10.0 * I_1x1, 2.0 * kOne,
8.0));
expected.inequalities.add(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0); // 2x + y >= 2
expected.inequalities.add(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1); // -x + 2y <= 6
expected.inequalities.add(X1, I_1x1, 20, 4); // x<= 20
expected.inequalities.add(X1, -I_1x1, 0, 2); // x >= 0
expected.inequalities.add(X2, -I_1x1, 0, 3); // y > = 0
return {expected, exampleqp};
};
TEST(QPSolver, ParserSyntaticTest) {
auto expectedActual = testParser(QPSParser("QPExample.QPS"));
CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost,
auto result = testParser(QPSParser("QPExample.QPS"));
CHECK(assert_equal(result.first.cost, result.second.cost, 1e-7));
CHECK(assert_equal(result.first.inequalities, result.second.inequalities,
1e-7));
CHECK(assert_equal(expectedActual.first.inequalities,
expectedActual.second.inequalities, 1e-7));
CHECK(assert_equal(expectedActual.first.equalities,
expectedActual.second.equalities, 1e-7));
CHECK(assert_equal(result.first.equalities, result.second.equalities, 1e-7));
}
TEST(QPSolver, ParserSemanticTest) {
auto expected_actual = testParser(QPSParser("QPExample.QPS"));
VectorValues actualSolution, expectedSolution;
boost::tie(expectedSolution, boost::tuples::ignore) =
QPSolver(expected_actual.first).optimize();
boost::tie(actualSolution, boost::tuples::ignore) =
QPSolver(expected_actual.second).optimize();
CHECK(assert_equal(actualSolution, expectedSolution, 1e-7));
auto result = testParser(QPSParser("QPExample.QPS"));
VectorValues expected = QPSolver(result.first).optimize().first;
VectorValues actual = QPSolver(result.second).optimize().first;
CHECK(assert_equal(actual, expected, 1e-7));
}
TEST(QPSolver, QPExampleTest){
TEST(QPSolver, QPExampleTest) {
QP problem = QPSParser("QPExample.QPS").Parse();
VectorValues actualSolution;
auto solver = QPSolver(problem);
boost::tie(actualSolution, boost::tuples::ignore) = solver.optimize();
VectorValues expectedSolution;
expectedSolution.insert(Symbol('X',1),0.7625*I_1x1);
expectedSolution.insert(Symbol('X',2),0.4750*I_1x1);
double error_expected = problem.cost.error(expectedSolution);
double error_actual = problem.cost.error(actualSolution);
CHECK(assert_equal(expectedSolution, actualSolution, 1e-7))
VectorValues actual = solver.optimize().first;
VectorValues expected;
expected.insert(Symbol('X', 1), 0.7625 * I_1x1);
expected.insert(Symbol('X', 2), 0.4750 * I_1x1);
double error_expected = problem.cost.error(expected);
double error_actual = problem.cost.error(actual);
CHECK(assert_equal(expected, actual, 1e-7))
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);
VectorValues expected;
expected.insert(Symbol('X', 1), 2.0 * I_1x1);
expected.insert(Symbol('X', 2), 0.0 * I_1x1);
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
CHECK(assert_equal(-99.9599999, error_actual, 1e-7))
CHECK(assert_equal(expectedSolution, actualSolution))
CHECK(assert_equal(expected, actual))
}
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))
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
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))
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
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))
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
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))
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
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
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))
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
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))
VectorValues actual = QPSolver(problem).optimize().first;
double error_actual = problem.cost.error(actual);
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 qp;
// Objective functions 0.5*x1^2 + x2^2 - x1*x2 - 2*x1 -6*x2
// Note the Hessian encodes:
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 + 0.5*f
// 0.5*x1'*G11*x1 + x1'*G12*x2 + 0.5*x2'*G22*x2 - x1'*g1 - x2'*g2 +
// 0.5*f
// Hence, we have G11=1, G12 = -1, g1 = +2, G22 = 2, g2 = +6, f = 0
qp.cost.push_back(
HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, 2.0 * I_1x1,
6 * I_1x1, 1000.0));
qp.cost.push_back(HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1,
2.0 * I_1x1, 6 * I_1x1, 1000.0));
// Inequality constraints
qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2
qp.inequalities.push_back(
LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2
qp.inequalities.push_back(
LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 3)); // -x1 <= 0
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); // -x2 <= 0
qp.inequalities.add(X(1), I_1x1, X(2), I_1x1, 2, 0); // x1 + x2 <= 2
qp.inequalities.add(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1); //-x1 + 2*x2 <=2
qp.inequalities.add(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2); // 2*x1 + x2 <=3
qp.inequalities.add(X(1), -I_1x1, 0, 3); // -x1 <= 0
qp.inequalities.add(X(2), -I_1x1, 0, 4); // -x2 <= 0
return qp;
}
///* ************************************************************************* */
///* *************************************************************************
///*/
TEST(QPSolver, optimizeMatlabEx) {
QP qp = createTestMatlabQPEx();
QPSolver solver(qp);
VectorValues initialValues;
initialValues.insert(X(1), Z_1x1);
initialValues.insert(X(2), Z_1x1);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-7));
VectorValues solution = solver.optimize(initialValues).first;
VectorValues expected;
expected.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
expected.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
CHECK(assert_equal(expected, solution, 1e-7));
}
///* ************************************************************************* */
///* *************************************************************************
///*/
TEST(QPSolver, optimizeMatlabExNoinitials) {
QP qp = createTestMatlabQPEx();
QPSolver solver(qp);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize();
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-7));
VectorValues solution = solver.optimize().first;
VectorValues expected;
expected.insert(X(1), (Vector(1) << 2.0 / 3.0).finished());
expected.insert(X(2), (Vector(1) << 4.0 / 3.0).finished());
CHECK(assert_equal(expected, solution, 1e-7));
}
/* ************************************************************************* */
@ -387,18 +366,15 @@ TEST(QPSolver, optimizeMatlabExNoinitials) {
QP createTestNocedal06bookEx16_4() {
QP qp;
qp.cost.push_back(JacobianFactor(X(1), I_1x1, I_1x1));
qp.cost.push_back(JacobianFactor(X(2), I_1x1, 2.5 * I_1x1));
qp.cost.add(X(1), I_1x1, I_1x1);
qp.cost.add(X(2), I_1x1, 2.5 * I_1x1);
// Inequality constraints
qp.inequalities.push_back(
LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0));
qp.inequalities.push_back(
LinearInequality(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1));
qp.inequalities.push_back(
LinearInequality(X(1), I_1x1, X(2), -2 * I_1x1, 2, 2));
qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0.0, 3));
qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0.0, 4));
qp.inequalities.add(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0);
qp.inequalities.add(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1);
qp.inequalities.add(X(1), I_1x1, X(2), -2 * I_1x1, 2, 2);
qp.inequalities.add(X(1), -I_1x1, 0.0, 3);
qp.inequalities.add(X(2), -I_1x1, 0.0, 4);
return qp;
}
@ -410,21 +386,19 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) {
initialValues.insert(X(1), (Vector(1) << 2.0).finished());
initialValues.insert(X(2), Z_1x1);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues expectedSolution;
expectedSolution.insert(X(1), (Vector(1) << 1.4).finished());
expectedSolution.insert(X(2), (Vector(1) << 1.7).finished());
CHECK(assert_equal(expectedSolution, solution, 1e-7));
VectorValues solution = solver.optimize(initialValues).first;
VectorValues expected;
expected.insert(X(1), (Vector(1) << 1.4).finished());
expected.insert(X(2), (Vector(1) << 1.7).finished());
CHECK(assert_equal(expected, solution, 1e-7));
}
/* ************************************************************************* */
TEST(QPSolver, failedSubproblem) {
QP qp;
qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1));
qp.cost.add(X(1), I_2x2, Z_2x1);
qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0));
qp.inequalities.push_back(
LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0));
qp.inequalities.add(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0);
VectorValues expected;
expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished());
@ -433,8 +407,7 @@ TEST(QPSolver, failedSubproblem) {
initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished());
QPSolver solver(qp);
VectorValues solution;
boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues);
VectorValues solution = solver.optimize(initialValues).first;
CHECK(assert_equal(expected, solution, 1e-7));
}
@ -442,10 +415,9 @@ TEST(QPSolver, failedSubproblem) {
/* ************************************************************************* */
TEST(QPSolver, infeasibleInitial) {
QP qp;
qp.cost.push_back(JacobianFactor(X(1), I_2x2, Vector::Zero(2)));
qp.cost.add(X(1), I_2x2, Vector::Zero(2));
qp.cost.push_back(HessianFactor(X(1), Z_2x2, Vector::Zero(2), 100.0));
qp.inequalities.push_back(
LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0));
qp.inequalities.add(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0);
VectorValues expected;
expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished());
@ -464,4 +436,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */