From b6d85e83aed239a76225f84d9034fab93e536033 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Mon, 22 Dec 2014 18:56:19 -0500 Subject: [PATCH] Fixed includes. --- .../LinearEqualityManifoldFactorGraph.h | 2 + .../NonlinearInequalityFactorGraph.h | 1 + gtsam_unstable/nonlinear/SQPSimple.h | 172 ++++++++++++++++++ .../nonlinear/tests/testSQPSimple.cpp | 169 +---------------- 4 files changed, 179 insertions(+), 165 deletions(-) create mode 100644 gtsam_unstable/nonlinear/SQPSimple.h diff --git a/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h b/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h index 50f9c3245..03a01fd3c 100644 --- a/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h +++ b/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h @@ -8,6 +8,8 @@ #pragma once #include +#include +#include namespace gtsam { diff --git a/gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h b/gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h index a76caeeb6..301c66505 100644 --- a/gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h +++ b/gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h @@ -6,6 +6,7 @@ #pragma once #include +#include namespace gtsam { class NonlinearInequalityFactorGraph : public FactorGraph { diff --git a/gtsam_unstable/nonlinear/SQPSimple.h b/gtsam_unstable/nonlinear/SQPSimple.h new file mode 100644 index 000000000..df44f1cda --- /dev/null +++ b/gtsam_unstable/nonlinear/SQPSimple.h @@ -0,0 +1,172 @@ +/** + * @file SQPSimple.h + * @author Krunal Chande + * @date Dec 22, 2014 + */ + +#pragma once +#include +#include +#include +#include +#include +#include +#include + + + +namespace gtsam { + +struct NLP { + NonlinearFactorGraph cost; + NonlinearEqualityFactorGraph linearEqualities; + NonlinearEqualityFactorGraph nonlinearEqualities; + NonlinearInequalityFactorGraph linearInequalities; +}; + +struct SQPSimpleState { + Values values; + VectorValues duals; + bool converged; + size_t iterations; + + /// Default constructor + SQPSimpleState() : values(), duals(), converged(false), iterations(0) {} + + /// Constructor with an initialValues + SQPSimpleState(const Values& initialValues) : + values(initialValues), duals(VectorValues()), converged(false), iterations(0) { + } +}; + +/** + * Simple SQP optimizer to solve nonlinear constrained problems. + * This simple version won't care about nonconvexity, which needs + * more advanced techniques to solve, e.g., merit function, line search, second-order correction etc. + */ +class SQPSimple { + NLP nlp_; + static const double errorTol = 1e-5; +public: + SQPSimple(const NLP& nlp) : + nlp_(nlp) { + } + + /// Check if \nabla f(x) - \lambda * \nabla c(x) == 0 + bool isStationary(const VectorValues& delta) const { + return delta.vector().lpNorm() < errorTol; + } + + /// Check if c_E(x) == 0 + bool isPrimalFeasible(const SQPSimpleState& state) const { + return nlp_.linearEqualities.checkFeasibility(state.values, errorTol) + && nlp_.nonlinearEqualities.checkFeasibility(state.values, errorTol); + } + + /** + * Dual variables of inequality constraints need to be >=0 + * For active inequalities, the dual needs to be > 0 + * For inactive inequalities, they need to be == 0. However, we don't compute + * dual variables for inactive constraints in the qp subproblem, so we don't care. + */ + bool isDualFeasible(const VectorValues& duals) const { + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, nlp_.linearInequalities) { + NonlinearConstraint::shared_ptr inequality = boost::dynamic_pointer_cast(factor); + Key dualKey = inequality->dualKey(); + if (!duals.exists(dualKey)) continue; // should be inactive constraint! + double dual = duals.at(dualKey)[0]; // because we only support single-valued inequalities + if (dual < 0.0) + return false; + } + return true; + } + + /** + * Check complimentary slackness condition: + * For all inequality constraints, + * dual * constraintError(primals) == 0. + * If the constraint is active, we need to check constraintError(primals) == 0, and ignore the dual + * If it is inactive, the dual should be 0, regardless of the error. However, we don't compute + * dual variables for inactive constraints in the QP subproblem, so we don't care. + */ + bool isComplementary(const SQPSimpleState& state) const { + return nlp_.linearInequalities.checkFeasibilityAndComplimentary(state.values, state.duals, errorTol); + } + + /// Check convergence + bool checkConvergence(const SQPSimpleState& state, const VectorValues& delta) const { + return isStationary(delta) && isPrimalFeasible(state) && isDualFeasible(state.duals) && isComplementary(state); + } + + /** + * Single iteration of SQP + */ + SQPSimpleState iterate(const SQPSimpleState& state) const { + static const bool debug = false; + + // construct the qp subproblem + QP qp; + qp.cost = *nlp_.cost.linearize(state.values); + GaussianFactorGraph::shared_ptr multipliedHessians = nlp_.nonlinearEqualities.multipliedHessians(state.values, state.duals); + qp.cost.push_back(*multipliedHessians); + + qp.equalities.add(*nlp_.linearEqualities.linearize(state.values)); + qp.equalities.add(*nlp_.nonlinearEqualities.linearize(state.values)); + + qp.inequalities.add(*nlp_.linearInequalities.linearize(state.values)); + + if (debug) + qp.print("QP subproblem:"); + + // solve the QP subproblem + VectorValues delta, duals; + QPSolver qpSolver(qp); + boost::tie(delta, duals) = qpSolver.optimize(); + + if (debug) + delta.print("delta = "); + + if (debug) + duals.print("duals = "); + + // update new state + SQPSimpleState newState; + newState.values = state.values.retract(delta); + newState.duals = duals; + newState.converged = checkConvergence(newState, delta); + newState.iterations = state.iterations + 1; + + return newState; + } + + VectorValues initializeDuals() const { + VectorValues duals; + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, nlp_.linearEqualities) { + NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); + duals.insert(constraint->dualKey(), zero(factor->dim())); + } + + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, nlp_.nonlinearEqualities) { + NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); + duals.insert(constraint->dualKey(), zero(factor->dim())); + } + return duals; + } + + /** + * Main optimization function. + */ + std::pair optimize(const Values& initialValues) const { + SQPSimpleState state(initialValues); + state.duals = initializeDuals(); + + while (!state.converged && state.iterations < 100) { + state = iterate(state); + } + + return std::make_pair(state.values, state.duals); + } + + +}; +} diff --git a/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp b/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp index e8a14203b..19e9205cc 100644 --- a/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp +++ b/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp @@ -18,178 +18,17 @@ * @date Dec 15, 2014 */ +#include +#include #include -#include #include +#include #include -#include +#include #include -#include -#include -#include #include #include -namespace gtsam { - -struct NLP { - NonlinearFactorGraph cost; - NonlinearEqualityFactorGraph linearEqualities; - NonlinearEqualityFactorGraph nonlinearEqualities; - NonlinearInequalityFactorGraph linearInequalities; -}; - -struct SQPSimpleState { - Values values; - VectorValues duals; - bool converged; - size_t iterations; - - /// Default constructor - SQPSimpleState() : values(), duals(), converged(false), iterations(0) {} - - /// Constructor with an initialValues - SQPSimpleState(const Values& initialValues) : - values(initialValues), duals(VectorValues()), converged(false), iterations(0) { - } -}; - -/** - * Simple SQP optimizer to solve nonlinear constrained problems. - * This simple version won't care about nonconvexity, which needs - * more advanced techniques to solve, e.g., merit function, line search, second-order correction etc. - */ -class SQPSimple { - NLP nlp_; - static const double errorTol = 1e-5; -public: - SQPSimple(const NLP& nlp) : - nlp_(nlp) { - } - - /// Check if \nabla f(x) - \lambda * \nabla c(x) == 0 - bool isStationary(const VectorValues& delta) const { - return delta.vector().lpNorm() < errorTol; - } - - /// Check if c_E(x) == 0 - bool isPrimalFeasible(const SQPSimpleState& state) const { - return nlp_.linearEqualities.checkFeasibility(state.values, errorTol) - && nlp_.nonlinearEqualities.checkFeasibility(state.values, errorTol); - } - - /** - * Dual variables of inequality constraints need to be >=0 - * For active inequalities, the dual needs to be > 0 - * For inactive inequalities, they need to be == 0. However, we don't compute - * dual variables for inactive constraints in the qp subproblem, so we don't care. - */ - bool isDualFeasible(const VectorValues& duals) const { - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, nlp_.linearInequalities) { - NonlinearConstraint::shared_ptr inequality = boost::dynamic_pointer_cast(factor); - Key dualKey = inequality->dualKey(); - if (!duals.exists(dualKey)) continue; // should be inactive constraint! - double dual = duals.at(dualKey)[0]; // because we only support single-valued inequalities - if (dual < 0.0) - return false; - } - return true; - } - - /** - * Check complimentary slackness condition: - * For all inequality constraints, - * dual * constraintError(primals) == 0. - * If the constraint is active, we need to check constraintError(primals) == 0, and ignore the dual - * If it is inactive, the dual should be 0, regardless of the error. However, we don't compute - * dual variables for inactive constraints in the QP subproblem, so we don't care. - */ - bool isComplementary(const SQPSimpleState& state) const { - return nlp_.linearInequalities.checkFeasibilityAndComplimentary(state.values, state.duals, errorTol); - } - - /// Check convergence - bool checkConvergence(const SQPSimpleState& state, const VectorValues& delta) const { - return isStationary(delta) && isPrimalFeasible(state) && isDualFeasible(state.duals) && isComplementary(state); - } - - /** - * Single iteration of SQP - */ - SQPSimpleState iterate(const SQPSimpleState& state) const { - static const bool debug = false; - - // construct the qp subproblem - QP qp; - qp.cost = *nlp_.cost.linearize(state.values); - GaussianFactorGraph::shared_ptr multipliedHessians = nlp_.nonlinearEqualities.multipliedHessians(state.values, state.duals); - qp.cost.push_back(*multipliedHessians); - - qp.equalities.add(*nlp_.linearEqualities.linearize(state.values)); - qp.equalities.add(*nlp_.nonlinearEqualities.linearize(state.values)); - - qp.inequalities.add(*nlp_.linearInequalities.linearize(state.values)); - - if (debug) - qp.print("QP subproblem:"); - - // solve the QP subproblem - VectorValues delta, duals; - QPSolver qpSolver(qp); - boost::tie(delta, duals) = qpSolver.optimize(); - - if (debug) - delta.print("delta = "); - - if (debug) - duals.print("duals = "); - - // update new state - SQPSimpleState newState; - newState.values = state.values.retract(delta); - newState.duals = duals; - newState.converged = checkConvergence(newState, delta); - newState.iterations = state.iterations + 1; - - return newState; - } - - VectorValues initializeDuals() const { - VectorValues duals; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, nlp_.linearEqualities) { - NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); - duals.insert(constraint->dualKey(), zero(factor->dim())); - } - - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, nlp_.nonlinearEqualities) { - NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); - duals.insert(constraint->dualKey(), zero(factor->dim())); - } - return duals; - } - - /** - * Main optimization function. - */ - std::pair optimize(const Values& initialValues) const { - SQPSimpleState state(initialValues); - state.duals = initializeDuals(); - - while (!state.converged && state.iterations < 100) { - state = iterate(state); - } - - return std::make_pair(state.values, state.duals); - } - - -}; -} - -#include -#include -#include - using namespace std; using namespace gtsam::symbol_shorthand; using namespace gtsam;