Get rid of deprecated functions
dellaert
parent
272303bc90
commit
9cd6f0b066
|
|
@ -139,7 +139,7 @@ TEST(LPInitSolver, initialization) {
|
|||
xy0.insert(yKey, Vector::Constant(1, y0));
|
||||
VectorValues xyInit = lpSolveInit.optimize(xy0).first;
|
||||
VectorValues expected_init;
|
||||
expected_init.insert(1, Vector2( 1, 1));
|
||||
expected_init.insert(1, Vector::Ones(2));
|
||||
expected_init.insert(2, Vector::Constant(1, -1));
|
||||
CHECK(assert_equal(expected_init, xyInit, 1e-10));
|
||||
|
||||
|
|
@ -188,7 +188,7 @@ init.insert(1, Vector::Zero(2));
|
|||
VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init,
|
||||
InequalityFactorGraph());
|
||||
VectorValues expected_x1;
|
||||
expected_x1.insert(1, Vector2( 1, 1));
|
||||
expected_x1.insert(1, Vector::Ones(2));
|
||||
CHECK(assert_equal(expected_x1, x1, 1e-10));
|
||||
|
||||
VectorValues result, duals;
|
||||
|
|
|
|||
|
|
@ -28,7 +28,7 @@ using namespace std;
|
|||
using namespace gtsam;
|
||||
using namespace gtsam::symbol_shorthand;
|
||||
|
||||
const Matrix One = I_1x1;
|
||||
static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1);
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Create test graph according to Forst10book_pg171Ex5
|
||||
|
|
@ -42,8 +42,8 @@ QP createTestCase() {
|
|||
//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 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * I_1x1,
|
||||
2.0 * Matrix::Ones(1, 1), Z_1x1, 10.0));
|
||||
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
|
||||
|
|
@ -57,8 +57,8 @@ QP createTestCase() {
|
|||
TEST(QPSolver, TestCase) {
|
||||
VectorValues values;
|
||||
double x1 = 5, x2 = 7;
|
||||
values.insert(X(1), x1 * Matrix::Ones(1, 1));
|
||||
values.insert(X(2), x2 * Matrix::Ones(1, 1));
|
||||
values.insert(X(1), x1 * I_1x1);
|
||||
values.insert(X(2), x2 * I_1x1);
|
||||
QP qp = createTestCase();
|
||||
DOUBLES_EQUAL(29, x1 * x1 - x1 * x2 + x2 * x2 - 3 * x1 + 5, 1e-9);
|
||||
DOUBLES_EQUAL(29, qp.cost[0]->error(values), 1e-9);
|
||||
|
|
@ -71,7 +71,7 @@ TEST(QPSolver, constraintsAux) {
|
|||
|
||||
VectorValues lambdas;
|
||||
lambdas.insert(0, (Vector(1) << -0.5).finished());
|
||||
lambdas.insert(1, (Vector(1) << 0.0).finished());
|
||||
lambdas.insert(1, kZero);
|
||||
lambdas.insert(2, (Vector(1) << 0.3).finished());
|
||||
lambdas.insert(3, (Vector(1) << 0.1).finished());
|
||||
int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas);
|
||||
|
|
@ -79,7 +79,7 @@ TEST(QPSolver, constraintsAux) {
|
|||
|
||||
VectorValues lambdas2;
|
||||
lambdas2.insert(0, (Vector(1) << -0.5).finished());
|
||||
lambdas2.insert(1, (Vector(1) << 0.0).finished());
|
||||
lambdas2.insert(1, kZero);
|
||||
lambdas2.insert(2, (Vector(1) << -0.3).finished());
|
||||
lambdas2.insert(3, (Vector(1) << -0.1).finished());
|
||||
int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2);
|
||||
|
|
@ -96,14 +96,14 @@ QP createEqualityConstrainedTest() {
|
|||
// 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 * Matrix::Ones(1, 1), Z_1x1, Z_1x1,
|
||||
2.0 * Matrix::Ones(1, 1), Z_1x1, 0.0));
|
||||
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 = (Matrix(1, 1) << 1).finished();
|
||||
Matrix A2 = (Matrix(1, 1) << 1).finished();
|
||||
Vector b = -(Vector(1) << 1).finished();
|
||||
Matrix A1 = I_1x1;
|
||||
Matrix A2 = I_1x1;
|
||||
Vector b = -kOne;
|
||||
qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0));
|
||||
|
||||
return qp;
|
||||
|
|
@ -119,7 +119,8 @@ TEST(QPSolver, dual) {
|
|||
|
||||
QPSolver solver(qp);
|
||||
|
||||
GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph(qp.inequalities, initialValues);
|
||||
GaussianFactorGraph::shared_ptr dualGraph = solver.buildDualGraph(
|
||||
qp.inequalities, initialValues);
|
||||
VectorValues dual = dualGraph->optimize();
|
||||
VectorValues expectedDual;
|
||||
expectedDual.insert(0, (Vector(1) << 2.0).finished());
|
||||
|
|
@ -135,18 +136,18 @@ TEST(QPSolver, indentifyActiveConstraints) {
|
|||
currentSolution.insert(X(1), Z_1x1);
|
||||
currentSolution.insert(X(2), Z_1x1);
|
||||
|
||||
InequalityFactorGraph workingSet =
|
||||
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
|
||||
InequalityFactorGraph 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.solveWithCurrentWorkingSet(workingSet);
|
||||
VectorValues expectedSolution;
|
||||
expectedSolution.insert(X(1), (Vector(1) << 0.0).finished());
|
||||
expectedSolution.insert(X(2), (Vector(1) << 0.0).finished());
|
||||
expectedSolution.insert(X(1), kZero);
|
||||
expectedSolution.insert(X(2), kZero);
|
||||
CHECK(assert_equal(expectedSolution, solution, 1e-100));
|
||||
|
||||
}
|
||||
|
|
@ -161,13 +162,13 @@ TEST(QPSolver, iterate) {
|
|||
currentSolution.insert(X(2), Z_1x1);
|
||||
|
||||
std::vector<VectorValues> expectedSolutions(4), expectedDuals(4);
|
||||
expectedSolutions[0].insert(X(1), (Vector(1) << 0.0).finished());
|
||||
expectedSolutions[0].insert(X(2), (Vector(1) << 0.0).finished());
|
||||
expectedSolutions[0].insert(X(1), kZero);
|
||||
expectedSolutions[0].insert(X(2), kZero);
|
||||
expectedDuals[0].insert(1, (Vector(1) << 3).finished());
|
||||
expectedDuals[0].insert(2, (Vector(1) << 0).finished());
|
||||
expectedDuals[0].insert(2, kZero);
|
||||
|
||||
expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished());
|
||||
expectedSolutions[1].insert(X(2), (Vector(1) << 0.0).finished());
|
||||
expectedSolutions[1].insert(X(2), kZero);
|
||||
expectedDuals[1].insert(3, (Vector(1) << 1.5).finished());
|
||||
|
||||
expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished());
|
||||
|
|
@ -176,8 +177,8 @@ TEST(QPSolver, iterate) {
|
|||
expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished());
|
||||
expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished());
|
||||
|
||||
InequalityFactorGraph workingSet =
|
||||
solver.identifyActiveConstraints(qp.inequalities, currentSolution);
|
||||
InequalityFactorGraph workingSet = solver.identifyActiveConstraints(
|
||||
qp.inequalities, currentSolution);
|
||||
|
||||
QPState state(currentSolution, VectorValues(), workingSet, false, 100);
|
||||
|
||||
|
|
@ -214,38 +215,46 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) {
|
|||
pair<QP, QP> testParser(QPSParser parser) {
|
||||
QP exampleqp = parser.Parse();
|
||||
QP expectedqp;
|
||||
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
|
||||
expectedqp.cost.push_back(
|
||||
HessianFactor(X1, X2, 8.0 * ones(1, 1), 2.0 * ones(1, 1),
|
||||
1.5 * ones(1), 10.0 * ones(1, 1), -2.0 * ones(1), 4.0));
|
||||
HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, 1.5 * kOne,
|
||||
10.0 * I_1x1, -2.0 * kOne, 4.0));
|
||||
// 2x + y >= 2
|
||||
// -x + 2y <= 6
|
||||
expectedqp.inequalities.push_back(
|
||||
LinearInequality(X2, -ones(1, 1), X1, -2.0 * ones(1, 1), -2, 0));
|
||||
LinearInequality(X2, -I_1x1, X1, -2.0 * I_1x1, -2, 0));
|
||||
expectedqp.inequalities.push_back(
|
||||
LinearInequality(X2, 2.0 * ones(1, 1), X1, -ones(1, 1), 6, 1));
|
||||
LinearInequality(X2, 2.0 * I_1x1, X1, -I_1x1, 6, 1));
|
||||
// x<= 20
|
||||
expectedqp.inequalities.push_back(LinearInequality(X1, ones(1, 1), 20, 4));
|
||||
expectedqp.inequalities.push_back(LinearInequality(X1, I_1x1, 20, 4));
|
||||
//x >= 0
|
||||
expectedqp.inequalities.push_back(LinearInequality(X1, -ones(1, 1), 0, 2));
|
||||
expectedqp.inequalities.push_back(LinearInequality(X1, -I_1x1, 0, 2));
|
||||
// y > = 0
|
||||
expectedqp.inequalities.push_back(LinearInequality(X2, -ones(1, 1), 0, 3));
|
||||
expectedqp.inequalities.push_back(LinearInequality(X2, -I_1x1, 0, 3));
|
||||
return std::make_pair(expectedqp, exampleqp);
|
||||
};
|
||||
}
|
||||
;
|
||||
|
||||
TEST(QPSolver, ParserSyntaticTest){
|
||||
TEST(QPSolver, ParserSyntaticTest) {
|
||||
auto expectedActual = testParser(QPSParser("QPExample.QPS"));
|
||||
CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost, 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(expectedActual.first.cost, expectedActual.second.cost, 1e-7));
|
||||
CHECK(
|
||||
assert_equal(expectedActual.first.inequalities,
|
||||
expectedActual.second.inequalities, 1e-7));
|
||||
CHECK(
|
||||
assert_equal(expectedActual.first.equalities,
|
||||
expectedActual.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();
|
||||
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));
|
||||
}
|
||||
|
||||
|
|
@ -259,15 +268,15 @@ QP createTestMatlabQPEx() {
|
|||
// 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, -Matrix::Ones(1, 1), 2.0 * I_1x1,
|
||||
2.0 * Matrix::Ones(1, 1), 6 * I_1x1, 1000.0));
|
||||
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), One, X(2), One, 2, 0)); // x1 + x2 <= 2
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 1)); //-x1 + 2*x2 <=2
|
||||
qp.inequalities.push_back(LinearInequality(X(1), 2 * One, X(2), One, 3, 2)); // 2*x1 + x2 <=3
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -One, 0, 3)); // -x1 <= 0
|
||||
qp.inequalities.push_back(LinearInequality(X(2), -One, 0, 4)); // -x2 <= 0
|
||||
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
|
||||
|
||||
return qp;
|
||||
}
|
||||
|
|
@ -304,18 +313,18 @@ TEST(QPSolver, optimizeMatlabExNoinitials) {
|
|||
QP createTestNocedal06bookEx16_4() {
|
||||
QP qp;
|
||||
|
||||
qp.cost.push_back(JacobianFactor(X(1), Matrix::Ones(1, 1), I_1x1));
|
||||
qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * I_1x1));
|
||||
qp.cost.push_back(JacobianFactor(X(1), I_1x1, I_1x1));
|
||||
qp.cost.push_back(JacobianFactor(X(2), I_1x1, 2.5 * I_1x1));
|
||||
|
||||
// Inequality constraints
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), 2, 0));
|
||||
LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0));
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6, 1));
|
||||
LinearInequality(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1));
|
||||
qp.inequalities.push_back(
|
||||
LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2, 2));
|
||||
qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), 0.0, 3));
|
||||
qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), 0.0, 4));
|
||||
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));
|
||||
|
||||
return qp;
|
||||
}
|
||||
|
|
@ -341,7 +350,7 @@ TEST(QPSolver, failedSubproblem) {
|
|||
qp.cost.push_back(JacobianFactor(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));
|
||||
LinearInequality(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());
|
||||
|
|
@ -359,10 +368,10 @@ TEST(QPSolver, failedSubproblem) {
|
|||
/* ************************************************************************* */
|
||||
TEST(QPSolver, infeasibleInitial) {
|
||||
QP qp;
|
||||
qp.cost.push_back(JacobianFactor(X(1), eye(2), zero(2)));
|
||||
qp.cost.push_back(HessianFactor(X(1), zeros(2, 2), zero(2), 100.0));
|
||||
qp.cost.push_back(JacobianFactor(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));
|
||||
LinearInequality(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());
|
||||
|
|
@ -372,10 +381,7 @@ TEST(QPSolver, infeasibleInitial) {
|
|||
|
||||
QPSolver solver(qp);
|
||||
VectorValues solution;
|
||||
CHECK_EXCEPTION(
|
||||
solver.optimize(initialValues),
|
||||
InfeasibleInitialValues
|
||||
);
|
||||
CHECK_EXCEPTION(solver.optimize(initialValues), InfeasibleInitialValues);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue