/* ---------------------------------------------------------------------------- * 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 #include #include #include #include #include #include #include #include #include using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; /* ************************************************************************* */ /** * min -x1-x2 * s.t. x1 + 2x2 <= 4 * 4x1 + 2x2 <= 12 * -x1 + x2 <= 1 * x1, x2 >= 0 */ 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 return lp; } /* ************************************************************************* */ namespace gtsam { TEST(LPInitSolverMatlab, infinite_loop_multi_var) { LP initchecker; Key X = symbol('X',1); Key Y = symbol('Y',1); Key Z = symbol('Z',1); initchecker.cost = LinearCost(Z, ones(1)); //min alpha // initchecker.cost = LinearCost(Z, ones(1), X, zero(1), Y, zero(1)); //min alpha initchecker.inequalities.push_back(LinearInequality(X,-2.0*ones(1), Y, -1.0*ones(1), Z, -1.0*ones(1),-2,1));//-2x-y-alpha <= -2 initchecker.inequalities.push_back(LinearInequality(X, -1.0*ones(1), Y, 2.0*ones(1), Z, -1.0*ones(1), 6, 2));// -x+2y-alpha <= 6 initchecker.inequalities.push_back(LinearInequality(X, -1.0*ones(1), Z, -1.0*ones(1), 0,3));// -x - alpha <= 0 initchecker.inequalities.push_back(LinearInequality(X, 1.0*ones(1), Z, -1.0*ones(1), 20, 4));//x - alpha <= 20 initchecker.inequalities.push_back(LinearInequality(Y, -1.0*ones(1), Z, -1.0*ones(1), 0, 5));// -y - alpha <= 0 LPSolver solver(initchecker); VectorValues starter; starter.insert(X, zero(1)); starter.insert(Y, zero(1)); starter.insert(Z, 2*ones(1)); VectorValues results, duals; boost::tie(results, duals) = solver.optimize(starter); VectorValues expected; expected.insert(X, Vector::Constant(1, 13.5)); expected.insert(Y, Vector::Constant(1,6.5)); expected.insert(Z, Vector::Constant(1,-6.5)); CHECK(assert_equal(results, expected, 1e-7)); } TEST(LPInitSolverMatlab, 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); VectorValues starter; starter.insert(1,Vector3(0,0,2)); VectorValues results, duals; boost::tie(results, duals) = solver.optimize(starter); VectorValues expected; expected.insert(1, Vector3(13.5, 6.5, -6.5)); CHECK(assert_equal(results, expected, 1e-7)); } TEST(LPInitSolverMatlab, initialization) { LP lp = simpleLP1(); LPSolver lpSolver(lp); LPInitSolverMatlab initSolver(lpSolver); GaussianFactorGraph::shared_ptr initOfInitGraph = initSolver.buildInitOfInitGraph(); VectorValues x0 = initOfInitGraph->optimize(); VectorValues expected_x0; expected_x0.insert(1, zero(2)); CHECK(assert_equal(expected_x0, x0, 1e-10)); double y0 = initSolver.compute_y0(x0); double expected_y0 = 0.0; DOUBLES_EQUAL(expected_y0, y0, 1e-7); Key yKey = 2; LP::shared_ptr initLP = initSolver.buildInitialLP(yKey); LP expectedInitLP; expectedInitLP.cost = LinearCost(yKey, ones(1)); 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 CHECK(assert_equal(expectedInitLP, *initLP, 1e-10)); LPSolver lpSolveInit(*initLP); VectorValues xy0(x0); 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(2, Vector::Constant(1, -1)); CHECK(assert_equal(expected_init, xyInit, 1e-10)); VectorValues x = initSolver.solve(); CHECK(lp.isFeasible(x)); } } /* ************************************************************************* */ /** * TEST gtsam solver with an over-constrained system * x + y = 1 * x - y = 5 * x + 2y = 6 */ 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); VectorValues x = graph.optimize(); // This check confirms that gtsam linear constraint solver can't handle over-constrained system CHECK(factor.error(x) != 0.0); } TEST(LPSolver, overConstrainedLinearSystem2) { GaussianFactorGraph graph; graph.push_back(JacobianFactor(1, ones(1, 1), 2, ones(1, 1), ones(1), noiseModel::Constrained::All(1))); graph.push_back(JacobianFactor(1, ones(1, 1), 2, -ones(1, 1), 5*ones(1), noiseModel::Constrained::All(1))); graph.push_back(JacobianFactor(1, ones(1, 1), 2, 2*ones(1, 1), 6*ones(1), noiseModel::Constrained::All(1))); VectorValues x = graph.optimize(); // This check confirms that gtsam linear constraint solver can't handle over-constrained system CHECK(graph.error(x) != 0.0); } /* ************************************************************************* */ TEST(LPSolver, simpleTest1) { LP lp = simpleLP1(); LPSolver lpSolver(lp); VectorValues init; init.insert(1, zero(2)); VectorValues x1 = lpSolver.solveWithCurrentWorkingSet(init, InequalityFactorGraph()); VectorValues expected_x1; expected_x1.insert(1, Vector2( 1, 1)); CHECK(assert_equal(expected_x1, x1, 1e-10)); VectorValues result, duals; boost::tie(result, duals) = lpSolver.optimize(init); VectorValues expectedResult; expectedResult.insert(1, Vector2(8./3., 2./3.)); CHECK(assert_equal(expectedResult, result, 1e-10)); } /* ************************************************************************* */ TEST(LPSolver, testWithoutInitialValues) { LP lp = simpleLP1(); LPSolver lpSolver(lp); VectorValues result,duals, expectedResult; expectedResult.insert(1, Vector2(8./3., 2./3.)); boost::tie(result, duals) = lpSolver.optimize(); CHECK(assert_equal(expectedResult, result)); } /** * TODO: More TEST cases: * - Infeasible * - Unbounded * - Underdetermined */ /* ************************************************************************* */ TEST(LPSolver, LinearCost) { LinearCost cost(1, Vector3( 2., 4., 6.)); VectorValues x; x.insert(1, Vector3( 1., 3., 5.)); double error = cost.error(x); double expectedError = 44.0; DOUBLES_EQUAL(expectedError, error, 1e-100); } /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */