/* ---------------------------------------------------------------------------- * 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; /* ************************************************************************* */ // 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 // Jacobian factors represent Ax-b, hence // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 Matrix A1 = (Matrix(4, 1) << 1, -1, 0, 1); Matrix A2 = (Matrix(4, 1) << 1, 0, -1, 0); Vector b = (Vector(4) << 2, 0, 0, 1.5); qp.inequalities.push_back(LinearInequality(X(1), A1, X(2), A2, b, 0)); 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(4) << -0.5, 0.0, 0.3, 0.1)); int factorIx, lambdaIx; boost::tie(factorIx, lambdaIx) = solver.identifyLeavingConstraint( qp.inequalities, lambdas); LONGS_EQUAL(0, factorIx); LONGS_EQUAL(2, lambdaIx); VectorValues lambdas2; lambdas2.insert(0, (Vector(4) << -0.5, 0.0, -0.3, -0.1)); int factorIx2, lambdaIx2; boost::tie(factorIx2, lambdaIx2) = solver.identifyLeavingConstraint( qp.inequalities, lambdas2); LONGS_EQUAL(-1, factorIx2); LONGS_EQUAL(-1, lambdaIx2); } /* ************************************************************************* */ // 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); Matrix A2 = (Matrix(1, 1) << 1); Vector b = -(Vector(1) << 1); 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)); 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); Vector actualSigmas = workingSet.at(0)->get_model()->sigmas(); Vector expectedSigmas = (Vector(4) << INACTIVE, ACTIVE, ACTIVE, INACTIVE); CHECK(assert_equal(expectedSigmas, actualSigmas, 1e-100)); VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet); VectorValues expectedSolution; expectedSolution.insert(X(1), (Vector(1) << 0.0)); expectedSolution.insert(X(2), (Vector(1) << 0.0)); 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)); expectedSolutions[0].insert(X(2), (Vector(1) << 0.0)); expectedDuals[0].insert(0, (Vector(4) << 0, 3, 0, 0)); expectedSolutions[1].insert(X(1), (Vector(1) << 1.5)); expectedSolutions[1].insert(X(2), (Vector(1) << 0.0)); expectedDuals[1].insert(0, (Vector(4) << 0, 0, 1.5, 0)); expectedSolutions[2].insert(X(1), (Vector(1) << 1.5)); expectedSolutions[2].insert(X(2), (Vector(1) << 0.75)); expectedDuals[2].insert(0, (Vector(4) << 0, 0, 1.5, 0)); expectedSolutions[3].insert(X(1), (Vector(1) << 1.5)); expectedSolutions[3].insert(X(2), (Vector(1) << 0.5)); expectedDuals[3].insert(0, (Vector(4) << -0.5, 0, 0, 0)); 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)); expectedSolution.insert(X(2), (Vector(1) << 0.5)); 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 // Jacobian factors represent Ax-b, hence // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 Matrix A1 = (Matrix(5, 1) << 1, -1, 2, -1, 0); Matrix A2 = (Matrix(5, 1) << 1, 2, 1, 0, -1); Vector b = (Vector(5) << 2, 2, 3, 0, 0); qp.inequalities.push_back(LinearInequality(X(1), A1, X(2), A2, b, 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)); expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0)); 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), -ones(1, 1), X(2), 2 * ones(1, 1), 2 * ones(1), 0)); qp.inequalities.push_back( LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), 1)); qp.inequalities.push_back( LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2 * ones(1), 2)); qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), zero(1), 3)); qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), zero(1), 4)); return qp; } TEST(QPSolver, optimizeNocedal06bookEx16_4) { QP qp = createTestNocedal06bookEx16_4(); QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), (Vector(1) << 2.0)); 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)); expectedSolution.insert(X(2), (Vector(1) << 1.7)); CHECK(assert_equal(expectedSolution, solution, 1e-7)); } /* ************************************************************************* */ /* Create test graph as in Nocedal06book, Ex 16.4, pg. 475 with the first constraint (16.49b) is replaced by x1 - 2 x2 - 1 >=0 so that the trivial initial point (0,0) is infeasible ==== H = [2 0; 0 2]; f = [-2; -5]; A =[-1 2; 1 2 1 -2]; b = [-1; 6; 2]; lb = zeros(2,1); opts = optimoptions('quadprog','Algorithm','active-set','Display','off'); [x,fval,exitflag,output,lambda] = ... quadprog(H,f,A,b,[],[],lb,[],[],opts); ==== x = 2.0000 0.5000 */ QP modifyNocedal06bookEx16_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 noiseModel::Constrained::shared_ptr noise = noiseModel::Constrained::MixedSigmas((Vector(1) << -1)); qp.inequalities.push_back( LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), -1 * ones(1), 0)); qp.inequalities.push_back( LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), 6 * ones(1), 1)); qp.inequalities.push_back( LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), 2 * ones(1), 2)); qp.inequalities.push_back(LinearInequality(X(1), -ones(1, 1), zero(1), 3)); qp.inequalities.push_back(LinearInequality(X(2), -ones(1, 1), zero(1), 4)); return qp; } TEST(QPSolver, optimizeNocedal06bookEx16_4_findInitialPoint) { QP qp = modifyNocedal06bookEx16_4(); QPSolver solver(qp); VectorValues initialsLP; Key firstSlackKey, lastSlackKey; boost::tie(initialsLP, firstSlackKey, lastSlackKey) = solver.initialValuesLP(); EXPECT(assert_equal(zero(1), initialsLP.at(X(1)))); EXPECT(assert_equal(zero(1), initialsLP.at(X(2)))); LONGS_EQUAL(X(2) + 1, firstSlackKey); EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey))); EXPECT(assert_equal(ones(1) * 6.0, initialsLP.at(firstSlackKey + 1))); EXPECT(assert_equal(ones(1) * 2.0, initialsLP.at(firstSlackKey + 2))); EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey + 3))); EXPECT(assert_equal(zero(1), initialsLP.at(firstSlackKey + 4))); VectorValues objCoeffs = solver.objectiveCoeffsLP(firstSlackKey); for (size_t i = 0; i < 5; ++i) EXPECT(assert_equal(ones(1), objCoeffs.at(firstSlackKey + i))); LinearEqualityFactorGraph::shared_ptr equalities; LinearInequalityFactorGraph::shared_ptr inequalities; VectorValues lowerBounds; boost::tie(equalities, inequalities, lowerBounds) = solver.constraintsLP( firstSlackKey); for (size_t i = 0; i < 5; ++i) EXPECT(assert_equal(zero(1), lowerBounds.at(firstSlackKey + i))); LinearInequalityFactorGraph expectedInequalities; expectedInequalities.push_back( LinearInequality(X(1), -ones(1, 1), X(2), 2 * ones(1, 1), X(3), -ones(1, 1), -1 * ones(1), 0)); expectedInequalities.push_back( LinearInequality(X(1), ones(1, 1), X(2), 2 * ones(1, 1), X(4), -ones(1, 1), 6 * ones(1), 1)); expectedInequalities.push_back( LinearInequality(X(1), ones(1, 1), X(2), -2 * ones(1, 1), X(5), -ones(1, 1), 2 * ones(1), 2)); expectedInequalities.push_back( LinearInequality(X(1), -ones(1, 1), X(6), -ones(1, 1), zero(1), 3)); expectedInequalities.push_back( LinearInequality(X(2), -ones(1, 1), X(7), -ones(1, 1), zero(1), 4)); EXPECT(assert_equal(expectedInequalities, *inequalities)); bool isFeasible; VectorValues initialValues; boost::tie(isFeasible, initialValues) = solver.findFeasibleInitialValues(); EXPECT(assert_equal(1.0 * ones(1), initialValues.at(X(1)))); EXPECT(assert_equal(0.0 * ones(1), initialValues.at(X(2)))); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(); EXPECT(assert_equal(2.0 * ones(1), solution.at(X(1)))); EXPECT(assert_equal(0.5 * ones(1), solution.at(X(2)))); } TEST(QPSolver, optimizeNocedal06bookEx16_4_2) { QP qp = createTestNocedal06bookEx16_4(); QPSolver solver(qp); VectorValues initialValues; initialValues.insert(X(1), (Vector(1) << 0.0)); initialValues.insert(X(2), (Vector(1) << 100.0)); VectorValues expectedSolution; expectedSolution.insert(X(1), (Vector(1) << 1.4)); expectedSolution.insert(X(2), (Vector(1) << 1.7)); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); // THIS should fail because of the bad infeasible initial point!! // CHECK(assert_equal(expectedSolution, solution, 1e-7)); VectorValues solution2; boost::tie(solution2, boost::tuples::ignore) = solver.optimize(); CHECK(assert_equal(expectedSolution, solution2, 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), -ones(1), 0)); VectorValues expected; expected.insert(X(1), (Vector(2) << 1.0, 0.0)); QPSolver solver(qp); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(); // graph.print("Graph: "); // solution.print("Solution: "); CHECK(assert_equal(expected, solution, 1e-7)); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */