/* ---------------------------------------------------------------------------- * GTSAM Copyright 2010, Georgia Tech Research Corporation, * Atlanta, Georgia 30332-0415 * All Rights Reserved * Authors: Frank Dellaert, et al. (see THANKS for the full author list) * See LICENSE for the license information * -------------------------------------------------------------------------- */ /** * @file testQPSolver.cpp * @brief Test simple QP solver for a linear inequality constraint * @date Apr 10, 2014 * @author Duy-Nguyen Ta */ #include #include #include #include using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; const Matrix One = ones(1,1); /* ************************************************************************* */ // Create test graph according to Forst10book_pg171Ex5 QP createTestCase() { QP qp; // Objective functions x1^2 - x1*x2 + x2^2 - 3*x1 + 5 // 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 = -1, g1 = +3, G22 = 2, g2 = 0, f = 10 qp.cost.push_back( HessianFactor(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), 2.0 * ones(1, 1), zero(1), 10.0)); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), X(2), ones(1,1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 qp.inequalities.push_back(LinearInequality(X(1), -ones(1,1), 0, 1)); // -x1 <= 0 qp.inequalities.push_back(LinearInequality(X(2), -ones(1,1), 0, 2)); // -x2 <= 0 qp.inequalities.push_back(LinearInequality(X(1), ones(1,1), 1.5, 3)); // x1 <= 3/2 return qp; } TEST(QPSolver, TestCase) { VectorValues values; double x1 = 5, x2 = 7; values.insert(X(1), x1 * ones(1, 1)); values.insert(X(2), x2 * ones(1, 1)); 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); } TEST(QPSolver, constraintsAux) { QP qp = createTestCase(); QPSolver solver(qp); VectorValues lambdas; lambdas.insert(0, (Vector(1) << -0.5).finished()); lambdas.insert(1, (Vector(1) << 0.0).finished()); lambdas.insert(2, (Vector(1) << 0.3).finished()); lambdas.insert(3, (Vector(1) << 0.1).finished()); int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas); LONGS_EQUAL(2, factorIx); VectorValues lambdas2; lambdas2.insert(0, (Vector(1) << -0.5).finished()); lambdas2.insert(1, (Vector(1) << 0.0).finished()); lambdas2.insert(2, (Vector(1) << -0.3).finished()); lambdas2.insert(3, (Vector(1) << -0.1).finished()); int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2); LONGS_EQUAL(-1, factorIx2); } /* ************************************************************************* */ // Create a simple test graph with one equality constraint QP createEqualityConstrainedTest() { QP qp; // Objective functions x1^2 + x2^2 // 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 * ones(1, 1), zeros(1, 1), zero(1), 2.0 * ones(1, 1), zero(1), 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(); qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0)); return qp; } TEST(QPSolver, dual) { QP qp = createEqualityConstrainedTest(); // Initials values VectorValues initialValues; initialValues.insert(X(1), ones(1)); initialValues.insert(X(2), ones(1)); QPSolver solver(qp); GaussianFactorGraph::shared_ptr 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)); } /* ************************************************************************* */ TEST(QPSolver, indentifyActiveConstraints) { QP qp = createTestCase(); QPSolver solver(qp); VectorValues currentSolution; currentSolution.insert(X(1), zero(1)); currentSolution.insert(X(2), zero(1)); LinearInequalityFactorGraph 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 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()); CHECK(assert_equal(expectedSolution, solution, 1e-100)); } /* ************************************************************************* */ TEST(QPSolver, iterate) { QP qp = createTestCase(); QPSolver solver(qp); VectorValues currentSolution; currentSolution.insert(X(1), zero(1)); currentSolution.insert(X(2), zero(1)); 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()); expectedDuals[0].insert(1, (Vector(1) << 3).finished()); expectedDuals[0].insert(2, (Vector(1) << 0).finished()); expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished()); expectedSolutions[1].insert(X(2), (Vector(1) << 0.0).finished()); 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()); expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished()); expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished()); LinearInequalityFactorGraph workingSet = solver.identifyActiveConstraints(qp.inequalities, currentSolution); QPState state(currentSolution, VectorValues(), workingSet, false); int it = 0; while (!state.converged) { state = solver.iterate(state); // These checks will fail because the expected solutions obtained from // 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)); it++; } CHECK(assert_equal(expectedSolutions[3], state.values, 1e-10)); } /* ************************************************************************* */ TEST(QPSolver, optimizeForst10book_pg171Ex5) { QP qp = createTestCase(); QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), zero(1)); initialValues.insert(X(2), zero(1)); 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)); } /* ************************************************************************* */ // 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 // 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 * ones(1, 1), -ones(1, 1), 2.0 * ones(1), 2.0 * ones(1, 1), 6 * ones(1), 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 return qp; } TEST(QPSolver, optimizeMatlabEx) { QP qp = createTestMatlabQPEx(); QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), zero(1)); initialValues.insert(X(2), zero(1)); 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)); } /* ************************************************************************* */ // Create test graph as in Nocedal06book, Ex 16.4, pg. 475 QP createTestNocedal06bookEx16_4() { QP qp; qp.cost.push_back(JacobianFactor(X(1), ones(1, 1), ones(1))); qp.cost.push_back(JacobianFactor(X(2), ones(1, 1), 2.5 * ones(1))); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0)); qp.inequalities.push_back(LinearInequality(X(1), One, X(2), 2 * One, 6, 1)); qp.inequalities.push_back(LinearInequality(X(1), One, X(2), -2 * One, 2, 2)); qp.inequalities.push_back(LinearInequality(X(1), -One, 0.0, 3)); qp.inequalities.push_back(LinearInequality(X(2), -One, 0.0, 4)); return qp; } TEST(QPSolver, optimizeNocedal06bookEx16_4) { QP qp = createTestNocedal06bookEx16_4(); QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), (Vector(1) << 2.0).finished()); initialValues.insert(X(2), zero(1)); 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)); } /* ************************************************************************* */ TEST(QPSolver, failedSubproblem) { 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.inequalities.push_back( 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()); VectorValues initialValues; 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); // graph.print("Graph: "); // solution.print("Solution: "); CHECK(assert_equal(expected, solution, 1e-7)); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */