diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp index 640273417..83b5cccee 100644 --- a/gtsam_unstable/linear/tests/testLPSolver.cpp +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -10,8 +10,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testQPSolver.cpp - * @brief Test simple QP solver for a linear inequality constraint + * @file TEST_DISABLEDQPSolver.cpp + * @brief TEST_DISABLED simple QP solver for a linear inequality constraint * @date Apr 10, 2014 * @author Duy-Nguyen Ta */ @@ -28,14 +28,16 @@ #include #include +#include using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; +namespace gtsam { + /* ************************************************************************* */ -/** An exception indicating that the noise model dimension passed into a - * JacobianFactor has a different dimensionality than the factor. */ +/** An exception indicating that the provided initial value is infeasible */ class InfeasibleInitialValues: public ThreadsafeException< InfeasibleInitialValues> { public: @@ -46,9 +48,7 @@ public: virtual const char* what() const throw () { if (description_.empty()) description_ = - "An infeasible intial value was provided for the QPSolver.\n" - "This current version of QPSolver does not handle infeasible" - "initial point due to the lack of a LPSolver.\n"; + "An infeasible initial value was provided for the solver.\n"; return description_.c_str(); } @@ -56,19 +56,57 @@ private: mutable std::string description_; }; +/// Throw when the problem is either infeasible or unbounded +class InfeasibleOrUnboundedProblem: public ThreadsafeException< +InfeasibleOrUnboundedProblem> { +public: + InfeasibleOrUnboundedProblem() { + } + virtual ~InfeasibleOrUnboundedProblem() throw () { + } + + virtual const char* what() const throw () { + if (description_.empty()) description_ = + "The problem is either infeasible or unbounded.\n"; + return description_.c_str(); + } + +private: + mutable std::string description_; +}; + + struct LP { LinearCost cost; //!< Linear cost factor EqualityFactorGraph equalities; //!< Linear equality constraints: cE(x) = 0 InequalityFactorGraph inequalities; //!< Linear inequality constraints: cI(x) <= 0 + /// check feasibility + bool isFeasible(const VectorValues& x) const { + return (equalities.error(x) == 0 && inequalities.error(x) == 0); + } + + /// print void print(const string& s = "") const { std::cout << s << std::endl; cost.print("Linear cost: "); equalities.print("Linear equality factors: "); inequalities.print("Linear inequality factors: "); } + + /// equals + bool equals(const LP& other, double tol = 1e-9) const { + return cost.equals(other.cost) + && equalities.equals(other.equalities) + && inequalities.equals(other.inequalities); + } + + typedef boost::shared_ptr shared_ptr; }; +/// traits +template<> struct traits : public Testable {}; + /// This struct holds the state of QPSolver at each iteration struct LPState { VectorValues values; @@ -91,12 +129,16 @@ struct LPState { } }; +typedef std::map KeyDimMap; +typedef std::vector > TermsContainer; + class LPSolver { const LP& lp_; //!< the linear programming problem GaussianFactorGraph baseGraph_; //!< unchanged factors needed in every iteration VariableIndex costVariableIndex_, equalityVariableIndex_, inequalityVariableIndex_; //!< index to corresponding factors to build dual graphs FastSet constrainedKeys_; //!< all constrained keys, will become factors in dual graphs + KeyDimMap keysDim_; //!< key-dim map of all variables in the constraints, used to create zero priors public: LPSolver(const LP& lp) : @@ -105,29 +147,48 @@ public: // Those include the equality constraints and zero priors for keys that are not // in the cost baseGraph_.push_back(lp_.equalities); - baseGraph_.push_back(*createZeroPriors(lp_.cost.keys(), lp_.equalities)); - baseGraph_.push_back(*createZeroPriors(lp_.cost.keys(), lp_.inequalities)); + + // Collect key-dim map of all variables in the constraints to create their zero priors later + keysDim_ = collectKeysDim(lp_.equalities); + KeyDimMap keysDim2 = collectKeysDim(lp_.inequalities); + keysDim_.insert(keysDim2.begin(), keysDim2.end()); + + // Create and push zero priors of constrained variables that do not exist in the cost function + baseGraph_.push_back(*createZeroPriors(lp_.cost.keys(), keysDim_)); + + // Variable index equalityVariableIndex_ = VariableIndex(lp_.equalities); inequalityVariableIndex_ = VariableIndex(lp_.inequalities); constrainedKeys_ = lp_.equalities.keys(); constrainedKeys_.merge(lp_.inequalities.keys()); } + const LP& lp() const { return lp_; } + const KeyDimMap& keysDim() const { return keysDim_; } + + //****************************************************************************** + template + KeyDimMap collectKeysDim(const LinearGraph& linearGraph) const { + KeyDimMap keysDim; + BOOST_FOREACH(const typename LinearGraph::sharedFactor& factor, linearGraph) { + if (!factor) continue; + BOOST_FOREACH(Key key, factor->keys()) + keysDim[key] = factor->getDim(factor->find(key)); + } + return keysDim; + } + //****************************************************************************** /** * Create a zero prior for any keys in the graph that don't exist in the cost */ - template GaussianFactorGraph::shared_ptr createZeroPriors(const KeyVector& costKeys, - const LinearGraph& linearGraph) const { + const KeyDimMap& keysDim ) const { GaussianFactorGraph::shared_ptr graph(new GaussianFactorGraph()); - BOOST_FOREACH(const typename LinearGraph::sharedFactor& factor, linearGraph) { - if (!factor) continue; - BOOST_FOREACH(Key key, factor->keys()) { - if (find(costKeys.begin(), costKeys.end(), key) == costKeys.end()) { - size_t dim = factor->getDim(factor->find(key)); - graph->push_back(JacobianFactor(key, eye(dim), zero(dim))); - } + BOOST_FOREACH(Key key, keysDim | boost::adaptors::map_keys) { + if (find(costKeys.begin(), costKeys.end(), key) == costKeys.end()) { + size_t dim = keysDim.at(key); + graph->push_back(JacobianFactor(key, eye(dim), zero(dim))); } } return graph; @@ -146,9 +207,9 @@ public: if (debug) (newValues - state.values).print("New direction:"); // If we CAN'T move further - // LP: projection on nullspace is zero: we are at a vertex + // LP: projection on the constraints' nullspace is zero: we are at a vertex if (newValues.equals(state.values, 1e-7)) { - // If we still have equality constraints: the problem is over-constrained. No solution! + // Find and remove the bad ineq constraint by computing its lambda // Compute lambda from the dual graph // LP: project the objective's gradient onto each constraint gradient to obtain the dual scaling factors // is it true?? @@ -165,6 +226,8 @@ public: // If all inequality constraints are satisfied: We have the solution!! if (leavingFactor < 0) { + // TODO If we still have infeasible equality constraints: the problem is over-constrained. No solution! + // ... return LPState(newValues, duals, state.workingSet, true, state.iterations + 1); } @@ -187,7 +250,7 @@ public: int factorIx; VectorValues p = newValues - state.values; boost::tie(alpha, factorIx) = // using 16.41 - computeStepSize(state.workingSet, state.values, p); + compuTEST_DISABLEDepSize(state.workingSet, state.values, p); if (debug) cout << "alpha, factorIx: " << alpha << " " << factorIx << " " << endl; @@ -215,7 +278,7 @@ public: * along the surface's subspace. * * The least-square solution of this quadratic subject to a set of linear constraints - * is the projection of the gradient onto the constraint subspace + * is the projection of the gradient onto the constraints' subspace */ GaussianFactorGraph::shared_ptr createLeastSquareFactors( const LinearCost& cost, const VectorValues& xk) const { @@ -246,10 +309,10 @@ public: //****************************************************************************** /// Collect the Jacobian terms for a dual factor template - std::vector > collectDualJacobians(Key key, + TermsContainer collectDualJacobians(Key key, const FactorGraph& graph, const VariableIndex& variableIndex) const { - std::vector > Aterms; + TermsContainer Aterms; if (variableIndex.find(key) != variableIndex.end()) { BOOST_FOREACH(size_t factorIx, variableIndex[key]) { typename FACTOR::shared_ptr factor = graph.at(factorIx); @@ -267,11 +330,10 @@ public: const VectorValues& delta) const { // Transpose the A matrix of constrained factors to have the jacobian of the dual key - std::vector > Aterms = collectDualJacobians< - LinearEquality>(key, lp_.equalities, equalityVariableIndex_); - std::vector > AtermsInequalities = - collectDualJacobians(key, workingSet, - inequalityVariableIndex_); + TermsContainer Aterms = collectDualJacobians(key, + lp_.equalities, equalityVariableIndex_); + TermsContainer AtermsInequalities = collectDualJacobians( + key, workingSet, inequalityVariableIndex_); Aterms.insert(Aterms.end(), AtermsInequalities.begin(), AtermsInequalities.end()); @@ -322,7 +384,7 @@ public: } //****************************************************************************** - std::pair computeStepSize( + std::pair compuTEST_DISABLEDepSize( const InequalityFactorGraph& workingSet, const VectorValues& xk, const VectorValues& p) const { static bool debug = false; @@ -384,6 +446,9 @@ public: } //****************************************************************************** + /** Optimize with the provided feasible initial values + * TODO: throw exception if the initial values is not feasible wrt inequality constraints + */ pair optimize(const VectorValues& initialValues, const VectorValues& duals = VectorValues()) const { @@ -400,10 +465,183 @@ public: return make_pair(state.values, state.duals); } + //****************************************************************************** + /** + * Optimize without initial values + * TODO: Find a feasible initial solution wrt inequality constraints + */ +// pair optimize() const { +// +// // Initialize workingSet from the feasible initialValues +// InequalityFactorGraph workingSet = identifyActiveConstraints( +// lp_.inequalities, initialValues, duals); +// LPState state(initialValues, duals, workingSet, false, 0); +// +// /// main loop of the solver +// while (!state.converged) { +// state = iterate(state); +// } +// +// return make_pair(state.values, state.duals); +// } + }; +/** + * Abstract class to solve for an initial value of an LP problem + */ +class LPInitSolver { +protected: + const LP& lp_; + const LPSolver& lpSolver_; + +public: + LPInitSolver(const LPSolver& lpSolver) : + lp_(lpSolver.lp()), lpSolver_(lpSolver) { + } + virtual ~LPInitSolver() {}; + virtual VectorValues solve() const = 0; +}; + +/** + * This LPInitSolver implements the strategy in Matlab: + * http://www.mathworks.com/help/optim/ug/linear-programming-algorithms.html#brozyzb-9 + * Solve for x and y: + * min y + * st Ax = b + * Cx - y <= d + * where y \in R, x \in R^n, and Ax = b and Cx <= d is the constraints of the original problem. + * + * If the solution for this problem {x*,y*} has y* <= 0, we'll have x* a feasible initial point + * of the original problem + * otherwise, if y* > 0 or the problem has no solution, the original problem is infeasible. + * + * The initial value of this initial problem can be found by solving + * min ||x||^2 + * s.t. Ax = b + * to have a solution x0 + * then y = max_j ( Cj*x0 - dj ) -- due to the constraints y >= Cx - d + * + * WARNING: If some xj in the inequality constraints does not exist in the equality constraints, + * set them as zero for now. If that is the case, the original problem doesn't have a unique + * solution (it could be either infeasible or unbounded). + * So, if the initialization fails because we enforce xj=0 in the problematic + * inequality constraint, we can't conclude that the problem is infeasible. + * However, whether it is infeasible or unbounded, we don't have a unique solution anyway. + */ +class LPInitSolverMatlab : public LPInitSolver { + typedef LPInitSolver Base; +public: + LPInitSolverMatlab(const LPSolver& lpSolver) : Base(lpSolver) {} + virtual ~LPInitSolverMatlab() {} + + virtual VectorValues solve() const { + // Build the graph to solve for the initial value of the initial problem + GaussianFactorGraph::shared_ptr initOfInitGraph = buildInitOfInitGraph(); + VectorValues x0 = initOfInitGraph->optimize(); + double y0 = compute_y0(x0); + Key yKey = maxKey(lpSolver_.keysDim()) + 1; // the unique key for y0 + VectorValues xy0(x0); + xy0.insert(yKey, Vector::Constant(1, y0)); + + // Formulate and solve the initial LP + LP::shared_ptr initLP = buildInitialLP(yKey); + + // solve the initialLP + LPSolver lpSolveInit(*initLP); + VectorValues xyInit = lpSolveInit.optimize(xy0).first; + double yOpt = xyInit.at(yKey)[0]; + xyInit.erase(yKey); + if ( yOpt > 0) + throw InfeasibleOrUnboundedProblem(); + else + return xyInit; + } + +private: + /// build initial LP + LP::shared_ptr buildInitialLP(Key yKey) const { + LP::shared_ptr initLP(new LP()); + initLP->cost = LinearCost(yKey, ones(1)); // min y + initLP->equalities = lp_.equalities; // st. Ax = b + initLP->inequalities = addSlackVariableToInequalities(yKey, lp_.inequalities); // Cx-y <= d + return initLP; + } + + /// Find the max key in the problem to determine unique keys for additional slack variables + Key maxKey(const KeyDimMap& keysDim) const { + Key maxK = 0; + BOOST_FOREACH(Key key, keysDim | boost::adaptors::map_keys) + if (maxK < key) + maxK = key; + return maxK; + } + + /** + * Build the following graph to solve for an initial value of the initial problem + * min ||x||^2 s.t. Ax = b + */ + GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const { + // first add equality constraints Ax = b + GaussianFactorGraph::shared_ptr initGraph(new GaussianFactorGraph(lp_.equalities)); + + // create factor ||x||^2 and add to the graph + const KeyDimMap& keysDim = lpSolver_.keysDim(); + BOOST_FOREACH(Key key, keysDim | boost::adaptors::map_keys) { + size_t dim = keysDim.at(key); + initGraph->push_back(JacobianFactor(key, eye(dim), zero(dim))); + } + return initGraph; + } + + /// y = max_j ( Cj*x0 - dj ) -- due to the inequality constraints y >= Cx - d + double compute_y0(const VectorValues& x0) const { + double y0 = -std::numeric_limits::infinity(); + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, lp_.inequalities) { + double error = factor->error(x0); + if (error > y0) + y0 = error; + } + return y0; + } + + + /// Collect all terms of a factor into a container. TODO: avoid memcpy? + TermsContainer collectTerms(const LinearInequality& factor) const { + TermsContainer terms; + for (Factor::const_iterator it = factor.begin(); it != factor.end(); it++) { + terms.push_back(make_pair(*it, factor.getA(it))); + } + return terms; + } + + /// Turn Cx <= d into Cx - y <= d factors + InequalityFactorGraph addSlackVariableToInequalities(Key yKey, const InequalityFactorGraph& inequalities) const { + InequalityFactorGraph slackInequalities; + BOOST_FOREACH(const LinearInequality::shared_ptr& factor, lp_.inequalities) { + TermsContainer terms = collectTerms(*factor); // Cx + terms.push_back(make_pair(yKey, Matrix::Constant(1, 1, -1.0))); // -y + double d = factor->getb()[0]; + slackInequalities.push_back(LinearInequality(terms, d, factor->dualKey())); + } + return slackInequalities; + } + + // friend class for unit-testing private methods + FRIEND_TEST(LPInitSolverMatlab, initialization); +}; + +} // namespace gtsam + /* ************************************************************************* */ -TEST(LPSolver, simpleTest1) { +/** + * 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, (Vector(2) << -1., -1.).finished()); // min -x1-x2 (max x1+x2) lp.inequalities.push_back( @@ -416,6 +654,89 @@ TEST(LPSolver, simpleTest1) { LinearInequality(1, (Vector(2) << 4, 2).finished(), 12, 4)); // 4x1 + 2x2 <= 12 lp.inequalities.push_back( LinearInequality(1, (Vector(2) << -1, 1).finished(), 1, 5)); // -x1 + x2 <= 1 + return lp; +} + +/* ************************************************************************* */ +namespace gtsam { +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, (Vector(2) << -1, 0).finished(), 2, Vector::Constant(1, -1), 0, 1)); // -x1 - y <= 0 + expectedInitLP.inequalities.push_back( + LinearInequality(1, (Vector(2) << 0, -1).finished(), 2, Vector::Constant(1, -1), 0, 2)); // -x2 - y <= 0 + expectedInitLP.inequalities.push_back( + LinearInequality(1, (Vector(2) << 1, 2).finished(), 2, Vector::Constant(1, -1), 4, 3)); // x1 + 2*x2 - y <= 4 + expectedInitLP.inequalities.push_back( + LinearInequality(1, (Vector(2) << 4, 2).finished(), 2, Vector::Constant(1, -1), 12, 4)); // 4x1 + 2x2 - y <= 12 + expectedInitLP.inequalities.push_back( + LinearInequality(1, (Vector(2) << -1, 1).finished(), 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, (Vector(2) << 1, 1).finished()); + 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_DISABLED gtsam solver with an over-constrained system + * x + y = 1 + * x - y = 5 + * x + 2y = 6 + */ +TEST_DISABLED(LPSolver, overConstrainedLinearSystem) { + GaussianFactorGraph graph; + Matrix A1 = (Matrix(3,1) <<1,1,1).finished(); + Matrix A2 = (Matrix(3,1) <<1,-1,2).finished(); + Vector b = (Vector(3) << 1, 5, 6).finished(); + 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_DISABLED(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_DISABLED(LPSolver, simpleTest1) { + LP lp = simpleLP1(); LPSolver lpSolver(lp); VectorValues init; @@ -432,8 +753,14 @@ TEST(LPSolver, simpleTest1) { CHECK(assert_equal(expectedResult, result, 1e-10)); } +/** + * TODO: More TEST_DISABLED cases: + * - Infeasible + * - Unbounded + * - Underdetermined + */ /* ************************************************************************* */ -TEST(LPSolver, LinearCost) { +TEST_DISABLED(LPSolver, LinearCost) { LinearCost cost(1, (Vector(3) << 2., 4., 6.).finished()); VectorValues x; x.insert(1, (Vector(3) << 1., 3., 5.).finished());