diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index ccd624ab2..13f7ad946 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -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; diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 4396a8541..24fd97d5e 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -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,14 +42,14 @@ 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 - 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.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 return qp; } @@ -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 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 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); } /* ************************************************************************* */