From de7149af63fc9521701dda30875c570ff658db01 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Mon, 22 Dec 2014 19:49:32 -0500 Subject: [PATCH] Added licensing information. --- gtsam_unstable/linear/QPSolver.h | 10 ++ .../LinearEqualityManifoldFactorGraph.h | 10 ++ .../nonlinear/NonlinearConstraint.h | 10 ++ .../nonlinear/NonlinearEqualityFactorGraph.h | 19 ++- .../nonlinear/NonlinearInequality.h | 10 ++ .../NonlinearInequalityFactorGraph.h | 19 ++- gtsam_unstable/nonlinear/SQPSimple.cpp | 124 ++++++++++++++++++ gtsam_unstable/nonlinear/SQPSimple.h | 115 ++++------------ .../nonlinear/tests/testSQPSimple.cpp | 2 +- 9 files changed, 221 insertions(+), 98 deletions(-) create mode 100644 gtsam_unstable/nonlinear/SQPSimple.cpp diff --git a/gtsam_unstable/linear/QPSolver.h b/gtsam_unstable/linear/QPSolver.h index f7a575f8c..53cb6ca6d 100644 --- a/gtsam_unstable/linear/QPSolver.h +++ b/gtsam_unstable/linear/QPSolver.h @@ -1,3 +1,13 @@ +/* ---------------------------------------------------------------------------- + + * 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 + + * -------------------------------------------------------------------------- */ /* * QPSolver.h * @brief: A quadratic programming solver implements the active set method diff --git a/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h b/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h index 03a01fd3c..a20f2e5a3 100644 --- a/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h +++ b/gtsam_unstable/nonlinear/LinearEqualityManifoldFactorGraph.h @@ -1,3 +1,13 @@ +/* ---------------------------------------------------------------------------- + + * 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 LinearEqualityManifoldFactorGraph.h * @author Duy-Nguyen Ta diff --git a/gtsam_unstable/nonlinear/NonlinearConstraint.h b/gtsam_unstable/nonlinear/NonlinearConstraint.h index d4c7f86b5..81208872f 100644 --- a/gtsam_unstable/nonlinear/NonlinearConstraint.h +++ b/gtsam_unstable/nonlinear/NonlinearConstraint.h @@ -1,3 +1,13 @@ +/* ---------------------------------------------------------------------------- + + * 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 NonlinearConstraint.h * @brief diff --git a/gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h b/gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h index 116b273fb..1d82c6fbf 100644 --- a/gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h +++ b/gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h @@ -1,7 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * 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 NonlinearEqualityFactorGraph.h - * @author Krunal Chande - * @date Dec 22, 2014 + * @file NonlinearEqualityFactorGraph.h + * @author Duy-Nguyen Ta + * @author Krunal Chande + * @author Luca Carlone + * @date Dec 15, 2014 */ #pragma once diff --git a/gtsam_unstable/nonlinear/NonlinearInequality.h b/gtsam_unstable/nonlinear/NonlinearInequality.h index 8d41b3ec0..439d5e070 100644 --- a/gtsam_unstable/nonlinear/NonlinearInequality.h +++ b/gtsam_unstable/nonlinear/NonlinearInequality.h @@ -1,3 +1,13 @@ +/* ---------------------------------------------------------------------------- + + * 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 NonlinearConstraint.h * @brief diff --git a/gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h b/gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h index 301c66505..0eccd1fb0 100644 --- a/gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h +++ b/gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h @@ -1,7 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * 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 NonlinearInequalityFactorGraph.h - * @author Krunal Chande - * @date Dec 22, 2014 + * @file NonlinearInequalityFactorGraph.h + * @author Duy-Nguyen Ta + * @author Krunal Chande + * @author Luca Carlone + * @date Dec 15, 2014 */ #pragma once diff --git a/gtsam_unstable/nonlinear/SQPSimple.cpp b/gtsam_unstable/nonlinear/SQPSimple.cpp new file mode 100644 index 000000000..35cfb0a1b --- /dev/null +++ b/gtsam_unstable/nonlinear/SQPSimple.cpp @@ -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 SQPSimple.cpp + * @author Duy-Nguyen Ta + * @author Krunal Chande + * @author Luca Carlone + * @date Dec 15, 2014 + */ + +#include +namespace gtsam { + + +/* ************************************************************************* */ +bool SQPSimple::isStationary(const VectorValues& delta) const { + return delta.vector().lpNorm() < errorTol; +} + +/* ************************************************************************* */ +bool SQPSimple::isPrimalFeasible(const SQPSimpleState& state) const { + return nlp_.linearEqualities.checkFeasibility(state.values, errorTol) + && nlp_.nonlinearEqualities.checkFeasibility(state.values, errorTol); +} + +/* ************************************************************************* */ +bool SQPSimple::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; +} + +/* ************************************************************************* */ +bool SQPSimple::isComplementary(const SQPSimpleState& state) const { + return nlp_.linearInequalities.checkFeasibilityAndComplimentary(state.values, state.duals, errorTol); +} + +/* ************************************************************************* */ +bool SQPSimple::checkConvergence(const SQPSimpleState& state, const VectorValues& delta) const { + return isStationary(delta) && isPrimalFeasible(state) && isDualFeasible(state.duals) && isComplementary(state); +} + +/* ************************************************************************* */ +VectorValues SQPSimple::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; +} + +/* ************************************************************************* */ +std::pair SQPSimple::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); +} + +/* ************************************************************************* */ +SQPSimpleState SQPSimple::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; +} +} + + + diff --git a/gtsam_unstable/nonlinear/SQPSimple.h b/gtsam_unstable/nonlinear/SQPSimple.h index df44f1cda..ceda522ea 100644 --- a/gtsam_unstable/nonlinear/SQPSimple.h +++ b/gtsam_unstable/nonlinear/SQPSimple.h @@ -1,7 +1,20 @@ +/* ---------------------------------------------------------------------------- + + * 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 SQPSimple.h - * @author Krunal Chande - * @date Dec 22, 2014 + * @file SQPSimple.h + * @author Duy-Nguyen Ta + * @author Krunal Chande + * @author Luca Carlone + * @date Dec 15, 2014 */ #pragma once @@ -13,8 +26,6 @@ #include #include - - namespace gtsam { struct NLP { @@ -53,15 +64,10 @@ public: } /// Check if \nabla f(x) - \lambda * \nabla c(x) == 0 - bool isStationary(const VectorValues& delta) const { - return delta.vector().lpNorm() < errorTol; - } + bool isStationary(const VectorValues& delta) const; /// 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); - } + bool isPrimalFeasible(const SQPSimpleState& state) const; /** * Dual variables of inequality constraints need to be >=0 @@ -69,17 +75,7 @@ public: * 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; - } + bool isDualFeasible(const VectorValues& duals) const; /** * Check complimentary slackness condition: @@ -89,84 +85,21 @@ public: * 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); - } + bool isComplementary(const SQPSimpleState& state) const; /// Check convergence - bool checkConvergence(const SQPSimpleState& state, const VectorValues& delta) const { - return isStationary(delta) && isPrimalFeasible(state) && isDualFeasible(state.duals) && isComplementary(state); - } + bool checkConvergence(const SQPSimpleState& state, const VectorValues& delta) const; /** * 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; - } + SQPSimpleState iterate(const SQPSimpleState& state) const; + VectorValues initializeDuals() const; /** * 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); - } - + std::pair optimize(const Values& initialValues) const; }; } diff --git a/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp b/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp index 94e7b08e4..a14e52d71 100644 --- a/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp +++ b/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp @@ -12,8 +12,8 @@ /** * @file testQPSimple.cpp * @brief Unit tests for testQPSimple - * @author Krunal Chande * @author Duy-Nguyen Ta + * @author Krunal Chande * @author Luca Carlone * @date Dec 15, 2014 */