diff --git a/gtsam_unstable/linear/LPInitSolver.h b/gtsam_unstable/linear/LPInitSolver.h index 91ad3573e..53a20807a 100644 --- a/gtsam_unstable/linear/LPInitSolver.h +++ b/gtsam_unstable/linear/LPInitSolver.h @@ -1,24 +1,156 @@ +/** + * @file LPInitSolver.h + * @brief This LPInitSolver implements the strategy in Matlab. + * @author Duy Nguyen Ta + * @author Ivan Dario Jimenez + * @date 1/24/16 + */ + #pragma once -#include "QPSolver.h" +#include +#include +#include namespace gtsam { /** - * Abstract class to solve for an initial value of an LP problem + * 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 LPInitSolver { -protected: - const LP& lp_; +private: const LPSolver& lpSolver_; + const LP& lp_; public: LPInitSolver(const LPSolver& lpSolver) : - lp_(lpSolver.lp()), lpSolver_(lpSolver) { + lpSolver_(lpSolver), lp_(lpSolver.lp()) { } virtual ~LPInitSolver() { } + + 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, I_1x1); // 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; + for (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(); + for (Key key : keysDim | boost::adaptors::map_keys) { + size_t dim = keysDim.at(key); + initGraph->push_back( + JacobianFactor(key, Matrix::Identity(dim, dim), Vector::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(); + for (const auto& factor : lp_.inequalities) { + double error = factor->error(x0); + if (error > y0) + y0 = error; + } + return y0; + } + + /// Collect all terms of a factor into a container. + std::vector > collectTerms( + const LinearInequality& factor) const { + std::vector > 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; + for (const auto& factor : lp_.inequalities) { + std::vector > 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(LPInitSolver, initialization) ; - virtual VectorValues solve() const = 0; }; } diff --git a/gtsam_unstable/linear/LPInitSolverMatlab.h b/gtsam_unstable/linear/LPInitSolverMatlab.h deleted file mode 100644 index 3ddff32ed..000000000 --- a/gtsam_unstable/linear/LPInitSolverMatlab.h +++ /dev/null @@ -1,149 +0,0 @@ -/** - * @file LPInitSolverMatlab.h - * @brief This LPInitSolver implements the strategy in Matlab: - * @author Ivan Dario Jimenez - * @date 1/24/16 - */ - -#pragma once - -#include -#include -#include -#include - -namespace gtsam { -/** - * 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. - std::vector > collectTerms( - const LinearInequality& factor) const { - std::vector < std::pair > 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) { - std::vector > 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); -}; -} diff --git a/gtsam_unstable/linear/LinearCost.h b/gtsam_unstable/linear/LinearCost.h index d3b5f6beb..1e6463399 100644 --- a/gtsam_unstable/linear/LinearCost.h +++ b/gtsam_unstable/linear/LinearCost.h @@ -61,19 +61,19 @@ public: /** Construct unary factor */ LinearCost(Key i1, const RowVector& A1) : - Base(i1, A1, zero(1)) { + Base(i1, A1, Vector1::Zero()) { } /** Construct binary factor */ LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b) : - Base(i1, A1, i2, A2, zero(1)) { + Base(i1, A1, i2, A2, Vector1::Zero()) { } /** Construct ternary factor */ LinearCost(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3, const RowVector& A3) : - Base(i1, A1, i2, A2, i3, A3, zero(1)) { + Base(i1, A1, i2, A2, i3, A3, Vector1::Zero()) { } /** Construct an n-ary factor @@ -81,7 +81,7 @@ public: * collection of keys and matrices making up the factor. */ template LinearCost(const TERMS& terms) : - Base(terms, zero(1)) { + Base(terms, Vector1::Zero()) { } /** Virtual destructor */