diff --git a/.gitignore b/.gitignore index 04e8e76d1..b3ea282dd 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,5 @@ /build* +.idea *.pyc *.DS_Store /examples/Data/dubrovnik-3-7-pre-rewritten.txt diff --git a/CppUnitLite/Test.h b/CppUnitLite/Test.h index ff1f1b692..23d1e2573 100644 --- a/CppUnitLite/Test.h +++ b/CppUnitLite/Test.h @@ -68,6 +68,12 @@ protected: testGroup##testName##Instance; \ void testGroup##testName##Test::run (TestResult& result_) +/** + * Declare friend in a class to test its private methods + */ +#define FRIEND_TEST(testGroup, testName) \ + friend class testGroup##testName##Test; + /** * For debugging only: use TEST_UNSAFE to allow debuggers to have access to exceptions, as this * will not wrap execution with a try/catch block diff --git a/CppUnitLite/TestResult.h b/CppUnitLite/TestResult.h index a30275647..b64b40d75 100644 --- a/CppUnitLite/TestResult.h +++ b/CppUnitLite/TestResult.h @@ -18,8 +18,6 @@ // /////////////////////////////////////////////////////////////////////////////// - - #ifndef TESTRESULT_H #define TESTRESULT_H diff --git a/examples/Data/QPExample.QPS b/examples/Data/QPExample.QPS new file mode 100644 index 000000000..d67c59e0a --- /dev/null +++ b/examples/Data/QPExample.QPS @@ -0,0 +1,21 @@ +NAME QP example +ROWS + N obj + G r1 + L r2 +COLUMNS + c1 r1 2.0 r2 -1.0 + c1 obj 1.5 + c2 r1 1.0 r2 2.0 + c2 obj -2.0 +RHS + rhs1 obj -4.0 + rhs1 r1 2.0 r2 6.0 +RANGES +BOUNDS + UP BOUNDS c1 20.0 +QUADOBJ + c1 c1 8.0 + c1 c2 2.0 + c2 c2 10.0 +ENDATA \ No newline at end of file diff --git a/gtsam/inference/FactorGraph.h b/gtsam/inference/FactorGraph.h index fcafc9b42..38f30f5a3 100644 --- a/gtsam/inference/FactorGraph.h +++ b/gtsam/inference/FactorGraph.h @@ -354,3 +354,5 @@ namespace gtsam { }; // FactorGraph } // namespace gtsam + +#include \ No newline at end of file diff --git a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h index 7b5a0d493..fa39d2097 100644 --- a/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h +++ b/gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h @@ -142,7 +142,7 @@ boost::tuple nonlinearConjugateGradient(const S &system, // GTSAM_CONCEPT_MANIFOLD_TYPE(V); - int iteration = 0; + size_t iteration = 0; // check if we're already close enough double currentError = system.error(initial); diff --git a/gtsam/nonlinear/NonlinearFactor.h b/gtsam/nonlinear/NonlinearFactor.h index 505f58394..dd4c9123a 100644 --- a/gtsam/nonlinear/NonlinearFactor.h +++ b/gtsam/nonlinear/NonlinearFactor.h @@ -131,7 +131,8 @@ public: } /** - * Creates a shared_ptr clone of the factor with different keys using + * Creates a shared_ptr clone of the + * factor with different keys using * a map from old->new keys */ shared_ptr rekey(const std::map& rekey_mapping) const; diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index ca75bb02a..88fc0f850 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -38,7 +38,7 @@ public: SILENT, TERMINATION, ERROR, VALUES, DELTA, LINEAR }; - int maxIterations; ///< The maximum iterations to stop iterating (default 100) + size_t maxIterations; ///< The maximum iterations to stop iterating (default 100) double relativeErrorTol; ///< The maximum relative error decrease to stop iterating (default 1e-5) double absoluteErrorTol; ///< The maximum absolute error decrease to stop iterating (default 1e-5) double errorTol; ///< The maximum total error to stop iterating (default 0.0) @@ -54,7 +54,7 @@ public: } virtual void print(const std::string& str = "") const; - int getMaxIterations() const { + size_t getMaxIterations() const { return maxIterations; } double getRelativeErrorTol() const { diff --git a/gtsam_unstable/linear/ActiveSetSolver-inl.h b/gtsam_unstable/linear/ActiveSetSolver-inl.h new file mode 100644 index 000000000..18dc07aec --- /dev/null +++ b/gtsam_unstable/linear/ActiveSetSolver-inl.h @@ -0,0 +1,290 @@ +/* ---------------------------------------------------------------------------- + + * 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 ActiveSetSolver-inl.h + * @brief Implmentation of ActiveSetSolver. + * @author Ivan Dario Jimenez + * @author Duy Nguyen Ta + * @date 2/11/16 + */ + +#include + +/******************************************************************************/ +// Convenient macros to reduce syntactic noise. undef later. +#define Template template +#define This ActiveSetSolver + +/******************************************************************************/ + +namespace gtsam { + +/* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints + * If some inactive inequality constraints complain about the full step (alpha = 1), + * we have to adjust alpha to stay within the inequality constraints' feasible regions. + * + * For each inactive inequality j: + * - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints + * - We want: aj'*(xk + alpha*p) - bj <= 0 + * - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0 + * it's good! + * - We only care when aj'*p > 0. In this case, we need to choose alpha so that + * aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p) + * We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p) + * + * We want the minimum of all those alphas among all inactive inequality. + */ +Template boost::tuple This::computeStepSize( + const InequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p, const double& maxAlpha) const { + double minAlpha = maxAlpha; + int closestFactorIx = -1; + for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { + const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); + double b = factor->getb()[0]; + // only check inactive factors + if (!factor->active()) { + // Compute a'*p + double aTp = factor->dotProductRow(p); + + // Check if a'*p >0. Don't care if it's not. + if (aTp <= 0) + continue; + + // Compute a'*xk + double aTx = factor->dotProductRow(xk); + + // alpha = (b - a'*xk) / (a'*p) + double alpha = (b - aTx) / aTp; + // We want the minimum of all those max alphas + if (alpha < minAlpha) { + closestFactorIx = factorIx; + minAlpha = alpha; + } + } + } + return boost::make_tuple(minAlpha, closestFactorIx); +} + +/******************************************************************************/ +/* + * The goal of this function is to find currently active inequality constraints + * that violate the condition to be active. The one that violates the condition + * the most will be removed from the active set. See Nocedal06book, pg 469-471 + * + * Find the BAD active inequality that pulls x strongest to the wrong direction + * of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0) + * + * For active inequality constraints (those that are enforced as equality constraints + * in the current working set), we want lambda < 0. + * This is because: + * - From the Lagrangian L = f - lambda*c, we know that the constraint force + * is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay + * on the constraint surface, the constraint force has to balance out with + * other unconstrained forces that are pulling x towards the unconstrained + * minimum point. The other unconstrained forces are pulling x toward (-\grad f), + * hence the constraint force has to be exactly \grad f, so that the total + * force is 0. + * - We also know that at the constraint surface c(x)=0, \grad c points towards + (>= 0), + * while we are solving for - (<=0) constraint. + * - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction + * i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied. + * That means we want lambda < 0. + * - This is because when the constrained force pulls x towards the infeasible region (+), + * the unconstrained force is pulling x towards the opposite direction into + * the feasible region (again because the total force has to be 0 to make x stay still) + * So we can drop this constraint to have a lower error but feasible solution. + * + * In short, active inequality constraints with lambda > 0 are BAD, because they + * violate the condition to be active. + * + * And we want to remove the worst one with the largest lambda from the active set. + * + */ +Template int This::identifyLeavingConstraint( + const InequalityFactorGraph& workingSet, + const VectorValues& lambdas) const { + int worstFactorIx = -1; + // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either + // inactive or a good inequality constraint, so we don't care! + double maxLambda = 0.0; + for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { + const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); + if (factor->active()) { + double lambda = lambdas.at(factor->dualKey())[0]; + if (lambda > maxLambda) { + worstFactorIx = factorIx; + maxLambda = lambda; + } + } + } + return worstFactorIx; +} + +//****************************************************************************** +Template JacobianFactor::shared_ptr This::createDualFactor( + Key key, const InequalityFactorGraph& workingSet, + const VectorValues& delta) const { + // Transpose the A matrix of constrained factors to have the jacobian of the + // dual key + TermsContainer Aterms = collectDualJacobians( + key, problem_.equalities, equalityVariableIndex_); + TermsContainer AtermsInequalities = collectDualJacobians( + key, workingSet, inequalityVariableIndex_); + Aterms.insert(Aterms.end(), AtermsInequalities.begin(), + AtermsInequalities.end()); + + // Collect the gradients of unconstrained cost factors to the b vector + if (Aterms.size() > 0) { + Vector b = problem_.costGradient(key, delta); + // to compute the least-square approximation of dual variables + return boost::make_shared(Aterms, b); + } else { + return boost::make_shared(); + } +} + +/******************************************************************************/ +/* This function will create a dual graph that solves for the + * lagrange multipliers for the current working set. + * You can use lagrange multipliers as a necessary condition for optimality. + * The factor graph that is being solved is f' = -lambda * g' + * where f is the optimized function and g is the function resulting from + * aggregating the working set. + * The lambdas give you information about the feasibility of a constraint. + * if lambda < 0 the constraint is Ok + * if lambda = 0 you are on the constraint + * if lambda > 0 you are violating the constraint. + */ +Template GaussianFactorGraph::shared_ptr This::buildDualGraph( + const InequalityFactorGraph& workingSet, const VectorValues& delta) const { + GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); + for (Key key : constrainedKeys_) { + // Each constrained key becomes a factor in the dual graph + JacobianFactor::shared_ptr dualFactor = + createDualFactor(key, workingSet, delta); + if (!dualFactor->empty()) dualGraph->push_back(dualFactor); + } + return dualGraph; +} + +//****************************************************************************** +Template GaussianFactorGraph +This::buildWorkingGraph(const InequalityFactorGraph& workingSet, + const VectorValues& xk) const { + GaussianFactorGraph workingGraph; + workingGraph.push_back(POLICY::buildCostFunction(problem_, xk)); + workingGraph.push_back(problem_.equalities); + for (const LinearInequality::shared_ptr& factor : workingSet) + if (factor->active()) workingGraph.push_back(factor); + return workingGraph; +} + +//****************************************************************************** +Template typename This::State This::iterate( + const typename This::State& state) const { + // Algorithm 16.3 from Nocedal06book. + // Solve with the current working set eqn 16.39, but instead of solving for p + // solve for x + GaussianFactorGraph workingGraph = + buildWorkingGraph(state.workingSet, state.values); + VectorValues newValues = workingGraph.optimize(); + // If we CAN'T move further + // if p_k = 0 is the original condition, modified by Duy to say that the state + // update is zero. + if (newValues.equals(state.values, 1e-7)) { + // Compute lambda from the dual graph + GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, + newValues); + VectorValues duals = dualGraph->optimize(); + int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); + // If all inequality constraints are satisfied: We have the solution!! + if (leavingFactor < 0) { + return State(newValues, duals, state.workingSet, true, + state.iterations + 1); + } else { + // Inactivate the leaving constraint + InequalityFactorGraph newWorkingSet = state.workingSet; + newWorkingSet.at(leavingFactor)->inactivate(); + return State(newValues, duals, newWorkingSet, false, + state.iterations + 1); + } + } else { + // If we CAN make some progress, i.e. p_k != 0 + // Adapt stepsize if some inactive constraints complain about this move + double alpha; + int factorIx; + VectorValues p = newValues - state.values; + boost::tie(alpha, factorIx) = // using 16.41 + computeStepSize(state.workingSet, state.values, p, POLICY::maxAlpha); + // also add to the working set the one that complains the most + InequalityFactorGraph newWorkingSet = state.workingSet; + if (factorIx >= 0) + newWorkingSet.at(factorIx)->activate(); + // step! + newValues = state.values + alpha * p; + return State(newValues, state.duals, newWorkingSet, false, + state.iterations + 1); + } +} + +//****************************************************************************** +Template InequalityFactorGraph This::identifyActiveConstraints( + const InequalityFactorGraph& inequalities, + const VectorValues& initialValues, const VectorValues& duals, + bool useWarmStart) const { + InequalityFactorGraph workingSet; + for (const LinearInequality::shared_ptr& factor : inequalities) { + LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); + if (useWarmStart && duals.size() > 0) { + if (duals.exists(workingFactor->dualKey())) workingFactor->activate(); + else workingFactor->inactivate(); + } else { + double error = workingFactor->error(initialValues); + // Safety guard. This should not happen unless users provide a bad init + if (error > 0) throw InfeasibleInitialValues(); + if (fabs(error) < 1e-7) + workingFactor->activate(); + else + workingFactor->inactivate(); + } + workingSet.push_back(workingFactor); + } + return workingSet; +} + +//****************************************************************************** +Template std::pair This::optimize( + const VectorValues& initialValues, const VectorValues& duals, + bool useWarmStart) const { + // Initialize workingSet from the feasible initialValues + InequalityFactorGraph workingSet = identifyActiveConstraints( + problem_.inequalities, initialValues, duals, useWarmStart); + State state(initialValues, duals, workingSet, false, 0); + + /// main loop of the solver + while (!state.converged) state = iterate(state); + + return std::make_pair(state.values, state.duals); +} + +//****************************************************************************** +Template std::pair This::optimize() const { + INITSOLVER initSolver(problem_); + VectorValues initValues = initSolver.solve(); + return optimize(initValues); +} + +} + +#undef Template +#undef This \ No newline at end of file diff --git a/gtsam_unstable/linear/ActiveSetSolver.h b/gtsam_unstable/linear/ActiveSetSolver.h new file mode 100644 index 000000000..a5c60f311 --- /dev/null +++ b/gtsam_unstable/linear/ActiveSetSolver.h @@ -0,0 +1,204 @@ +/* ---------------------------------------------------------------------------- + + * 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 ActiveSetSolver.h + * @brief Active set method for solving LP, QP problems + * @author Ivan Dario Jimenez + * @author Duy Nguyen Ta + * @date 1/25/16 + */ +#pragma once + +#include +#include +#include + +namespace gtsam { + +/** + * This class implements the active set algorithm for solving convex + * Programming problems. + * + * @tparam PROBLEM Type of the problem to solve, e.g. LP (linear program) or + * QP (quadratic program). + * @tparam POLICY specific detail policy tailored for the particular program + * @tparam INITSOLVER Solver for an initial feasible solution of this problem. + */ +template +class ActiveSetSolver { +public: + /// This struct contains the state information for a single iteration + struct State { + VectorValues values; //!< current best values at each step + VectorValues duals; //!< current values of dual variables at each step + InequalityFactorGraph workingSet; /*!< keep track of current active/inactive + inequality constraints */ + bool converged; //!< True if the algorithm has converged to a solution + size_t iterations; /*!< Number of iterations. Incremented at the end of + each iteration. */ + + /// Default constructor + State() + : values(), duals(), workingSet(), converged(false), iterations(0) {} + + /// Constructor with initial values + State(const VectorValues& initialValues, const VectorValues& initialDuals, + const InequalityFactorGraph& initialWorkingSet, bool _converged, + size_t _iterations) + : values(initialValues), + duals(initialDuals), + workingSet(initialWorkingSet), + converged(_converged), + iterations(_iterations) {} + }; + +protected: + const PROBLEM& problem_; //!< the particular [convex] problem to solve + VariableIndex equalityVariableIndex_, + inequalityVariableIndex_; /*!< index to corresponding factors to build + dual graphs */ + KeySet constrainedKeys_; /*!< all constrained keys, will become factors in + dual graphs */ + + /// Vector of key matrix pairs. Matrices are usually the A term for a factor. + typedef std::vector > TermsContainer; + +public: + /// Constructor + ActiveSetSolver(const PROBLEM& problem) : problem_(problem) { + equalityVariableIndex_ = VariableIndex(problem_.equalities); + inequalityVariableIndex_ = VariableIndex(problem_.inequalities); + constrainedKeys_ = problem_.equalities.keys(); + constrainedKeys_.merge(problem_.inequalities.keys()); + } + + /** + * Optimize with provided initial values + * For this version, it is the responsibility of the caller to provide + * a feasible initial value, otherwise, an exception will be thrown. + * @return a pair of solutions + */ + std::pair optimize( + const VectorValues& initialValues, + const VectorValues& duals = VectorValues(), + bool useWarmStart = false) const; + + /** + * For this version the caller will not have to provide an initial value + * @return a pair of solutions + */ + std::pair optimize() const; + +protected: + /** + * Compute minimum step size alpha to move from the current point @p xk to the + * next feasible point along a direction @p p: x' = xk + alpha*p, + * where alpha \in [0,maxAlpha]. + * + * For QP, maxAlpha = 1. For LP: maxAlpha = Inf. + * + * @return a tuple of (minAlpha, closestFactorIndex) where closestFactorIndex + * is the closest inactive inequality constraint that blocks xk to move + * further and that has the minimum alpha, or (-1, maxAlpha) if there is no + * such inactive blocking constraint. + * + * If there is a blocking constraint, the closest one will be added to the + * working set and become active in the next iteration. + */ + boost::tuple computeStepSize( + const InequalityFactorGraph& workingSet, const VectorValues& xk, + const VectorValues& p, const double& maxAlpha) const; + + /** + * Finds the active constraints in the given factor graph and returns the + * Dual Jacobians used to build a dual factor graph. + */ + template + TermsContainer collectDualJacobians(Key key, const FactorGraph& graph, + const VariableIndex& variableIndex) const { + /* + * Iterates through each factor in the factor graph and checks + * whether it's active. If the factor is active it reutrns the A + * term of the factor. + */ + TermsContainer Aterms; + if (variableIndex.find(key) != variableIndex.end()) { + for (size_t factorIx : variableIndex[key]) { + typename FACTOR::shared_ptr factor = graph.at(factorIx); + if (!factor->active()) + continue; + Matrix Ai = factor->getA(factor->find(key)).transpose(); + Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); + } + } + return Aterms; + } + + /** + * Creates a dual factor from the current workingSet and the key of the + * the variable used to created the dual factor. + */ + JacobianFactor::shared_ptr createDualFactor( + Key key, const InequalityFactorGraph& workingSet, + const VectorValues& delta) const; + +public: /// Just for testing... + + /// Builds a dual graph from the current working set. + GaussianFactorGraph::shared_ptr buildDualGraph( + const InequalityFactorGraph& workingSet, const VectorValues& delta) const; + + /** + * Build a working graph of cost, equality and active inequality constraints + * to solve at each iteration. + * @param workingSet the collection of all cost and constrained factors + * @param xk current solution, used to build a special quadratic cost in LP + * @return a new better solution + */ + GaussianFactorGraph buildWorkingGraph( + const InequalityFactorGraph& workingSet, + const VectorValues& xk = VectorValues()) const; + + /// Iterate 1 step, return a new state with a new workingSet and values + State iterate(const State& state) const; + + /// Identify active constraints based on initial values. + InequalityFactorGraph identifyActiveConstraints( + const InequalityFactorGraph& inequalities, + const VectorValues& initialValues, + const VectorValues& duals = VectorValues(), + bool useWarmStart = false) const; + + /// Identifies active constraints that shouldn't be active anymore. + int identifyLeavingConstraint(const InequalityFactorGraph& workingSet, + const VectorValues& lambdas) const; + +}; + +/** + * Find the max key in a problem. + * Useful to determine unique keys for additional slack variables + */ +template +Key maxKey(const PROBLEM& problem) { + auto keys = problem.cost.keys(); + Key maxKey = *std::max_element(keys.begin(), keys.end()); + if (!problem.equalities.empty()) + maxKey = std::max(maxKey, *problem.equalities.keys().rbegin()); + if (!problem.inequalities.empty()) + maxKey = std::max(maxKey, *problem.inequalities.keys().rbegin()); + return maxKey; +} + +} // namespace gtsam + +#include \ No newline at end of file diff --git a/gtsam_unstable/linear/EqualityFactorGraph.h b/gtsam_unstable/linear/EqualityFactorGraph.h new file mode 100644 index 000000000..43befdbe0 --- /dev/null +++ b/gtsam_unstable/linear/EqualityFactorGraph.h @@ -0,0 +1,51 @@ +/* ---------------------------------------------------------------------------- + + * 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 EqualityFactorGraph.h + * @brief Factor graph of all LinearEquality factors + * @date Dec 8, 2014 + * @author Duy-Nguyen Ta + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * Collection of all Linear Equality constraints Ax=b of + * a Programming problem as a Factor Graph + */ +class EqualityFactorGraph: public FactorGraph { +public: + typedef boost::shared_ptr shared_ptr; + + /// Compute error of a guess. + double error(const VectorValues& x) const { + double total_error = 0.; + for (const sharedFactor& factor : *this) { + if (factor) + total_error += factor->error(x); + } + return total_error; + } +}; + +/// traits +template<> struct traits : public Testable< + EqualityFactorGraph> { +}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/InequalityFactorGraph.h b/gtsam_unstable/linear/InequalityFactorGraph.h new file mode 100644 index 000000000..c87645697 --- /dev/null +++ b/gtsam_unstable/linear/InequalityFactorGraph.h @@ -0,0 +1,69 @@ +/* ---------------------------------------------------------------------------- + + * 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 InequalityFactorGraph.h + * @brief Factor graph of all LinearInequality factors + * @date Dec 8, 2014 + * @author Duy-Nguyen Ta + */ + +#pragma once + +#include +#include +#include +#include + +namespace gtsam { + +/** + * Collection of all Linear Inequality constraints Ax-b <= 0 of + * a Programming problem as a Factor Graph + */ +class InequalityFactorGraph: public FactorGraph { +private: + typedef FactorGraph Base; + +public: + typedef boost::shared_ptr shared_ptr; + + /** print */ + void print(const std::string& str, const KeyFormatter& keyFormatter = + DefaultKeyFormatter) const { + Base::print(str, keyFormatter); + } + + /** equals */ + bool equals(const InequalityFactorGraph& other, double tol = 1e-9) const { + return Base::equals(other, tol); + } + + /** + * Compute error of a guess. + * Infinity error if it violates an inequality; zero otherwise. */ + double error(const VectorValues& x) const { + for (const sharedFactor& factor : *this) { + if (factor) + if (factor->error(x) > 1e-7) + return std::numeric_limits::infinity(); + } + return 0.0; + } +}; + +/// traits +template<> +struct traits : public Testable { +}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/InfeasibleInitialValues.h b/gtsam_unstable/linear/InfeasibleInitialValues.h new file mode 100644 index 000000000..60adb872e --- /dev/null +++ b/gtsam_unstable/linear/InfeasibleInitialValues.h @@ -0,0 +1,45 @@ +/* ---------------------------------------------------------------------------- + + * 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 InfeasibleInitialValues.h + * @brief Exception thrown when given Infeasible Initial Values. + * @date jan 24, 2015 + * @author Duy-Nguyen Ta + */ + +#pragma once + +namespace gtsam { +/* ************************************************************************* */ +/** An exception indicating that the provided initial value is infeasible + * Also used to inzdicatethat the noise model dimension passed into a + * JacobianFactor has a different dimensionality than the factor. */ +class InfeasibleInitialValues: public ThreadsafeException< + InfeasibleInitialValues> { +public: + InfeasibleInitialValues() { + } + + virtual ~InfeasibleInitialValues() throw () { + } + + virtual const char *what() const throw () { + if (description_.empty()) + description_ = + "An infeasible initial value was provided for the solver.\n"; + return description_.c_str(); + } + +private: + mutable std::string description_; +}; +} diff --git a/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h new file mode 100644 index 000000000..f7c5a5c79 --- /dev/null +++ b/gtsam_unstable/linear/InfeasibleOrUnboundedProblem.h @@ -0,0 +1,40 @@ +/* ---------------------------------------------------------------------------- + + * 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 InfeasibleOrUnboundedProblem.h + * @brief Throw when the problem is either infeasible or unbounded + * @author Ivan Dario Jimenez + * @date 1/24/16 + */ + +#pragma once + +namespace gtsam { + +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_; +}; +} diff --git a/gtsam_unstable/linear/LP.h b/gtsam_unstable/linear/LP.h new file mode 100644 index 000000000..fc00c2240 --- /dev/null +++ b/gtsam_unstable/linear/LP.h @@ -0,0 +1,102 @@ +/* ---------------------------------------------------------------------------- + + * 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 + * @author Ivan Dario Jimenez + * @date 1/24/16 + */ + +#pragma once + +#include +#include +#include + +#include + +namespace gtsam { + +using namespace std; + +/// Mapping between variable's key and its corresponding dimensionality +using KeyDimMap = std::map; +/* + * Iterates through every factor in a linear graph and generates a + * mapping between every factor key and it's corresponding dimensionality. + */ +template +KeyDimMap collectKeyDim(const LinearGraph& linearGraph) { + KeyDimMap keyDimMap; + for (const typename LinearGraph::sharedFactor& factor : linearGraph) { + if (!factor) continue; + for (Key key : factor->keys()) + keyDimMap[key] = factor->getDim(factor->find(key)); + } + return keyDimMap; +} + +/** + * Data structure of a Linear Program + */ +struct LP { + using shared_ptr = boost::shared_ptr; + + LinearCost cost; //!< Linear cost factor + EqualityFactorGraph equalities; //!< Linear equality constraints: cE(x) = 0 + InequalityFactorGraph inequalities; //!< Linear inequality constraints: cI(x) <= 0 +private: + mutable KeyDimMap cachedConstrainedKeyDimMap_; //!< cached key-dim map of all variables in the constraints + +public: + /// 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); + } + + const KeyDimMap& constrainedKeyDimMap() const { + if (!cachedConstrainedKeyDimMap_.empty()) + return cachedConstrainedKeyDimMap_; + // Collect key-dim map of all variables in the constraints + cachedConstrainedKeyDimMap_ = collectKeyDim(equalities); + KeyDimMap keysDim2 = collectKeyDim(inequalities); + cachedConstrainedKeyDimMap_.insert(keysDim2.begin(), keysDim2.end()); + return cachedConstrainedKeyDimMap_; + } + + Vector costGradient(Key key, const VectorValues& delta) const { + Vector g = Vector::Zero(delta.at(key).size()); + Factor::const_iterator it = cost.find(key); + if (it != cost.end()) g = cost.getA(it).transpose(); + return g; + } +}; + +/// traits +template<> struct traits : public Testable { +}; + +} diff --git a/gtsam_unstable/linear/LPInitSolver.cpp b/gtsam_unstable/linear/LPInitSolver.cpp new file mode 100644 index 000000000..8c3df3132 --- /dev/null +++ b/gtsam_unstable/linear/LPInitSolver.cpp @@ -0,0 +1,110 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#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 new file mode 100644 index 000000000..4eb672fbc --- /dev/null +++ b/gtsam_unstable/linear/LPInitSolver.h @@ -0,0 +1,89 @@ +/* ---------------------------------------------------------------------------- + + * 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. + * @author Duy Nguyen Ta + * @author Ivan Dario Jimenez + * @date 1/24/16 + */ + +#pragma once + +#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 LPInitSolver { +private: + const LP& lp_; + +public: + /// Construct with an LP problem + LPInitSolver(const LP& lp) : lp_(lp) {} + + ///@return a feasible initialization point + VectorValues solve() const; + +private: + /// build initial LP + 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; + + /// y = max_j ( Cj*x0 - dj ) -- due to the inequality constraints y >= Cx - d + double compute_y0(const VectorValues& x0) const; + + /// Collect all terms of a factor into a container. + std::vector> collectTerms( + const LinearInequality& factor) const; + + /// Turn Cx <= d into Cx - y <= d factors + InequalityFactorGraph addSlackVariableToInequalities(Key yKey, + const InequalityFactorGraph& inequalities) const; + + // friend class for unit-testing private methods + FRIEND_TEST(LPInitSolver, initialization); +}; + +} diff --git a/gtsam_unstable/linear/LPSolver.cpp b/gtsam_unstable/linear/LPSolver.cpp new file mode 100644 index 000000000..c1319a5ec --- /dev/null +++ b/gtsam_unstable/linear/LPSolver.cpp @@ -0,0 +1,25 @@ +/* ---------------------------------------------------------------------------- + + * 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 + * @author Duy Nguyen Ta + * @author Ivan Dario Jimenez + * @date 1/26/16 + */ + +#include + +namespace gtsam { +constexpr double LPPolicy::maxAlpha; +} + diff --git a/gtsam_unstable/linear/LPSolver.h b/gtsam_unstable/linear/LPSolver.h new file mode 100644 index 000000000..91cee3941 --- /dev/null +++ b/gtsam_unstable/linear/LPSolver.h @@ -0,0 +1,80 @@ +/* ---------------------------------------------------------------------------- + + * 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 Policy of ActiveSetSolver to solve Linear Programming Problems + * @author Duy Nguyen Ta + * @author Ivan Dario Jimenez + * @date 6/16/16 + */ + +#include +#include +#include + +#include +#include + +namespace gtsam { + +/// Policy for ActivetSetSolver to solve Linear Programming \sa LP problems +struct LPPolicy { + /// Maximum alpha for line search x'=xk + alpha*p, where p is the cost gradient + /// For LP, maxAlpha = Infinity + static constexpr double maxAlpha = std::numeric_limits::infinity(); + + /** + * Create the factor ||x-xk - (-g)||^2 where xk is the current feasible solution + * on the constraint surface and g is the gradient of the linear cost, + * i.e. -g is the direction we wish to follow to decrease the cost. + * + * Essentially, we try to match the direction d = x-xk with -g as much as possible + * subject to the condition that x needs to be on the constraint surface, i.e., d is + * 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 constraints' subspace + */ + static GaussianFactorGraph buildCostFunction(const LP& lp, + const VectorValues& xk) { + GaussianFactorGraph graph; + for (LinearCost::const_iterator it = lp.cost.begin(); it != lp.cost.end(); + ++it) { + size_t dim = lp.cost.getDim(it); + Vector b = xk.at(*it) - lp.cost.getA(it).transpose(); // b = xk-g + graph.push_back(JacobianFactor(*it, Matrix::Identity(dim, dim), b)); + } + + KeySet allKeys = lp.inequalities.keys(); + allKeys.merge(lp.equalities.keys()); + allKeys.merge(KeySet(lp.cost.keys())); + // Add corresponding factors for all variables that are not explicitly in + // the cost function. Gradients of the cost function wrt to these variables + // are zero (g=0), so b=xk + if (lp.cost.keys().size() != allKeys.size()) { + KeySet difference; + std::set_difference(allKeys.begin(), allKeys.end(), lp.cost.begin(), + lp.cost.end(), + std::inserter(difference, difference.end())); + for (Key k : difference) { + size_t dim = lp.constrainedKeyDimMap().at(k); + graph.push_back( + JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k))); + } + } + return graph; + } +}; + +using LPSolver = ActiveSetSolver; + +} \ No newline at end of file diff --git a/gtsam_unstable/linear/LinearCost.h b/gtsam_unstable/linear/LinearCost.h new file mode 100644 index 000000000..b489510af --- /dev/null +++ b/gtsam_unstable/linear/LinearCost.h @@ -0,0 +1,124 @@ +/* ---------------------------------------------------------------------------- + + * 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 LinearCost.h + * @brief LinearCost derived from JacobianFactor to support linear cost functions c'x + * @date Nov 27, 2014 + * @author Duy-Nguyen Ta + */ + +#pragma once + +#include + +namespace gtsam { + +typedef Eigen::RowVectorXd RowVector; + +/** + * This class defines a linear cost function c'x + * which is a JacobianFactor with only one row + */ +class LinearCost: public JacobianFactor { +public: + typedef LinearCost This; ///< Typedef to this class + typedef JacobianFactor Base; ///< Typedef to base class + typedef boost::shared_ptr shared_ptr; ///< shared_ptr to this class + +public: + /** default constructor for I/O */ + LinearCost() : + Base() { + } + + /** Conversion from HessianFactor */ + explicit LinearCost(const HessianFactor& hf) { + throw std::runtime_error("Cannot convert HessianFactor to LinearCost"); + } + + /** Conversion from JacobianFactor */ + explicit LinearCost(const JacobianFactor& jf) : + Base(jf) { + if (jf.isConstrained()) { + throw std::runtime_error( + "Cannot convert a constrained JacobianFactor to LinearCost"); + } + + if (jf.get_model()->dim() != 1) { + throw std::runtime_error( + "Only support single-valued linear cost factor!"); + } + } + + /** Construct unary factor */ + LinearCost(Key i1, const RowVector& A1) : + 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, 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, Vector1::Zero()) { + } + + /** Construct an n-ary factor + * @tparam TERMS A container whose value type is std::pair, specifying the + * collection of keys and matrices making up the factor. */ + template + LinearCost(const TERMS& terms) : + Base(terms, Vector1::Zero()) { + } + + /** Virtual destructor */ + virtual ~LinearCost() { + } + + /** equals */ + virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const { + return Base::equals(lf, tol); + } + + /** print */ + virtual void print(const std::string& s = "", const KeyFormatter& formatter = + DefaultKeyFormatter) const { + Base::print(s + " LinearCost: ", formatter); + } + + /** Clone this LinearCost */ + virtual GaussianFactor::shared_ptr clone() const { + return boost::static_pointer_cast < GaussianFactor + > (boost::make_shared < LinearCost > (*this)); + } + + /** Special error_vector for constraints (A*x-b) */ + Vector error_vector(const VectorValues& c) const { + return unweighted_error(c); + } + + /** Special error for single-valued inequality constraints. */ + virtual double error(const VectorValues& c) const { + return error_vector(c)[0]; + } +}; +// \ LinearCost + +/// traits +template<> struct traits : public Testable { +}; + +} // \ namespace gtsam + diff --git a/gtsam_unstable/linear/LinearEquality.h b/gtsam_unstable/linear/LinearEquality.h index bc1b2bc12..2463ef31c 100644 --- a/gtsam_unstable/linear/LinearEquality.h +++ b/gtsam_unstable/linear/LinearEquality.h @@ -9,11 +9,11 @@ * -------------------------------------------------------------------------- */ -/* - * LinearEquality.h - * @brief: LinearEquality derived from Base with constrained noise model - * @date: Nov 27, 2014 - * @author: thduynguyen +/** + * @file LinearEquality.h + * @brief LinearEquality derived from Base with constrained noise model + * @date Nov 27, 2014 + * @author Duy-Nguyen Ta */ #pragma once @@ -23,7 +23,7 @@ namespace gtsam { /** - * This class defines Linear constraints by inherit Base + * This class defines a linear equality constraints, inheriting JacobianFactor * with the special Constrained noise model */ class LinearEquality: public JacobianFactor { @@ -41,6 +41,17 @@ public: Base() { } + /** + * Construct from a constrained noisemodel JacobianFactor with a dual key. + */ + explicit LinearEquality(const JacobianFactor& jf, Key dualKey) : + Base(jf), dualKey_(dualKey) { + if (!jf.isConstrained()) { + throw std::runtime_error( + "Cannot convert an unconstrained JacobianFactor to LinearEquality"); + } + } + /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ explicit LinearEquality(const HessianFactor& hf) { throw std::runtime_error("Cannot convert HessianFactor to LinearEquality"); @@ -90,15 +101,19 @@ public: /** Clone this LinearEquality */ virtual GaussianFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - boost::make_shared(*this)); + return boost::static_pointer_cast < GaussianFactor + > (boost::make_shared < LinearEquality > (*this)); } /// dual key - Key dualKey() const { return dualKey_; } + Key dualKey() const { + return dualKey_; + } /// for active set method: equality constraints are always active - bool active() const { return true; } + bool active() const { + return true; + } /** Special error_vector for constraints (A*x-b) */ Vector error_vector(const VectorValues& c) const { @@ -113,11 +128,12 @@ public: return 0.0; } -}; // \ LinearEquality - +}; +// \ LinearEquality /// traits -template<> struct traits : public Testable {}; +template<> struct traits : public Testable { +}; } // \ namespace gtsam diff --git a/gtsam_unstable/linear/LinearEqualityFactorGraph.h b/gtsam_unstable/linear/LinearEqualityFactorGraph.h deleted file mode 100644 index 9c067ae3d..000000000 --- a/gtsam_unstable/linear/LinearEqualityFactorGraph.h +++ /dev/null @@ -1,37 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - - * -------------------------------------------------------------------------- */ - -/* - * LinearEqualityFactorGraph.h - * @brief: Factor graph of all LinearEquality factors - * @date: Dec 8, 2014 - * @author: thduynguyen - */ - -#pragma once - -#include -#include - -namespace gtsam { - -class LinearEqualityFactorGraph : public FactorGraph { -public: - typedef boost::shared_ptr shared_ptr; -}; - -/// traits -template<> struct traits : public Testable< - LinearEqualityFactorGraph> { -}; - -} // \ namespace gtsam - diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h index 7c62c3d54..1a31bd4e4 100644 --- a/gtsam_unstable/linear/LinearInequality.h +++ b/gtsam_unstable/linear/LinearInequality.h @@ -9,24 +9,26 @@ * -------------------------------------------------------------------------- */ -/* - * LinearInequality.h - * @brief: LinearInequality derived from Base with constrained noise model - * @date: Nov 27, 2014 - * @author: thduynguyen +/** + * @file LinearInequality.h + * @brief LinearInequality derived from Base with constrained noise model + * @date Nov 27, 2014 + * @author Duy-Nguyen Ta + * @author Ivan Dario Jimenez */ #pragma once #include +#include namespace gtsam { typedef Eigen::RowVectorXd RowVector; /** - * This class defines Linear constraints by inherit Base - * with the special Constrained noise model + * This class defines a linear inequality constraint Ax-b <= 0, + * inheriting JacobianFactor with the special Constrained noise model */ class LinearInequality: public JacobianFactor { public: @@ -44,35 +46,49 @@ public: Base(), active_(true) { } - /** Conversion from HessianFactor (does Cholesky to obtain Jacobian matrix) */ + /** Conversion from HessianFactor */ explicit LinearInequality(const HessianFactor& hf) { throw std::runtime_error( "Cannot convert HessianFactor to LinearInequality"); } + /** Conversion from JacobianFactor */ + explicit LinearInequality(const JacobianFactor& jf, Key dualKey) : + Base(jf), dualKey_(dualKey), active_(true) { + if (!jf.isConstrained()) { + throw std::runtime_error( + "Cannot convert an unconstrained JacobianFactor to LinearInequality"); + } + + if (jf.get_model()->dim() != 1) { + throw std::runtime_error("Only support single-valued inequality factor!"); + } + } + /** Construct unary factor */ LinearInequality(Key i1, const RowVector& A1, double b, Key dualKey) : - Base(i1, A1, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( - dualKey), active_(true) { + Base(i1, A1, (Vector(1) << b).finished(), + noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { } /** Construct binary factor */ - LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, double b, - Key dualKey) : - Base(i1, A1, i2, A2, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( - dualKey), active_(true) { + LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, + double b, Key dualKey) : + Base(i1, A1, i2, A2, (Vector(1) << b).finished(), + noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { } /** Construct ternary factor */ - LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, Key i3, - const RowVector& A3, double b, Key dualKey) : + LinearInequality(Key i1, const RowVector& A1, Key i2, const RowVector& A2, + Key i3, const RowVector& A3, double b, Key dualKey) : Base(i1, A1, i2, A2, i3, A3, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_(dualKey), active_(true) { } /** Construct an n-ary factor * @tparam TERMS A container whose value type is std::pair, specifying the - * collection of keys and matrices making up the factor. */ + * collection of keys and matrices making up the factor. + * In this inequality factor, each matrix must have only one row!! */ template LinearInequality(const TERMS& terms, double b, Key dualKey) : Base(terms, (Vector(1) << b).finished(), noiseModel::Constrained::All(1)), dualKey_( @@ -99,21 +115,29 @@ public: /** Clone this LinearInequality */ virtual GaussianFactor::shared_ptr clone() const { - return boost::static_pointer_cast( - boost::make_shared(*this)); + return boost::static_pointer_cast < GaussianFactor + > (boost::make_shared < LinearInequality > (*this)); } /// dual key - Key dualKey() const { return dualKey_; } + Key dualKey() const { + return dualKey_; + } /// return true if this constraint is active - bool active() const { return active_; } + bool active() const { + return active_; + } /// Make this inequality constraint active - void activate() { active_ = true; } + void activate() { + active_ = true; + } /// Make this inequality constraint inactive - void inactivate() { active_ = false; } + void inactivate() { + active_ = false; + } /** Special error_vector for constraints (A*x-b) */ Vector error_vector(const VectorValues& c) const { @@ -136,10 +160,12 @@ public: return aTp; } -}; // \ LinearInequality +}; +// \ LinearInequality /// traits -template<> struct traits : public Testable {}; +template<> struct traits : public Testable { +}; } // \ namespace gtsam diff --git a/gtsam_unstable/linear/LinearInequalityFactorGraph.h b/gtsam_unstable/linear/LinearInequalityFactorGraph.h deleted file mode 100644 index eca271941..000000000 --- a/gtsam_unstable/linear/LinearInequalityFactorGraph.h +++ /dev/null @@ -1,52 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 - - * -------------------------------------------------------------------------- */ - -/* - * LinearInequalityFactorGraph.h - * @brief: Factor graph of all LinearInequality factors - * @date: Dec 8, 2014 - * @author: thduynguyen - */ - -#pragma once - -#include -#include - -namespace gtsam { - -class LinearInequalityFactorGraph: public FactorGraph { -private: - typedef FactorGraph Base; - -public: - typedef boost::shared_ptr shared_ptr; - - /** print */ - void print(const std::string& str, const KeyFormatter& keyFormatter = - DefaultKeyFormatter) const { - Base::print(str, keyFormatter); - } - - /** equals */ - bool equals(const LinearInequalityFactorGraph& other, - double tol = 1e-9) const { - return Base::equals(other, tol); - } -}; - -/// traits -template<> struct traits : public Testable< - LinearInequalityFactorGraph> { -}; - -} // \ namespace gtsam - diff --git a/gtsam_unstable/linear/QP.h b/gtsam_unstable/linear/QP.h index 111ab506f..e610eb934 100644 --- a/gtsam_unstable/linear/QP.h +++ b/gtsam_unstable/linear/QP.h @@ -9,29 +9,34 @@ * -------------------------------------------------------------------------- */ -/* - * QP.h - * @brief: Factor graphs of a Quadratic Programming problem - * @date: Dec 8, 2014 - * @author: thduynguyen +/** + * @file QP.h + * @brief Factor graphs of a Quadratic Programming problem + * @date Dec 8, 2014 + * @author Duy-Nguyen Ta */ #pragma once #include -#include -#include +#include +#include +#include namespace gtsam { /** - * struct contains factor graphs of a Quadratic Programming problem + * Struct contains factor graphs of a Quadratic Programming problem */ struct QP { GaussianFactorGraph cost; //!< Quadratic cost factors - LinearEqualityFactorGraph equalities; //!< linear equality constraints - LinearInequalityFactorGraph inequalities; //!< linear inequality constraints + EqualityFactorGraph equalities; //!< linear equality constraints: cE(x) = 0 + InequalityFactorGraph inequalities; //!< linear inequality constraints: cI(x) <= 0 +private: + mutable VariableIndex cachedCostVariableIndex_; + +public: /** default constructor */ QP() : cost(), equalities(), inequalities() { @@ -39,8 +44,8 @@ struct QP { /** constructor */ QP(const GaussianFactorGraph& _cost, - const LinearEqualityFactorGraph& _linearEqualities, - const LinearInequalityFactorGraph& _linearInequalities) : + const EqualityFactorGraph& _linearEqualities, + const InequalityFactorGraph& _linearInequalities) : cost(_cost), equalities(_linearEqualities), inequalities( _linearInequalities) { } @@ -52,6 +57,23 @@ struct QP { equalities.print("Linear equality factors: "); inequalities.print("Linear inequality factors: "); } + + const VariableIndex& costVariableIndex() const { + if (cachedCostVariableIndex_.size() == 0) + cachedCostVariableIndex_ = VariableIndex(cost); + return cachedCostVariableIndex_; + } + + Vector costGradient(Key key, const VectorValues& delta) const { + Vector g = Vector::Zero(delta.at(key).size()); + if (costVariableIndex().find(key) != costVariableIndex().end()) { + for (size_t factorIx : costVariableIndex()[key]) { + GaussianFactor::shared_ptr factor = cost.at(factorIx); + g += factor->gradient(key, delta); + } + } + return g; + } }; } // namespace gtsam diff --git a/gtsam_unstable/linear/QPInitSolver.h b/gtsam_unstable/linear/QPInitSolver.h new file mode 100644 index 000000000..6742e9223 --- /dev/null +++ b/gtsam_unstable/linear/QPInitSolver.h @@ -0,0 +1,54 @@ +/* ---------------------------------------------------------------------------- + + * 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 finds a feasible solution for a QP problem + * @author Duy Nguyen Ta + * @author Ivan Dario Jimenez + * @date 6/16/16 + */ + +#pragma once + +#include + +namespace gtsam { + +/** + * This class finds a feasible solution for a QP problem. + * This uses the Matlab strategy for initialization + * For details, see + * http://www.mathworks.com/help/optim/ug/quadratic-programming-algorithms.html#brrzwpf-22 + */ +class QPInitSolver { + const QP& qp_; +public: + /// Constructor with a QP problem + QPInitSolver(const QP& qp) : qp_(qp) {} + + ///@return a feasible initialization point + VectorValues solve() const { + // Make an LP with any linear cost function. It doesn't matter for + // initialization. + LP initProblem; + // make an unrelated key for a random variable cost + Key newKey = maxKey(qp_) + 1; + initProblem.cost = LinearCost(newKey, Vector::Ones(1)); + initProblem.equalities = qp_.equalities; + initProblem.inequalities = qp_.inequalities; + LPInitSolver initSolver(initProblem); + return initSolver.solve(); + } +}; + + +} \ No newline at end of file diff --git a/gtsam_unstable/linear/QPSParser.cpp b/gtsam_unstable/linear/QPSParser.cpp new file mode 100644 index 000000000..748c4db38 --- /dev/null +++ b/gtsam_unstable/linear/QPSParser.cpp @@ -0,0 +1,126 @@ +/* ---------------------------------------------------------------------------- + + * 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 QPParser.cpp + * @author Ivan Dario Jimenez + * @date 3/5/16 + */ + +#define BOOST_SPIRIT_USE_PHOENIX_V3 1 + +#include +#include +#include + +#include +#include +#include +#include + +namespace bf = boost::fusion; +namespace qi = boost::spirit::qi; + +namespace gtsam { +typedef qi::grammar> base_grammar; + +struct QPSParser::MPSGrammar: base_grammar { + typedef std::vector Chars; + RawQP * rqp_; + boost::function const&)> setName; + boost::function const &)> addRow; + boost::function< + void(bf::vector const &)> rhsSingle; + boost::function< + void( + bf::vector)> rhsDouble; + boost::function< + void(bf::vector)> colSingle; + boost::function< + void( + bf::vector const &)> colDouble; + boost::function< + void(bf::vector const &)> addQuadTerm; + boost::function< + void( + bf::vector const &)> addBound; + boost::function< + void(bf::vector const &)> addBoundFr; + MPSGrammar(RawQP * rqp) : + base_grammar(start), rqp_(rqp), setName( + boost::bind(&RawQP::setName, rqp, ::_1)), addRow( + boost::bind(&RawQP::addRow, rqp, ::_1)), rhsSingle( + boost::bind(&RawQP::addRHS, rqp, ::_1)), rhsDouble( + boost::bind(&RawQP::addRHSDouble, rqp, ::_1)), colSingle( + boost::bind(&RawQP::addColumn, rqp, ::_1)), colDouble( + boost::bind(&RawQP::addColumnDouble, rqp, ::_1)), addQuadTerm( + boost::bind(&RawQP::addQuadTerm, rqp, ::_1)), addBound( + boost::bind(&RawQP::addBound, rqp, ::_1)), addBoundFr( + boost::bind(&RawQP::addBoundFr, rqp, ::_1)) { + using namespace boost::spirit; + using namespace boost::spirit::qi; + character = lexeme[alnum | '_' | '-' | '.']; + title = lexeme[character >> *(blank | character)]; + word = lexeme[+character]; + name = lexeme[lit("NAME") >> *blank >> title >> +space][setName]; + row = lexeme[*blank >> character >> +blank >> word >> *blank][addRow]; + rhs_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ + >> *blank][rhsSingle]; + rhs_double = lexeme[(*blank >> word >> +blank >> word >> +blank >> double_ + >> +blank >> word >> +blank >> double_)[rhsDouble] >> *blank]; + col_single = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ + >> *blank][colSingle]; + col_double = lexeme[*blank + >> (word >> +blank >> word >> +blank >> double_ >> +blank >> word + >> +blank >> double_)[colDouble] >> *blank]; + quad_l = lexeme[*blank >> word >> +blank >> word >> +blank >> double_ + >> *blank][addQuadTerm]; + bound = lexeme[*blank >> word >> +blank >> word >> +blank >> word >> +blank + >> double_ >> *blank][addBound]; + bound_fr = lexeme[*blank >> word >> +blank >> word >> +blank >> word + >> *blank][addBoundFr]; + rows = lexeme[lit("ROWS") >> *blank >> eol >> +(row >> eol)]; + rhs = lexeme[lit("RHS") >> *blank >> eol + >> +((rhs_double | rhs_single) >> eol)]; + cols = lexeme[lit("COLUMNS") >> *blank >> eol + >> +((col_double | col_single) >> eol)]; + quad = lexeme[lit("QUADOBJ") >> *blank >> eol >> +(quad_l >> eol)]; + bounds = lexeme[lit("BOUNDS") >> +space >> +((bound | bound_fr) >> eol)]; + ranges = lexeme[lit("RANGES") >> +space]; + end = lexeme[lit("ENDATA") >> *space]; + start = lexeme[name >> rows >> cols >> rhs >> -ranges >> bounds >> quad + >> end]; + } + + qi::rule, char()> character; + qi::rule, Chars()> word, title; + qi::rule > row, end, col_single, + col_double, rhs_single, rhs_double, ranges, bound, bound_fr, bounds, quad, + quad_l, rows, cols, rhs, name, start; +}; + +QP QPSParser::Parse() { + RawQP rawData; + std::fstream stream(fileName_.c_str()); + stream.unsetf(std::ios::skipws); + boost::spirit::basic_istream_iterator begin(stream); + boost::spirit::basic_istream_iterator last; + + if (!parse(begin, last, MPSGrammar(&rawData)) || begin != last) { + throw QPSParserException(); + } + + return rawData.makeQP(); +} + +} diff --git a/gtsam_unstable/linear/QPSParser.h b/gtsam_unstable/linear/QPSParser.h new file mode 100644 index 000000000..088168829 --- /dev/null +++ b/gtsam_unstable/linear/QPSParser.h @@ -0,0 +1,40 @@ +/* ---------------------------------------------------------------------------- + + * 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 QPParser.h + * @brief QPS parser implementation + * @author Ivan Dario Jimenez + * @date 3/5/16 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +class QPSParser { + +private: + std::string fileName_; + struct MPSGrammar; +public: + + QPSParser(const std::string& fileName) : + fileName_(findExampleDataFile(fileName)) { + } + + QP Parse(); +}; +} + diff --git a/gtsam_unstable/linear/QPSParserException.h b/gtsam_unstable/linear/QPSParserException.h new file mode 100644 index 000000000..ed4c79bdd --- /dev/null +++ b/gtsam_unstable/linear/QPSParserException.h @@ -0,0 +1,42 @@ +/* ---------------------------------------------------------------------------- + + * 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 QPSParserException.h + * @brief Exception thrown if there is an error parsing a QPS file + * @author Ivan Dario Jimenez + * @date 3/5/16 + */ + +#pragma once + +namespace gtsam { + +class QPSParserException: public ThreadsafeException { +public: + QPSParserException() { + } + + virtual ~QPSParserException() throw () { + } + + virtual const char *what() const throw () { + if (description_.empty()) + description_ = "There is a problem parsing the QPS file.\n"; + return description_.c_str(); + } + +private: + mutable std::string description_; +}; + +} + diff --git a/gtsam_unstable/linear/QPSolver.cpp b/gtsam_unstable/linear/QPSolver.cpp index 9fd58af78..5a4ac79ca 100644 --- a/gtsam_unstable/linear/QPSolver.cpp +++ b/gtsam_unstable/linear/QPSolver.cpp @@ -1,252 +1,23 @@ -/* - * QPSolver.cpp - * @brief: - * @date: Apr 15, 2014 - * @author: thduynguyen +/* ---------------------------------------------------------------------------- + + * 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 QPSolver.cpp + * @brief + * @date Apr 15, 2014 + * @author Duy-Nguyen Ta */ -#include -#include #include -#include - -using namespace std; - namespace gtsam { - -//****************************************************************************** -QPSolver::QPSolver(const QP& qp) : qp_(qp) { - baseGraph_ = qp_.cost; - baseGraph_.push_back(qp_.equalities.begin(), qp_.equalities.end()); - costVariableIndex_ = VariableIndex(qp_.cost); - equalityVariableIndex_ = VariableIndex(qp_.equalities); - inequalityVariableIndex_ = VariableIndex(qp_.inequalities); - constrainedKeys_ = qp_.equalities.keys(); - constrainedKeys_.merge(qp_.inequalities.keys()); -} - -//****************************************************************************** -VectorValues QPSolver::solveWithCurrentWorkingSet( - const LinearInequalityFactorGraph& workingSet) const { - GaussianFactorGraph workingGraph = baseGraph_; - for(const LinearInequality::shared_ptr& factor: workingSet) { - if (factor->active()) - workingGraph.push_back(factor); - } - return workingGraph.optimize(); -} - -//****************************************************************************** -JacobianFactor::shared_ptr QPSolver::createDualFactor(Key key, - const LinearInequalityFactorGraph& workingSet, 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, qp_.equalities, equalityVariableIndex_); - std::vector > AtermsInequalities = collectDualJacobians - < LinearInequality > (key, workingSet, inequalityVariableIndex_); - Aterms.insert(Aterms.end(), AtermsInequalities.begin(), - AtermsInequalities.end()); - - // Collect the gradients of unconstrained cost factors to the b vector - if (Aterms.size() > 0) { - Vector b = Vector::Zero(delta.at(key).size()); - if (costVariableIndex_.find(key) != costVariableIndex_.end()) { - for(size_t factorIx: costVariableIndex_[key]) { - GaussianFactor::shared_ptr factor = qp_.cost.at(factorIx); - b += factor->gradient(key, delta); - } - } - return boost::make_shared(Aterms, b, noiseModel::Constrained::All(b.rows())); - } - else { - return boost::make_shared(); - } -} - -//****************************************************************************** -GaussianFactorGraph::shared_ptr QPSolver::buildDualGraph( - const LinearInequalityFactorGraph& workingSet, const VectorValues& delta) const { - GaussianFactorGraph::shared_ptr dualGraph(new GaussianFactorGraph()); - for(Key key: constrainedKeys_) { - // Each constrained key becomes a factor in the dual graph - JacobianFactor::shared_ptr dualFactor = createDualFactor(key, workingSet, delta); - if (!dualFactor->empty()) - dualGraph->push_back(dualFactor); - } - return dualGraph; -} - -//****************************************************************************** -int QPSolver::identifyLeavingConstraint( - const LinearInequalityFactorGraph& workingSet, - const VectorValues& lambdas) const { - int worstFactorIx = -1; - // preset the maxLambda to 0.0: if lambda is <= 0.0, the constraint is either - // inactive or a good inequality constraint, so we don't care! - double maxLambda = 0.0; - for (size_t factorIx = 0; factorIx < workingSet.size(); ++factorIx) { - const LinearInequality::shared_ptr& factor = workingSet.at(factorIx); - if (factor->active()) { - double lambda = lambdas.at(factor->dualKey())[0]; - if (lambda > maxLambda) { - worstFactorIx = factorIx; - maxLambda = lambda; - } - } - } - return worstFactorIx; -} - -//****************************************************************************** -/* We have to make sure the new solution with alpha satisfies all INACTIVE inequality constraints - * If some inactive inequality constraints complain about the full step (alpha = 1), - * we have to adjust alpha to stay within the inequality constraints' feasible regions. - * - * For each inactive inequality j: - * - We already have: aj'*xk - bj <= 0, since xk satisfies all inequality constraints - * - We want: aj'*(xk + alpha*p) - bj <= 0 - * - If aj'*p <= 0, we have: aj'*(xk + alpha*p) <= aj'*xk <= bj, for all alpha>0 - * it's good! - * - We only care when aj'*p > 0. In this case, we need to choose alpha so that - * aj'*xk + alpha*aj'*p - bj <= 0 --> alpha <= (bj - aj'*xk) / (aj'*p) - * We want to step as far as possible, so we should choose alpha = (bj - aj'*xk) / (aj'*p) - * - * We want the minimum of all those alphas among all inactive inequality. - */ -boost::tuple QPSolver::computeStepSize( - const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, - const VectorValues& p) const { - static bool debug = false; - - double minAlpha = 1.0; - int closestFactorIx = -1; - for(size_t factorIx = 0; factorIxgetb()[0]; - // only check inactive factors - if (!factor->active()) { - // Compute a'*p - double aTp = factor->dotProductRow(p); - - // Check if a'*p >0. Don't care if it's not. - if (aTp <= 0) - continue; - - // Compute a'*xk - double aTx = factor->dotProductRow(xk); - - // alpha = (b - a'*xk) / (a'*p) - double alpha = (b - aTx) / aTp; - if (debug) - cout << "alpha: " << alpha << endl; - - // We want the minimum of all those max alphas - if (alpha < minAlpha) { - closestFactorIx = factorIx; - minAlpha = alpha; - } - } - - } - - return boost::make_tuple(minAlpha, closestFactorIx); -} - -//****************************************************************************** -QPState QPSolver::iterate(const QPState& state) const { - static bool debug = false; - - // Solve with the current working set - VectorValues newValues = solveWithCurrentWorkingSet(state.workingSet); - if (debug) - newValues.print("New solution:"); - - // If we CAN'T move further - if (newValues.equals(state.values, 1e-5)) { - // Compute lambda from the dual graph - if (debug) - cout << "Building dual graph..." << endl; - GaussianFactorGraph::shared_ptr dualGraph = buildDualGraph(state.workingSet, newValues); - if (debug) - dualGraph->print("Dual graph: "); - VectorValues duals = dualGraph->optimize(); - if (debug) - duals.print("Duals :"); - - int leavingFactor = identifyLeavingConstraint(state.workingSet, duals); - if (debug) - cout << "leavingFactor: " << leavingFactor << endl; - - // If all inequality constraints are satisfied: We have the solution!! - if (leavingFactor < 0) { - return QPState(newValues, duals, state.workingSet, true); - } - else { - // Inactivate the leaving constraint - LinearInequalityFactorGraph newWorkingSet = state.workingSet; - newWorkingSet.at(leavingFactor)->inactivate(); - return QPState(newValues, duals, newWorkingSet, false); - } - } - else { - // If we CAN make some progress - // Adapt stepsize if some inactive constraints complain about this move - double alpha; - int factorIx; - VectorValues p = newValues - state.values; - boost::tie(alpha, factorIx) = // - computeStepSize(state.workingSet, state.values, p); - if (debug) - cout << "alpha, factorIx: " << alpha << " " << factorIx << " " - << endl; - - // also add to the working set the one that complains the most - LinearInequalityFactorGraph newWorkingSet = state.workingSet; - if (factorIx >= 0) - newWorkingSet.at(factorIx)->activate(); - - // step! - newValues = state.values + alpha * p; - - return QPState(newValues, state.duals, newWorkingSet, false); - } -} - -//****************************************************************************** -LinearInequalityFactorGraph QPSolver::identifyActiveConstraints( - const LinearInequalityFactorGraph& inequalities, - const VectorValues& initialValues) const { - LinearInequalityFactorGraph workingSet; - for(const LinearInequality::shared_ptr& factor: inequalities){ - LinearInequality::shared_ptr workingFactor(new LinearInequality(*factor)); - double error = workingFactor->error(initialValues); - if (fabs(error)>1e-7){ - workingFactor->inactivate(); - } else { - workingFactor->activate(); - } - workingSet.push_back(workingFactor); - } - return workingSet; -} - -//****************************************************************************** -pair QPSolver::optimize( - const VectorValues& initialValues) const { - - // Initialize workingSet from the feasible initialValues - LinearInequalityFactorGraph workingSet = - identifyActiveConstraints(qp_.inequalities, initialValues); - QPState state(initialValues, VectorValues(), workingSet, false); - - /// main loop of the solver - while (!state.converged) { - state = iterate(state); - } - - return make_pair(state.values, state.duals); -} - -} /* namespace gtsam */ +constexpr double QPPolicy::maxAlpha; +} \ No newline at end of file diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index 7929bc1cc..9efc23a67 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -1,188 +1,43 @@ -/* - * QPSolver.h - * @brief: A quadratic programming solver implements the active set method - * @date: Apr 15, 2014 - * @author: thduynguyen +/* ---------------------------------------------------------------------------- + + * 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 QPSolver.h + * @brief Policy of ActiveSetSolver to solve Quadratic Programming Problems + * @author Duy Nguyen Ta + * @author Ivan Dario Jimenez + * @date 6/16/16 */ -#pragma once - -#include #include - -#include -#include +#include +#include +#include +#include namespace gtsam { -/// This struct holds the state of QPSolver at each iteration -struct QPState { - VectorValues values; - VectorValues duals; - LinearInequalityFactorGraph workingSet; - bool converged; +/// Policy for ActivetSetSolver to solve Linear Programming \sa QP problems +struct QPPolicy { + /// Maximum alpha for line search x'=xk + alpha*p, where p is the cost gradient + /// For QP, maxAlpha = 1 is the minimum point of the quadratic cost + static constexpr double maxAlpha = 1.0; - /// default constructor - QPState() : - values(), duals(), workingSet(), converged(false) { - } - - /// constructor with initial values - QPState(const VectorValues& initialValues, const VectorValues& initialDuals, - const LinearInequalityFactorGraph& initialWorkingSet, bool _converged) : - values(initialValues), duals(initialDuals), workingSet(initialWorkingSet), converged( - _converged) { + /// Simply the cost of the QP problem + static const GaussianFactorGraph& buildCostFunction( + const QP& qp, const VectorValues& xk = VectorValues()) { + return qp.cost; } }; -/** - * This class implements the active set method to solve quadratic programming problems - * encoded in a GaussianFactorGraph with special mixed constrained noise models, in which - * a negative sigma denotes an inequality <=0 constraint, - * a zero sigma denotes an equality =0 constraint, - * and a positive sigma denotes a normal Gaussian noise model. - */ -class QPSolver { +using QPSolver = ActiveSetSolver; - const QP& qp_; //!< factor graphs of the QP problem, can't be modified! - GaussianFactorGraph baseGraph_; //!< factor graphs of cost factors and linear equalities. The working set of inequalities will be added to this base graph in the process. - VariableIndex costVariableIndex_, equalityVariableIndex_, - inequalityVariableIndex_; - KeySet constrainedKeys_; //!< all constrained keys, will become factors in the dual graph - -public: - /// Constructor - QPSolver(const QP& qp); - - /// Find solution with the current working set - VectorValues solveWithCurrentWorkingSet( - const LinearInequalityFactorGraph& workingSet) const; - - /// @name Build the dual graph - /// @{ - - /// Collect the Jacobian terms for a dual factor - template - std::vector > collectDualJacobians(Key key, - const FactorGraph& graph, - const VariableIndex& variableIndex) const { - std::vector > Aterms; - if (variableIndex.find(key) != variableIndex.end()) { - for(size_t factorIx: variableIndex[key]){ - typename FACTOR::shared_ptr factor = graph.at(factorIx); - if (!factor->active()) continue; - Matrix Ai = factor->getA(factor->find(key)).transpose(); - Aterms.push_back(std::make_pair(factor->dualKey(), Ai)); - } - } - return Aterms; - } - - /// Create a dual factor - JacobianFactor::shared_ptr createDualFactor(Key key, - const LinearInequalityFactorGraph& workingSet, - const VectorValues& delta) const; - - /** - * Build the dual graph to solve for the Lagrange multipliers. - * - * The Lagrangian function is: - * L(X,lambdas) = f(X) - \sum_k lambda_k * c_k(X), - * where the unconstrained part is - * f(X) = 0.5*X'*G*X - X'*g + 0.5*f0 - * and the linear equality constraints are - * c1(X), c2(X), ..., cm(X) - * - * Take the derivative of L wrt X at the solution and set it to 0, we have - * \grad f(X) = \sum_k lambda_k * \grad c_k(X) (*) - * - * For each set of rows of (*) corresponding to a variable xi involving in some constraints - * we have: - * \grad f(xi) = \frac{\partial f}{\partial xi}' = \sum_j G_ij*xj - gi - * \grad c_k(xi) = \frac{\partial c_k}{\partial xi}' - * - * Note: If xi does not involve in any constraint, we have the trivial condition - * \grad f(Xi) = 0, which should be satisfied as a usual condition for unconstrained variables. - * - * So each variable xi involving in some constraints becomes a linear factor A*lambdas - b = 0 - * on the constraints' lambda multipliers, as follows: - * - The jacobian term A_k for each lambda_k is \grad c_k(xi) - * - The constant term b is \grad f(xi), which can be computed from all unconstrained - * Hessian factors connecting to xi: \grad f(xi) = \sum_j G_ij*xj - gi - */ - GaussianFactorGraph::shared_ptr buildDualGraph( - const LinearInequalityFactorGraph& workingSet, - const VectorValues& delta) const; - - /// @} - - /** - * The goal of this function is to find currently active inequality constraints - * that violate the condition to be active. The one that violates the condition - * the most will be removed from the active set. See Nocedal06book, pg 469-471 - * - * Find the BAD active inequality that pulls x strongest to the wrong direction - * of its constraint (i.e. it is pulling towards >0, while its feasible region is <=0) - * - * For active inequality constraints (those that are enforced as equality constraints - * in the current working set), we want lambda < 0. - * This is because: - * - From the Lagrangian L = f - lambda*c, we know that the constraint force - * is (lambda * \grad c) = \grad f. Intuitively, to keep the solution x stay - * on the constraint surface, the constraint force has to balance out with - * other unconstrained forces that are pulling x towards the unconstrained - * minimum point. The other unconstrained forces are pulling x toward (-\grad f), - * hence the constraint force has to be exactly \grad f, so that the total - * force is 0. - * - We also know that at the constraint surface c(x)=0, \grad c points towards + (>= 0), - * while we are solving for - (<=0) constraint. - * - We want the constraint force (lambda * \grad c) to pull x towards the - (<=0) direction - * i.e., the opposite direction of \grad c where the inequality constraint <=0 is satisfied. - * That means we want lambda < 0. - * - This is because when the constrained force pulls x towards the infeasible region (+), - * the unconstrained force is pulling x towards the opposite direction into - * the feasible region (again because the total force has to be 0 to make x stay still) - * So we can drop this constraint to have a lower error but feasible solution. - * - * In short, active inequality constraints with lambda > 0 are BAD, because they - * violate the condition to be active. - * - * And we want to remove the worst one with the largest lambda from the active set. - * - */ - int identifyLeavingConstraint(const LinearInequalityFactorGraph& workingSet, - const VectorValues& lambdas) const; - - /** - * Compute step size alpha for the new solution x' = xk + alpha*p, where alpha \in [0,1] - * - * @return a tuple of (alpha, factorIndex, sigmaIndex) where (factorIndex, sigmaIndex) - * is the constraint that has minimum alpha, or (-1,-1) if alpha = 1. - * This constraint will be added to the working set and become active - * in the next iteration - */ - boost::tuple computeStepSize( - const LinearInequalityFactorGraph& workingSet, const VectorValues& xk, - const VectorValues& p) const; - - /** Iterate 1 step, return a new state with a new workingSet and values */ - QPState iterate(const QPState& state) const; - - /** - * Identify active constraints based on initial values. - */ - LinearInequalityFactorGraph identifyActiveConstraints( - const LinearInequalityFactorGraph& inequalities, - const VectorValues& initialValues) const; - - /** Optimize with a provided initial values - * For this version, it is the responsibility of the caller to provide - * a feasible initial value. - * @return a pair of solutions - */ - std::pair optimize( - const VectorValues& initialValues) const; - -}; - -} /* namespace gtsam */ +} \ No newline at end of file diff --git a/gtsam_unstable/linear/RawQP.cpp b/gtsam_unstable/linear/RawQP.cpp new file mode 100644 index 000000000..ec71cae5b --- /dev/null +++ b/gtsam_unstable/linear/RawQP.cpp @@ -0,0 +1,271 @@ +/* ---------------------------------------------------------------------------- + + * 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 RawQP.cpp + * @brief + * @author Ivan Dario Jimenez + * @date 3/5/16 + */ + +#include +#include + +using boost::fusion::at_c; + +namespace gtsam { + +void RawQP::setName( + boost::fusion::vector, std::vector, + std::vector> const &name) { + name_ = std::string(at_c < 1 > (name).begin(), at_c < 1 > (name).end()); + if (debug) { + std::cout << "Parsing file: " << name_ << std::endl; + } +} + +void RawQP::addColumn( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector> const &vars) { + + std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); + std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); + Matrix11 coefficient = at_c < 5 > (vars) * I_1x1; + + if (!varname_to_key.count(var_)) + varname_to_key[var_] = Symbol('X', varNumber++); + if (row_ == obj_name) { + g[varname_to_key[var_]] = coefficient; + return; + } + (*row_to_constraint_v[row_])[row_][varname_to_key[var_]] = coefficient; + if (debug) { + std::cout << "Added Column for Var: " << var_ << " Row: " << row_ + << " Coefficient: " << coefficient << std::endl; + } + +} + +void RawQP::addColumnDouble( + boost::fusion::vector, std::vector, + std::vector, std::vector, double, std::vector, + std::vector, std::vector, double> const &vars) { + + std::string var_(at_c < 0 > (vars).begin(), at_c < 0 > (vars).end()); + std::string row1_(at_c < 2 > (vars).begin(), at_c < 2 > (vars).end()); + std::string row2_(at_c < 6 > (vars).begin(), at_c < 6 > (vars).end()); + Matrix11 coefficient1 = at_c < 4 > (vars) * I_1x1; + Matrix11 coefficient2 = at_c < 8 > (vars) * I_1x1; + if (!varname_to_key.count(var_)) + varname_to_key.insert( { var_, Symbol('X', varNumber++) }); + if (row1_ == obj_name) + g[varname_to_key[var_]] = coefficient1; + else + (*row_to_constraint_v[row1_])[row1_][varname_to_key[var_]] = coefficient1; + if (row2_ == obj_name) + g[varname_to_key[var_]] = coefficient2; + else + (*row_to_constraint_v[row2_])[row2_][varname_to_key[var_]] = coefficient2; +} + +void RawQP::addRHS( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector> const &vars) { + + std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); + std::string row_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); + double coefficient = at_c < 5 > (vars); + if (row_ == obj_name) + f = -coefficient; + else + b[row_] = coefficient; + + if (debug) { + std::cout << "Added RHS for Var: " << var_ << " Row: " << row_ + << " Coefficient: " << coefficient << std::endl; + } +} + +void RawQP::addRHSDouble( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector, std::vector, std::vector, double> const &vars) { + + std::string var_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); + std::string row1_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); + std::string row2_(at_c < 7 > (vars).begin(), at_c < 7 > (vars).end()); + double coefficient1 = at_c < 5 > (vars); + double coefficient2 = at_c < 9 > (vars); + if (row1_ == obj_name) + f = -coefficient1; + else + b[row1_] = coefficient1; + + if (row2_ == obj_name) + f = -coefficient2; + else + b[row2_] = coefficient2; + + if (debug) { + std::cout << "Added RHS for Var: " << var_ << " Row: " << row1_ + << " Coefficient: " << coefficient1 << std::endl; + std::cout << " " << "Row: " << row2_ + << " Coefficient: " << coefficient2 << std::endl; + } +} + +void RawQP::addRow( + boost::fusion::vector, char, std::vector, + std::vector, std::vector> const &vars) { + + std::string name_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); + char type = at_c < 1 > (vars); + switch (type) { + case 'N': + obj_name = name_; + break; + case 'L': + row_to_constraint_v[name_] = &IL; + break; + case 'G': + row_to_constraint_v[name_] = &IG; + break; + case 'E': + row_to_constraint_v[name_] = &E; + break; + default: + std::cout << "invalid type: " << type << std::endl; + break; + } + if (debug) { + std::cout << "Added Row Type: " << type << " Name: " << name_ << std::endl; + } +} + +void RawQP::addBound( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, + std::vector, std::vector, double> const &vars) { + + std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); + std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end()); + double number = at_c < 7 > (vars); + if (type_.compare(std::string("UP")) == 0) + up[varname_to_key[var_]] = number; + else if (type_.compare(std::string("LO")) == 0) + lo[varname_to_key[var_]] = number; + else + std::cout << "Invalid Bound Type: " << type_ << std::endl; + + if (debug) { + std::cout << "Added Bound Type: " << type_ << " Var: " << var_ + << " Amount: " << number << std::endl; + } +} + +void RawQP::addBoundFr( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, + std::vector, std::vector> const &vars) { + std::string type_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); + std::string var_(at_c < 5 > (vars).begin(), at_c < 5 > (vars).end()); + Free.push_back(varname_to_key[var_]); + if (debug) { + std::cout << "Added Free Bound Type: " << type_ << " Var: " << var_ + << " Amount: " << std::endl; + } +} + +void RawQP::addQuadTerm( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector> const &vars) { + std::string var1_(at_c < 1 > (vars).begin(), at_c < 1 > (vars).end()); + std::string var2_(at_c < 3 > (vars).begin(), at_c < 3 > (vars).end()); + Matrix11 coefficient = at_c < 5 > (vars) * I_1x1; + + H[varname_to_key[var1_]][varname_to_key[var2_]] = coefficient; + H[varname_to_key[var2_]][varname_to_key[var1_]] = coefficient; + if (debug) { + std::cout << "Added QuadTerm for Var: " << var1_ << " Row: " << var2_ + << " Coefficient: " << coefficient << std::endl; + } +} + +QP RawQP::makeQP() { + std::vector < Key > keys; + std::vector < Matrix > Gs; + std::vector < Vector > gs; + for (auto kv : varname_to_key) { + keys.push_back(kv.second); + } + std::sort(keys.begin(), keys.end()); + for (unsigned int i = 0; i < keys.size(); ++i) { + for (unsigned int j = i; j < keys.size(); ++j) { + Gs.push_back(H[keys[i]][keys[j]]); + } + } + for (Key key1 : keys) { + gs.push_back(-g[key1]); + } + int dual_key_num = keys.size() + 1; + QP madeQP; + auto obj = HessianFactor(keys, Gs, gs, f); + + madeQP.cost.push_back(obj); + + for (auto kv : E) { + std::map keyMatrixMap; + for (auto km : kv.second) { + keyMatrixMap.insert(km); + } + madeQP.equalities.push_back( + LinearEquality(keyMatrixMap, b[kv.first] * I_1x1, dual_key_num++)); + } + + for (auto kv : IG) { + std::map keyMatrixMap; + for (auto km : kv.second) { + km.second = -km.second; + keyMatrixMap.insert(km); + } + madeQP.inequalities.push_back( + LinearInequality(keyMatrixMap, -b[kv.first], dual_key_num++)); + } + + for (auto kv : IL) { + std::map keyMatrixMap; + for (auto km : kv.second) { + keyMatrixMap.insert(km); + } + madeQP.inequalities.push_back( + LinearInequality(keyMatrixMap, b[kv.first], dual_key_num++)); + } + + for (Key k : keys) { + if (std::find(Free.begin(), Free.end(), k) != Free.end()) + continue; + if (up.count(k) == 1) + madeQP.inequalities.push_back( + LinearInequality(k, I_1x1, up[k], dual_key_num++)); + if (lo.count(k) == 1) + madeQP.inequalities.push_back( + LinearInequality(k, -I_1x1, lo[k], dual_key_num++)); + else + madeQP.inequalities.push_back( + LinearInequality(k, -I_1x1, 0, dual_key_num++)); + } + return madeQP; +} +} + diff --git a/gtsam_unstable/linear/RawQP.h b/gtsam_unstable/linear/RawQP.h new file mode 100644 index 000000000..aadf11e50 --- /dev/null +++ b/gtsam_unstable/linear/RawQP.h @@ -0,0 +1,106 @@ +/* ---------------------------------------------------------------------------- + + * 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 RawQP.h + * @brief + * @author Ivan Dario Jimenez + * @date 3/5/16 + */ + +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gtsam { +class RawQP { +private: + typedef std::unordered_map coefficient_v; + typedef std::unordered_map constraint_v; + + std::unordered_map row_to_constraint_v; + constraint_v E; + constraint_v IG; + constraint_v IL; + unsigned int varNumber; + std::unordered_map b; + std::unordered_map g; + std::unordered_map varname_to_key; + std::unordered_map > H; + double f; + std::string obj_name; + std::string name_; + std::unordered_map up; + std::unordered_map lo; + std::vector Free; + const bool debug = false; + +public: + RawQP() : + row_to_constraint_v(), E(), IG(), IL(), varNumber(1), b(), g(), varname_to_key(), H(), f(), obj_name(), name_(), up(), lo(), Free() { + } + + void setName( + boost::fusion::vector, std::vector, + std::vector> const & name); + + void addColumn( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector> const & vars); + + void addColumnDouble( + boost::fusion::vector, std::vector, + std::vector, std::vector, double, std::vector, + std::vector, std::vector, double> const & vars); + + void addRHS( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector> const & vars); + + void addRHSDouble( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector, std::vector, std::vector, double> const & vars); + + void addRow( + boost::fusion::vector, char, std::vector, + std::vector, std::vector> const & vars); + + void addBound( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, + std::vector, std::vector, double> const & vars); + + void addBoundFr( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, + std::vector, std::vector> const & vars); + + void addQuadTerm( + boost::fusion::vector, std::vector, + std::vector, std::vector, std::vector, double, + std::vector> const & vars); + + QP makeQP(); +} +; +} diff --git a/gtsam_unstable/linear/tests/testLPSolver.cpp b/gtsam_unstable/linear/tests/testLPSolver.cpp new file mode 100644 index 000000000..8bf6be56b --- /dev/null +++ b/gtsam_unstable/linear/tests/testLPSolver.cpp @@ -0,0 +1,255 @@ +/* ---------------------------------------------------------------------------- + + * 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; + +static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1); + +/* ************************************************************************* */ +/** + * 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(LPInitSolver, 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(LPInitSolver, 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, kOne); // min alpha + initchecker.inequalities.push_back( + LinearInequality(X, -2.0 * kOne, Y, -1.0 * kOne, Z, -1.0 * kOne, -2, + 1)); //-2x-y-alpha <= -2 + initchecker.inequalities.push_back( + LinearInequality(X, -1.0 * kOne, Y, 2.0 * kOne, Z, -1.0 * kOne, 6, + 2)); // -x+2y-alpha <= 6 + initchecker.inequalities.push_back(LinearInequality( + X, -1.0 * kOne, Z, -1.0 * kOne, 0, 3)); // -x - alpha <= 0 + initchecker.inequalities.push_back(LinearInequality( + X, 1.0 * kOne, Z, -1.0 * kOne, 20, 4)); // x - alpha <= 20 + initchecker.inequalities.push_back(LinearInequality( + Y, -1.0 * kOne, Z, -1.0 * kOne, 0, 5)); // -y - alpha <= 0 + LPSolver solver(initchecker); + VectorValues starter; + starter.insert(X, kZero); + starter.insert(Y, kZero); + starter.insert(Z, Vector::Constant(1, 2.0)); + 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(LPInitSolver, initialization) { + LP lp = simpleLP1(); + LPInitSolver initSolver(lp); + + GaussianFactorGraph::shared_ptr initOfInitGraph = + initSolver.buildInitOfInitGraph(); + VectorValues x0 = initOfInitGraph->optimize(); + VectorValues expected_x0; + expected_x0.insert(1, Vector::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, kOne); + 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, Vector::Ones(2)); + 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, I_1x1, 2, I_1x1, kOne, + noiseModel::Constrained::All(1))); + graph.push_back(JacobianFactor(1, I_1x1, 2, -I_1x1, 5 * kOne, + noiseModel::Constrained::All(1))); + graph.push_back(JacobianFactor(1, I_1x1, 2, 2 * I_1x1, 6 * kOne, + 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, Vector::Zero(2)); + + VectorValues x1 = + lpSolver.buildWorkingGraph(InequalityFactorGraph(), init).optimize(); + VectorValues expected_x1; + expected_x1.insert(1, Vector::Ones(2)); + 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); +} +/* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testLinearEquality.cpp b/gtsam_unstable/linear/tests/testLinearEquality.cpp index 90f3de586..fa94dd255 100644 --- a/gtsam_unstable/linear/tests/testLinearEquality.cpp +++ b/gtsam_unstable/linear/tests/testLinearEquality.cpp @@ -12,7 +12,7 @@ /** * @file testLinearEquality.cpp * @brief Unit tests for LinearEquality - * @author thduynguyen + * @author Duy-Nguyen Ta **/ #include @@ -28,20 +28,20 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -GTSAM_CONCEPT_TESTABLE_INST(LinearEquality) +GTSAM_CONCEPT_TESTABLE_INST (LinearEquality) namespace { - namespace simple { - // Terms we'll use - const vector > terms = list_of > - (make_pair(5, Matrix3::Identity())) - (make_pair(10, 2*Matrix3::Identity())) - (make_pair(15, 3*Matrix3::Identity())); +namespace simple { +// Terms we'll use +const vector > terms = list_of < pair + > (make_pair(5, Matrix3::Identity()))( + make_pair(10, 2 * Matrix3::Identity()))( + make_pair(15, 3 * Matrix3::Identity())); - // RHS and sigmas - const Vector b = (Vector(3) << 1., 2., 3.).finished(); - const SharedDiagonal noise = noiseModel::Constrained::All(3); - } +// RHS and sigmas +const Vector b = (Vector(3) << 1., 2., 3.).finished(); +const SharedDiagonal noise = noiseModel::Constrained::All(3); +} } /* ************************************************************************* */ @@ -53,7 +53,7 @@ TEST(LinearEquality, constructors_and_accessors) { // One term constructor LinearEquality expected( - boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0); + boost::make_iterator_range(terms.begin(), terms.begin() + 1), b, 0); LinearEquality actual(terms[0].first, terms[0].second, b, 0); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[0].first, (long)actual.keys().back()); @@ -65,9 +65,9 @@ TEST(LinearEquality, constructors_and_accessors) { // Two term constructor LinearEquality expected( - boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0); + boost::make_iterator_range(terms.begin(), terms.begin() + 2), b, 0); LinearEquality actual(terms[0].first, terms[0].second, - terms[1].first, terms[1].second, b, 0); + terms[1].first, terms[1].second, b, 0); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[1].first, (long)actual.keys().back()); EXPECT(assert_equal(terms[1].second, actual.getA(actual.end() - 1))); @@ -78,9 +78,9 @@ TEST(LinearEquality, constructors_and_accessors) { // Three term constructor LinearEquality expected( - boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0); + boost::make_iterator_range(terms.begin(), terms.begin() + 3), b, 0); LinearEquality actual(terms[0].first, terms[0].second, - terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0); + terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, 0); EXPECT(assert_equal(expected, actual)); LONGS_EQUAL((long)terms[2].first, (long)actual.keys().back()); EXPECT(assert_equal(terms[2].second, actual.getA(actual.end() - 1))); @@ -93,10 +93,10 @@ TEST(LinearEquality, constructors_and_accessors) /* ************************************************************************* */ TEST(LinearEquality, Hessian_conversion) { HessianFactor hessian(0, (Matrix(4,4) << - 1.57, 2.695, -1.1, -2.35, - 2.695, 11.3125, -0.65, -10.225, - -1.1, -0.65, 1, 0.5, - -2.35, -10.225, 0.5, 9.25).finished(), + 1.57, 2.695, -1.1, -2.35, + 2.695, 11.3125, -0.65, -10.225, + -1.1, -0.65, 1, 0.5, + -2.35, -10.225, 0.5, 9.25).finished(), (Vector(4) << -7.885, -28.5175, 2.75, 25.675).finished(), 73.1725); @@ -169,8 +169,8 @@ TEST(LinearEquality, matrices) augmentedJacobianExpected << jacobianExpected, rhsExpected; Matrix augmentedHessianExpected = - augmentedJacobianExpected.transpose() * simple::noise->R().transpose() - * simple::noise->R() * augmentedJacobianExpected; + augmentedJacobianExpected.transpose() * simple::noise->R().transpose() + * simple::noise->R() * augmentedJacobianExpected; // Whitened Jacobian EXPECT(assert_equal(jacobianExpected, factor.jacobian().first)); @@ -210,8 +210,8 @@ TEST(LinearEquality, operators ) // test gradient at zero Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); VectorValues expectedG; - expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished()); - expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished()); + expectedG.insert(1, (Vector(2) << 0.2, -0.1).finished()); + expectedG.insert(2, (Vector(2) << -0.2, 0.1).finished()); VectorValues actualG = lf.gradientAtZero(); EXPECT(assert_equal(expectedG, actualG)); } @@ -233,5 +233,8 @@ TEST(LinearEquality, empty ) } /* ************************************************************************* */ -int main() { TestResult tr; return TestRegistry::runAllTests(tr);} +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} /* ************************************************************************* */ diff --git a/gtsam_unstable/linear/tests/testQPSolver.cpp b/gtsam_unstable/linear/tests/testQPSolver.cpp index 7442540f5..bc9dd1f98 100644 --- a/gtsam_unstable/linear/tests/testQPSolver.cpp +++ b/gtsam_unstable/linear/tests/testQPSolver.cpp @@ -19,14 +19,14 @@ #include #include #include - +#include #include using namespace std; using namespace gtsam; using namespace gtsam::symbol_shorthand; -const Matrix One = I_1x1; +static const Vector kOne = Vector::Ones(1), kZero = Vector::Zero(1); /* ************************************************************************* */ // Create test graph according to Forst10book_pg171Ex5 @@ -37,15 +37,17 @@ QP createTestCase() { // 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 + //TODO: THIS TEST MIGHT BE WRONG : the last parameter might be 5 instead of 10 because the form of the equation + // Should be 0.5x'Gx + gx + f : Nocedal 449 qp.cost.push_back( - HessianFactor(X(1), X(2), 2.0 * Matrix::Ones(1, 1), -Matrix::Ones(1, 1), 3.0 * I_1x1, - 2.0 * Matrix::Ones(1, 1), Z_1x1, 10.0)); + HessianFactor(X(1), X(2), 2.0 * I_1x1, -I_1x1, 3.0 * I_1x1, 2.0 * I_1x1, + Z_1x1, 10.0)); // Inequality constraints qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 - qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0 - qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2 + qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 1)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 2)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), I_1x1, 1.5, 3)); // x1 <= 3/2 return qp; } @@ -53,8 +55,8 @@ QP createTestCase() { TEST(QPSolver, TestCase) { VectorValues values; double x1 = 5, x2 = 7; - values.insert(X(1), x1 * Matrix::Ones(1, 1)); - values.insert(X(2), x2 * Matrix::Ones(1, 1)); + values.insert(X(1), x1 * I_1x1); + values.insert(X(2), x2 * I_1x1); 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); @@ -67,15 +69,15 @@ TEST(QPSolver, constraintsAux) { VectorValues lambdas; lambdas.insert(0, (Vector(1) << -0.5).finished()); - lambdas.insert(1, (Vector(1) << 0.0).finished()); - lambdas.insert(2, (Vector(1) << 0.3).finished()); - lambdas.insert(3, (Vector(1) << 0.1).finished()); + lambdas.insert(1, kZero); + lambdas.insert(2, (Vector(1) << 0.3).finished()); + lambdas.insert(3, (Vector(1) << 0.1).finished()); int factorIx = solver.identifyLeavingConstraint(qp.inequalities, lambdas); LONGS_EQUAL(2, factorIx); VectorValues lambdas2; lambdas2.insert(0, (Vector(1) << -0.5).finished()); - lambdas2.insert(1, (Vector(1) << 0.0).finished()); + lambdas2.insert(1, kZero); lambdas2.insert(2, (Vector(1) << -0.3).finished()); lambdas2.insert(3, (Vector(1) << -0.1).finished()); int factorIx2 = solver.identifyLeavingConstraint(qp.inequalities, lambdas2); @@ -92,14 +94,14 @@ QP createEqualityConstrainedTest() { // 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 * Matrix::Ones(1, 1), Z_1x1, Z_1x1, - 2.0 * Matrix::Ones(1, 1), Z_1x1, 0.0)); + HessianFactor(X(1), X(2), 2.0 * I_1x1, Z_1x1, Z_1x1, 2.0 * I_1x1, Z_1x1, + 0.0)); // Equality constraints // x1 + x2 = 1 --> x1 + x2 -1 = 0, hence we negate the b vector - Matrix A1 = (Matrix(1, 1) << 1).finished(); - Matrix A2 = (Matrix(1, 1) << 1).finished(); - Vector b = -(Vector(1) << 1).finished(); + Matrix A1 = I_1x1; + Matrix A2 = I_1x1; + Vector b = -kOne; qp.equalities.push_back(LinearEquality(X(1), A1, X(2), A2, b, 0)); return qp; @@ -132,20 +134,19 @@ TEST(QPSolver, indentifyActiveConstraints) { currentSolution.insert(X(1), Z_1x1); currentSolution.insert(X(2), Z_1x1); - LinearInequalityFactorGraph workingSet = - solver.identifyActiveConstraints(qp.inequalities, currentSolution); + InequalityFactorGraph workingSet = solver.identifyActiveConstraints( + qp.inequalities, currentSolution); - CHECK(!workingSet.at(0)->active()); // inactive - CHECK(workingSet.at(1)->active()); // active - CHECK(workingSet.at(2)->active()); // active - CHECK(!workingSet.at(3)->active()); // inactive + CHECK(!workingSet.at(0)->active()); // inactive + CHECK(workingSet.at(1)->active());// active + CHECK(workingSet.at(2)->active());// active + CHECK(!workingSet.at(3)->active());// inactive - VectorValues solution = solver.solveWithCurrentWorkingSet(workingSet); + VectorValues solution = solver.buildWorkingGraph(workingSet).optimize(); VectorValues expectedSolution; - expectedSolution.insert(X(1), (Vector(1) << 0.0).finished()); - expectedSolution.insert(X(2), (Vector(1) << 0.0).finished()); + expectedSolution.insert(X(1), kZero); + expectedSolution.insert(X(2), kZero); CHECK(assert_equal(expectedSolution, solution, 1e-100)); - } /* ************************************************************************* */ @@ -158,13 +159,13 @@ TEST(QPSolver, iterate) { currentSolution.insert(X(2), Z_1x1); std::vector expectedSolutions(4), expectedDuals(4); - expectedSolutions[0].insert(X(1), (Vector(1) << 0.0).finished()); - expectedSolutions[0].insert(X(2), (Vector(1) << 0.0).finished()); + expectedSolutions[0].insert(X(1), kZero); + expectedSolutions[0].insert(X(2), kZero); expectedDuals[0].insert(1, (Vector(1) << 3).finished()); - expectedDuals[0].insert(2, (Vector(1) << 0).finished()); + expectedDuals[0].insert(2, kZero); expectedSolutions[1].insert(X(1), (Vector(1) << 1.5).finished()); - expectedSolutions[1].insert(X(2), (Vector(1) << 0.0).finished()); + expectedSolutions[1].insert(X(2), kZero); expectedDuals[1].insert(3, (Vector(1) << 1.5).finished()); expectedSolutions[2].insert(X(1), (Vector(1) << 1.5).finished()); @@ -173,10 +174,11 @@ TEST(QPSolver, iterate) { expectedSolutions[3].insert(X(1), (Vector(1) << 1.5).finished()); expectedSolutions[3].insert(X(2), (Vector(1) << 0.5).finished()); - LinearInequalityFactorGraph workingSet = - solver.identifyActiveConstraints(qp.inequalities, currentSolution); + InequalityFactorGraph workingSet = solver.identifyActiveConstraints( + qp.inequalities, currentSolution); - QPState state(currentSolution, VectorValues(), workingSet, false); + QPSolver::State state(currentSolution, VectorValues(), workingSet, false, + 100); int it = 0; while (!state.converged) { @@ -209,6 +211,64 @@ TEST(QPSolver, optimizeForst10book_pg171Ex5) { CHECK(assert_equal(expectedSolution, solution, 1e-100)); } +pair testParser(QPSParser parser) { + QP exampleqp = parser.Parse(); + QP expectedqp; + Key X1(Symbol('X', 1)), X2(Symbol('X', 2)); + // min f(x,y) = 4 + 1.5x -y + 0.58x^2 + 2xy + 2yx + 10y^2 + expectedqp.cost.push_back( + HessianFactor(X1, X2, 8.0 * I_1x1, 2.0 * I_1x1, -1.5 * kOne, 10.0 * I_1x1, + 2.0 * kOne, 4.0)); + // 2x + y >= 2 + // -x + 2y <= 6 + expectedqp.inequalities.push_back( + LinearInequality(X1, -2.0 * I_1x1, X2, -I_1x1, -2, 0)); + expectedqp.inequalities.push_back( + LinearInequality(X1, -I_1x1, X2, 2.0 * I_1x1, 6, 1)); + // x<= 20 + expectedqp.inequalities.push_back(LinearInequality(X1, I_1x1, 20, 4)); + //x >= 0 + expectedqp.inequalities.push_back(LinearInequality(X1, -I_1x1, 0, 2)); + // y > = 0 + expectedqp.inequalities.push_back(LinearInequality(X2, -I_1x1, 0, 3)); + return std::make_pair(expectedqp, exampleqp); +} +; + +TEST(QPSolver, ParserSyntaticTest) { + auto expectedActual = testParser(QPSParser("QPExample.QPS")); + CHECK(assert_equal(expectedActual.first.cost, expectedActual.second.cost, + 1e-7)); + CHECK(assert_equal(expectedActual.first.inequalities, + expectedActual.second.inequalities, 1e-7)); + CHECK(assert_equal(expectedActual.first.equalities, + expectedActual.second.equalities, 1e-7)); +} + +TEST(QPSolver, ParserSemanticTest) { + auto expected_actual = testParser(QPSParser("QPExample.QPS")); + VectorValues actualSolution, expectedSolution; + boost::tie(expectedSolution, boost::tuples::ignore) = + QPSolver(expected_actual.first).optimize(); + boost::tie(actualSolution, boost::tuples::ignore) = + QPSolver(expected_actual.second).optimize(); + CHECK(assert_equal(actualSolution, expectedSolution, 1e-7)); +} + +TEST(QPSolver, QPExampleTest){ + QP problem = QPSParser("QPExample.QPS").Parse(); + VectorValues actualSolution; + auto solver = QPSolver(problem); + boost::tie(actualSolution, boost::tuples::ignore) = solver.optimize(); + VectorValues expectedSolution; + expectedSolution.insert(Symbol('X',1),0.7625*I_1x1); + expectedSolution.insert(Symbol('X',2),0.4750*I_1x1); + double error_expected = problem.cost.error(expectedSolution); + double error_actual = problem.cost.error(actualSolution); + CHECK(assert_equal(expectedSolution, actualSolution, 1e-7)) + CHECK(assert_equal(error_expected, error_actual)) +} + /* ************************************************************************* */ // Create Matlab's test graph as in http://www.mathworks.com/help/optim/ug/quadprog.html QP createTestMatlabQPEx() { @@ -219,19 +279,22 @@ QP createTestMatlabQPEx() { // 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 * I_1x1, -Matrix::Ones(1, 1), 2.0 * I_1x1, - 2.0 * Matrix::Ones(1, 1), 6 * I_1x1, 1000.0)); + HessianFactor(X(1), X(2), 1.0 * I_1x1, -I_1x1, 2.0 * I_1x1, 2.0 * I_1x1, + 6 * I_1x1, 1000.0)); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), One, X(2), One, 2, 0)); // x1 + x2 <= 2 - qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2*One, 2, 1)); //-x1 + 2*x2 <=2 - qp.inequalities.push_back(LinearInequality(X(1), 2*One, X(2), One, 3, 2)); // 2*x1 + x2 <=3 - qp.inequalities.push_back(LinearInequality(X(1), -One, 0, 3)); // -x1 <= 0 - qp.inequalities.push_back(LinearInequality(X(2), -One, 0, 4)); // -x2 <= 0 + qp.inequalities.push_back(LinearInequality(X(1), I_1x1, X(2), I_1x1, 2, 0)); // x1 + x2 <= 2 + qp.inequalities.push_back( + LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 1)); //-x1 + 2*x2 <=2 + qp.inequalities.push_back( + LinearInequality(X(1), 2 * I_1x1, X(2), I_1x1, 3, 2)); // 2*x1 + x2 <=3 + qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0, 3)); // -x1 <= 0 + qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0, 4)); // -x2 <= 0 return qp; } +///* ************************************************************************* */ TEST(QPSolver, optimizeMatlabEx) { QP qp = createTestMatlabQPEx(); QPSolver solver(qp); @@ -246,20 +309,35 @@ TEST(QPSolver, optimizeMatlabEx) { CHECK(assert_equal(expectedSolution, solution, 1e-7)); } +///* ************************************************************************* */ +TEST(QPSolver, optimizeMatlabExNoinitials) { + QP qp = createTestMatlabQPEx(); + QPSolver solver(qp); + VectorValues solution; + boost::tie(solution, boost::tuples::ignore) = solver.optimize(); + VectorValues expectedSolution; + expectedSolution.insert(X(1), (Vector(1) << 2.0 / 3.0).finished()); + expectedSolution.insert(X(2), (Vector(1) << 4.0 / 3.0).finished()); + 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), Matrix::Ones(1, 1), I_1x1)); - qp.cost.push_back(JacobianFactor(X(2), Matrix::Ones(1, 1), 2.5 * I_1x1)); + qp.cost.push_back(JacobianFactor(X(1), I_1x1, I_1x1)); + qp.cost.push_back(JacobianFactor(X(2), I_1x1, 2.5 * I_1x1)); // Inequality constraints - qp.inequalities.push_back(LinearInequality(X(1), -One, X(2), 2 * One, 2, 0)); - qp.inequalities.push_back(LinearInequality(X(1), One, X(2), 2 * One, 6, 1)); - qp.inequalities.push_back(LinearInequality(X(1), One, X(2), -2 * One, 2, 2)); - qp.inequalities.push_back(LinearInequality(X(1), -One, 0.0, 3)); - qp.inequalities.push_back(LinearInequality(X(2), -One, 0.0, 4)); + qp.inequalities.push_back( + LinearInequality(X(1), -I_1x1, X(2), 2 * I_1x1, 2, 0)); + qp.inequalities.push_back( + LinearInequality(X(1), I_1x1, X(2), 2 * I_1x1, 6, 1)); + qp.inequalities.push_back( + LinearInequality(X(1), I_1x1, X(2), -2 * I_1x1, 2, 2)); + qp.inequalities.push_back(LinearInequality(X(1), -I_1x1, 0.0, 3)); + qp.inequalities.push_back(LinearInequality(X(2), -I_1x1, 0.0, 4)); return qp; } @@ -280,28 +358,45 @@ TEST(QPSolver, optimizeNocedal06bookEx16_4) { } /* ************************************************************************* */ - TEST(QPSolver, failedSubproblem) { QP qp; qp.cost.push_back(JacobianFactor(X(1), I_2x2, Z_2x1)); qp.cost.push_back(HessianFactor(X(1), Z_2x2, Z_2x1, 100.0)); qp.inequalities.push_back( - LinearInequality(X(1), (Matrix(1,2) << -1.0, 0.0).finished(), -1.0, 0)); + LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0)); VectorValues expected; expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); VectorValues initialValues; - initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished()); + initialValues.insert(X(1), (Vector(2) << 10.0, 100.0).finished()); QPSolver solver(qp); VectorValues solution; boost::tie(solution, boost::tuples::ignore) = solver.optimize(initialValues); -// graph.print("Graph: "); -// solution.print("Solution: "); + CHECK(assert_equal(expected, solution, 1e-7)); } +/* ************************************************************************* */ +TEST(QPSolver, infeasibleInitial) { + QP qp; + qp.cost.push_back(JacobianFactor(X(1), I_2x2, Vector::Zero(2))); + qp.cost.push_back(HessianFactor(X(1), Z_2x2, Vector::Zero(2), 100.0)); + qp.inequalities.push_back( + LinearInequality(X(1), (Matrix(1, 2) << -1.0, 0.0).finished(), -1.0, 0)); + + VectorValues expected; + expected.insert(X(1), (Vector(2) << 1.0, 0.0).finished()); + + VectorValues initialValues; + initialValues.insert(X(1), (Vector(2) << -10.0, 100.0).finished()); + + QPSolver solver(qp); + VectorValues solution; + CHECK_EXCEPTION(solver.optimize(initialValues), InfeasibleInitialValues); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/wrap/matlab.h b/wrap/matlab.h index 162aaa0e2..de3027eac 100644 --- a/wrap/matlab.h +++ b/wrap/matlab.h @@ -60,15 +60,15 @@ using namespace boost; // not usual, but for conciseness of generated code // "Unique" key to signal calling the matlab object constructor with a raw pointer // to a shared pointer of the same C++ object type as the MATLAB type. // Also present in utilities.h -static const uint64_t ptr_constructor_key = - (uint64_t('G') << 56) | - (uint64_t('T') << 48) | - (uint64_t('S') << 40) | - (uint64_t('A') << 32) | - (uint64_t('M') << 24) | - (uint64_t('p') << 16) | - (uint64_t('t') << 8) | - (uint64_t('r')); +static const boost::uint64_t ptr_constructor_key = + (boost::uint64_t('G') << 56) | + (boost::uint64_t('T') << 48) | + (boost::uint64_t('S') << 40) | + (boost::uint64_t('A') << 32) | + (boost::uint64_t('M') << 24) | + (boost::uint64_t('p') << 16) | + (boost::uint64_t('t') << 8) | + (boost::uint64_t('r')); //***************************************************************************** // Utilities @@ -244,9 +244,9 @@ template T myGetScalar(const mxArray* array) { switch (mxGetClassID(array)) { case mxINT64_CLASS: - return (T) *(int64_t*) mxGetData(array); + return (T) *(boost::int64_t*) mxGetData(array); case mxUINT64_CLASS: - return (T) *(uint64_t*) mxGetData(array); + return (T) *(boost::uint64_t*) mxGetData(array); default: // hope for the best! return (T) mxGetScalar(array); @@ -349,7 +349,7 @@ mxArray* create_object(const std::string& classname, void *pointer, bool isVirtu int nargin = 2; // First input argument is pointer constructor key input[0] = mxCreateNumericMatrix(1, 1, mxUINT64_CLASS, mxREAL); - *reinterpret_cast(mxGetData(input[0])) = ptr_constructor_key; + *reinterpret_cast(mxGetData(input[0])) = ptr_constructor_key; // Second input argument is the pointer input[1] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); *reinterpret_cast(mxGetData(input[1])) = pointer;