From c55229673a18723371f03733442d6a333fd1a68b Mon Sep 17 00:00:00 2001 From: Duy-Nguyen Ta Date: Thu, 16 Jun 2016 18:22:02 -0400 Subject: [PATCH] clean up headers, add banners, refactor implementation to cpp --- gtsam_unstable/linear/LP.h | 12 +++ gtsam_unstable/linear/LPInitSolver.cpp | 109 +++++++++++++++++++++++++ gtsam_unstable/linear/LPInitSolver.h | 101 ++++++----------------- gtsam_unstable/linear/LPSolver.cpp | 14 +++- gtsam_unstable/linear/LPSolver.h | 14 +++- gtsam_unstable/linear/QPInitSolver.h | 17 +++- gtsam_unstable/linear/QPSolver.cpp | 6 +- 7 files changed, 181 insertions(+), 92 deletions(-) create mode 100644 gtsam_unstable/linear/LPInitSolver.cpp diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h index 632b41e96..fc00c2240 100644 --- a/gtsam_unstable/linear/LP.h +++ b/gtsam_unstable/linear/LP.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 LP.h * @brief Struct used to hold a Linear Programming Problem @@ -9,6 +20,7 @@ #include #include +#include #include diff --git a/gtsam_unstable/linear/LPInitSolver.cpp b/gtsam_unstable/linear/LPInitSolver.cpp new file mode 100644 index 000000000..890803082 --- /dev/null +++ b/gtsam_unstable/linear/LPInitSolver.cpp @@ -0,0 +1,109 @@ +/* ---------------------------------------------------------------------------- + + * 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 LPInitSolver.h + * @brief This finds a feasible solution for an LP problem + * @author Duy Nguyen Ta + * @author Ivan Dario Jimenez + * @date 6/16/16 + */ + +#include +#include + +namespace gtsam { + +/******************************************************************************/ +VectorValues LPInitSolver::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(lp_) + 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; +} + +/******************************************************************************/ +LP::shared_ptr LPInitSolver::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; +} + +/******************************************************************************/ +GaussianFactorGraph::shared_ptr LPInitSolver::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& constrainedKeyDim = lp_.constrainedKeyDimMap(); + for (Key key : constrainedKeyDim | boost::adaptors::map_keys) { + size_t dim = constrainedKeyDim.at(key); + initGraph->push_back( + JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim))); + } + return initGraph; +} + +/******************************************************************************/ +double LPInitSolver::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; +} + +/******************************************************************************/ +std::vector > LPInitSolver::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; +} + +/******************************************************************************/ +InequalityFactorGraph LPInitSolver::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; +} + +} \ No newline at end of file diff --git a/gtsam_unstable/linear/LPInitSolver.h b/gtsam_unstable/linear/LPInitSolver.h index 5ab241eeb..a14e8e9f8 100644 --- a/gtsam_unstable/linear/LPInitSolver.h +++ b/gtsam_unstable/linear/LPInitSolver.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 LPInitSolver.h * @brief This LPInitSolver implements the strategy in Matlab. @@ -8,6 +19,7 @@ #pragma once +#include #include #include #include @@ -44,102 +56,35 @@ private: const LP& lp_; public: - LPInitSolver(const LP& lp) : lp_(lp) { - } + /// Construct with an LP problem + LPInitSolver(const LP& lp) : lp_(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(lp_) + 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; - } + ///@return a feasible initialization point + VectorValues solve() const; 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; - } + LP::shared_ptr buildInitialLP(Key yKey) const; /** * 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& constrainedKeyDim = lp_.constrainedKeyDimMap(); - for (Key key : constrainedKeyDim | boost::adaptors::map_keys) { - size_t dim = constrainedKeyDim.at(key); - initGraph->push_back( - JacobianFactor(key, Matrix::Identity(dim, dim), Vector::Zero(dim))); - } - return initGraph; - } + GaussianFactorGraph::shared_ptr buildInitOfInitGraph() const; /// 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; - } + double compute_y0(const VectorValues& x0) const; /// 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; - } + const LinearInequality& factor) const; /// 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 < std::pair > 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; - } + const InequalityFactorGraph& inequalities) const; // friend class for unit-testing private methods - FRIEND_TEST(LPInitSolver, initialization) - ; + FRIEND_TEST(LPInitSolver, initialization); }; + } diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp index 2658a8f7b..1cc409e22 100644 --- a/gtsam_unstable/linear/LPSolver.cpp +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 LPSolver.cpp * @brief @@ -7,9 +18,8 @@ */ #include -#include -#include #include +#include namespace gtsam { //****************************************************************************** diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h index 958574c61..7a0e6781b 100644 --- a/gtsam_unstable/linear/LPSolver.h +++ b/gtsam_unstable/linear/LPSolver.h @@ -1,3 +1,14 @@ +/* ---------------------------------------------------------------------------- + + * 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 LPSolver.h * @brief Class used to solve Linear Programming Problems as defined in LP.h @@ -14,11 +25,8 @@ #include #include -#include - namespace gtsam { - class LPSolver: public ActiveSetSolver { const LP &lp_; //!< the linear programming problem public: diff --git a/gtsam_unstable/linear/QPInitSolver.h b/gtsam_unstable/linear/QPInitSolver.h index 203cc616a..6742e9223 100644 --- a/gtsam_unstable/linear/QPInitSolver.h +++ b/gtsam_unstable/linear/QPInitSolver.h @@ -1,6 +1,17 @@ +/* ---------------------------------------------------------------------------- + + * 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 QPInitSolver.h - * @brief This QPInitSolver implements the strategy in Matlab. + * @brief This finds a feasible solution for a QP problem * @author Duy Nguyen Ta * @author Ivan Dario Jimenez * @date 6/16/16 @@ -24,9 +35,7 @@ public: /// Constructor with a QP problem QPInitSolver(const QP& qp) : qp_(qp) {} - /** - * @return a feasible initialization point - */ + ///@return a feasible initialization point VectorValues solve() const { // Make an LP with any linear cost function. It doesn't matter for // initialization. diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index c1984a75d..75a5e5c1a 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -16,13 +16,9 @@ * @author Duy-Nguyen Ta */ -#include -#include #include -#include -#include #include -#include +#include using namespace std;