Re-formatting and using "add"/"auto" where we can.
Frank Dellaert
parent
c51264ac98
commit
6b739b17be
|
|
@ -16,20 +16,20 @@
|
|||
* @author Duy-Nguyen Ta
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/inference/FactorGraph-inst.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.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/base/Testable.h>
|
||||
#include <gtsam/inference/FactorGraph-inst.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 <gtsam_unstable/linear/LPSolver.h>
|
||||
#include <gtsam_unstable/linear/LPInitSolver.h>
|
||||
#include <gtsam_unstable/linear/LPSolver.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -48,36 +48,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 +77,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 +107,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 +126,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 +152,7 @@ TEST(LPInitSolver, initialization) {
|
|||
VectorValues x = initSolver.solve();
|
||||
CHECK(lp.isFeasible(x));
|
||||
}
|
||||
}
|
||||
} // namespace gtsam
|
||||
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
|
|
@ -173,28 +161,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 +186,7 @@ TEST(LPSolver, overConstrainedLinearSystem2) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(LPSolver, simpleTest1) {
|
||||
TEST(LPSolver, SimpleTest1) {
|
||||
LP lp = simpleLP1();
|
||||
LPSolver lpSolver(lp);
|
||||
VectorValues init;
|
||||
|
|
@ -222,7 +206,7 @@ TEST(LPSolver, simpleTest1) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(LPSolver, testWithoutInitialValues) {
|
||||
TEST(LPSolver, TestWithoutInitialValues) {
|
||||
LP lp = simpleLP1();
|
||||
LPSolver lpSolver(lp);
|
||||
VectorValues result, duals, expectedResult;
|
||||
|
|
|
|||
|
|
@ -17,11 +17,11 @@
|
|||
* @author Ivan Dario Jimenez
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.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>
|
||||
#include <gtsam_unstable/linear/QPSolver.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
|
@ -40,15 +40,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 +94,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 +117,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,8 +133,8 @@ 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
|
||||
|
|
@ -144,10 +142,10 @@ TEST(QPSolver, indentifyActiveConstraints) {
|
|||
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 +157,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> expecteds(4), expectedDuals(4);
|
||||
expecteds[0].insert(X(1), kZero);
|
||||
expecteds[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);
|
||||
expecteds[1].insert(X(1), (Vector(1) << 1.5).finished());
|
||||
expecteds[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());
|
||||
expecteds[2].insert(X(1), (Vector(1) << 1.5).finished());
|
||||
expecteds[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());
|
||||
expecteds[3].insert(X(1), (Vector(1) << 1.5).finished());
|
||||
expecteds[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 +186,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(expecteds[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(expecteds[3], state.values, 1e-10));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -204,182 +202,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) {
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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);
|
||||
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 +364,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 +384,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 +405,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 +413,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 +434,3 @@ int main() {
|
|||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue