From d9773da12554128ffad0946c1a176afafd5f6ecd Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Tue, 24 Feb 2015 23:40:53 -0500 Subject: [PATCH] rename classes --- gtsam_unstable/nonlinear/ConstrainedFactor.h | 52 +++++ ...CNLPSolver.cpp => LinearConstraintSQP.cpp} | 58 ++--- .../{LCNLPSolver.h => LinearConstraintSQP.h} | 43 ++-- .../nonlinear/LinearEqualityFactor.h | 192 ++++++++++++++++ ...torGraph.h => LinearEqualityFactorGraph.h} | 35 ++- ...rInequality.h => LinearInequalityFactor.h} | 60 +++-- ...rGraph.h => LinearInequalityFactorGraph.h} | 55 +++-- .../nonlinear/NonlinearConstraint.h | 50 +--- ...stLCNLPSolver.cpp => testLCNLPSolver2.cpp} | 192 ++-------------- .../tests/testLinearConstraintSQP.cpp | 214 ++++++++++++++++++ 10 files changed, 602 insertions(+), 349 deletions(-) create mode 100644 gtsam_unstable/nonlinear/ConstrainedFactor.h rename gtsam_unstable/nonlinear/{LCNLPSolver.cpp => LinearConstraintSQP.cpp} (78%) rename gtsam_unstable/nonlinear/{LCNLPSolver.h => LinearConstraintSQP.h} (67%) create mode 100644 gtsam_unstable/nonlinear/LinearEqualityFactor.h rename gtsam_unstable/nonlinear/{NonlinearEqualityFactorGraph.h => LinearEqualityFactorGraph.h} (55%) rename gtsam_unstable/nonlinear/{NonlinearInequality.h => LinearInequalityFactor.h} (66%) rename gtsam_unstable/nonlinear/{NonlinearInequalityFactorGraph.h => LinearInequalityFactorGraph.h} (51%) rename gtsam_unstable/nonlinear/tests/{testLCNLPSolver.cpp => testLCNLPSolver2.cpp} (65%) create mode 100644 gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp diff --git a/gtsam_unstable/nonlinear/ConstrainedFactor.h b/gtsam_unstable/nonlinear/ConstrainedFactor.h new file mode 100644 index 000000000..b95ca4ed6 --- /dev/null +++ b/gtsam_unstable/nonlinear/ConstrainedFactor.h @@ -0,0 +1,52 @@ +/* ---------------------------------------------------------------------------- + + * 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 ConstrainedFactor.h + * @brief + * @author Duy-Nguyen Ta + * @date Sep 30, 2013 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * A base class for all ConstrainedFactor factors, + * containing additional information for the constraint, + * e.g., a unique key for the dual variable associated with it, + * and special treatments for nonlinear constraint linearization in SQP. + * + * Derived classes of ConstrainedFactor should also inherit from + * NoiseModelFactorX to reuse the normal linearization procedure in NonlinearFactor + */ +class ConstrainedFactor { + +protected: + Key dualKey_; //!< Unique key for the dual variable associated with this constraint + +public: + typedef boost::shared_ptr shared_ptr; + +public: + /// Construct with dual key + ConstrainedFactor(Key dualKey) : dualKey_(dualKey) {} + + /// Return the dual key + Key dualKey() const { return dualKey_; } + +}; + + +} /* namespace gtsam */ diff --git a/gtsam_unstable/nonlinear/LCNLPSolver.cpp b/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp similarity index 78% rename from gtsam_unstable/nonlinear/LCNLPSolver.cpp rename to gtsam_unstable/nonlinear/LinearConstraintSQP.cpp index 722576eba..1985fa3e8 100644 --- a/gtsam_unstable/nonlinear/LCNLPSolver.cpp +++ b/gtsam_unstable/nonlinear/LinearConstraintSQP.cpp @@ -10,34 +10,34 @@ * -------------------------------------------------------------------------- */ /** - * @file LCNLPSolver.cpp + * @file LinearConstraintSQP.cpp * @author Duy-Nguyen Ta * @author Krunal Chande * @author Luca Carlone * @date Dec 15, 2014 */ -#include +#include #include #include namespace gtsam { /* ************************************************************************* */ -bool LCNLPSolver::isStationary(const VectorValues& delta) const { +bool LinearConstraintSQP::isStationary(const VectorValues& delta) const { return delta.vector().lpNorm() < errorTol; } /* ************************************************************************* */ -bool LCNLPSolver::isPrimalFeasible(const LCNLPState& state) const { +bool LinearConstraintSQP::isPrimalFeasible(const LinearConstraintNLPState& state) const { return lcnlp_.linearEqualities.checkFeasibility(state.values, errorTol); } /* ************************************************************************* */ -bool LCNLPSolver::isDualFeasible(const VectorValues& duals) const { +bool LinearConstraintSQP::isDualFeasible(const VectorValues& duals) const { BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities){ - NonlinearConstraint::shared_ptr inequality - = boost::dynamic_pointer_cast(factor); + ConstrainedFactor::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 @@ -49,24 +49,24 @@ bool LCNLPSolver::isDualFeasible(const VectorValues& duals) const { } /* ************************************************************************* */ -bool LCNLPSolver::isComplementary(const LCNLPState& state) const { +bool LinearConstraintSQP::isComplementary(const LinearConstraintNLPState& state) const { return lcnlp_.linearInequalities.checkFeasibilityAndComplimentary( state.values, state.duals, errorTol); } /* ************************************************************************* */ -bool LCNLPSolver::checkConvergence(const LCNLPState& state, +bool LinearConstraintSQP::checkConvergence(const LinearConstraintNLPState& state, const VectorValues& delta) const { return isStationary(delta) && isPrimalFeasible(state) && isDualFeasible(state.duals) && isComplementary(state); } /* ************************************************************************* */ -VectorValues LCNLPSolver::initializeDuals() const { +VectorValues LinearConstraintSQP::initializeDuals() const { VectorValues duals; BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearEqualities){ - NonlinearConstraint::shared_ptr constraint - = boost::dynamic_pointer_cast(factor); + ConstrainedFactor::shared_ptr constraint + = boost::dynamic_pointer_cast(factor); duals.insert(constraint->dualKey(), zero(factor->dim())); } @@ -74,22 +74,7 @@ VectorValues LCNLPSolver::initializeDuals() const { } /* ************************************************************************* */ -std::pair LCNLPSolver::optimize( - const Values& initialValues, bool useWarmStart, bool debug) const { - LCNLPState state(initialValues); - state.duals = initializeDuals(); - while (!state.converged && state.iterations < 100) { - if (debug) - std::cout << "state: iteration " << state.iterations << std::endl; - state = iterate(state, useWarmStart, debug); - } - if (debug) - std::cout << "Number of iterations: " << state.iterations << std::endl; - return std::make_pair(state.values, state.duals); -} - -/* ************************************************************************* */ -LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart, +LinearConstraintNLPState LinearConstraintSQP::iterate(const LinearConstraintNLPState& state, bool useWarmStart, bool debug) const { // construct the qp subproblem @@ -120,7 +105,7 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart, // duals.print("duals = "); // update new state - LCNLPState newState; + LinearConstraintNLPState newState; newState.values = state.values.retract(delta); newState.duals = duals; newState.converged = checkConvergence(newState, delta); @@ -132,5 +117,20 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart, return newState; } +/* ************************************************************************* */ +std::pair LinearConstraintSQP::optimize( + const Values& initialValues, bool useWarmStart, bool debug) const { + LinearConstraintNLPState state(initialValues); + state.duals = initializeDuals(); + while (!state.converged && state.iterations < 100) { + if (debug) + std::cout << "state: iteration " << state.iterations << std::endl; + state = iterate(state, useWarmStart, debug); + } + if (debug) + std::cout << "Number of iterations: " << state.iterations << std::endl; + return std::make_pair(state.values, state.duals); +} + } // namespace gtsam diff --git a/gtsam_unstable/nonlinear/LCNLPSolver.h b/gtsam_unstable/nonlinear/LinearConstraintSQP.h similarity index 67% rename from gtsam_unstable/nonlinear/LCNLPSolver.h rename to gtsam_unstable/nonlinear/LinearConstraintSQP.h index b05ddf8bd..00e276943 100644 --- a/gtsam_unstable/nonlinear/LCNLPSolver.h +++ b/gtsam_unstable/nonlinear/LinearConstraintSQP.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file LCNLPSolver.h + * @file LinearConstraintSQP.h * @author Duy-Nguyen Ta * @author Krunal Chande * @author Luca Carlone @@ -19,8 +19,8 @@ #pragma once #include -#include -#include +#include +#include namespace gtsam { @@ -28,26 +28,26 @@ namespace gtsam { * Nonlinear Programming problem with * only linear constraints, encoded in factor graphs */ -struct LCNLP { +struct LinearConstraintNLP { NonlinearFactorGraph cost; - NonlinearEqualityFactorGraph linearEqualities; - NonlinearInequalityFactorGraph linearInequalities; + LinearEqualityFactorGraph linearEqualities; + LinearInequalityFactorGraph linearInequalities; }; /** - * State of LCNLPSolver before/after each iteration + * State of LinearConstraintSQP before/after each iteration */ -struct LCNLPState { - Values values; - VectorValues duals; - bool converged; - size_t iterations; +struct LinearConstraintNLPState { + Values values; //!< current solution + VectorValues duals; //!< current guess of the dual variables + bool converged; //!< convergence flag + size_t iterations; //!< number of iterations /// Default constructor - LCNLPState() : values(), duals(), converged(false), iterations(0) {} + LinearConstraintNLPState() : values(), duals(), converged(false), iterations(0) {} /// Constructor with an initialValues - LCNLPState(const Values& initialValues) : + LinearConstraintNLPState(const Values& initialValues) : values(initialValues), duals(VectorValues()), converged(false), iterations(0) { } @@ -66,11 +66,11 @@ struct LCNLPState { * Simple SQP optimizer to solve nonlinear constrained problems * ONLY linear constraints are supported. */ -class LCNLPSolver { - LCNLP lcnlp_; +class LinearConstraintSQP { + LinearConstraintNLP lcnlp_; static const double errorTol = 1e-5; public: - LCNLPSolver(const LCNLP& lcnlp) : + LinearConstraintSQP(const LinearConstraintNLP& lcnlp) : lcnlp_(lcnlp) { } @@ -78,7 +78,7 @@ public: bool isStationary(const VectorValues& delta) const; /// Check if c_E(x) == 0 - bool isPrimalFeasible(const LCNLPState& state) const; + bool isPrimalFeasible(const LinearConstraintNLPState& state) const; /** * Dual variables of inequality constraints need to be >=0 @@ -96,16 +96,17 @@ 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 LCNLPState& state) const; + bool isComplementary(const LinearConstraintNLPState& state) const; /// Check convergence - bool checkConvergence(const LCNLPState& state, const VectorValues& delta) const; + bool checkConvergence(const LinearConstraintNLPState& state, const VectorValues& delta) const; /** * Single iteration of SQP */ - LCNLPState iterate(const LCNLPState& state, bool useWarmStart = true, bool debug = false) const; + LinearConstraintNLPState iterate(const LinearConstraintNLPState& state, bool useWarmStart = true, bool debug = false) const; + /// Intialize all dual variables to zeros VectorValues initializeDuals() const; /** diff --git a/gtsam_unstable/nonlinear/LinearEqualityFactor.h b/gtsam_unstable/nonlinear/LinearEqualityFactor.h new file mode 100644 index 000000000..716bc0db2 --- /dev/null +++ b/gtsam_unstable/nonlinear/LinearEqualityFactor.h @@ -0,0 +1,192 @@ +/* ---------------------------------------------------------------------------- + + * 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 LinearEqualityFactor.h + * @brief Linear equality factors at the nonlinear level + * @author Duy-Nguyen Ta + * @date Sep 30, 2013 + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +/** A convenient base class for creating a linear equality constraint with 1 + * variables. To derive from this class, implement evaluateError(). + * Warning: It is the user's responsibility to make sure the Hessian is approximately zero + * + * TODO: Should we check Hessian = 0 automatically to make sure it's linear, like SNOPT? + * TODO: Sometimes the true Hessian is not 0, but an approximate one with a different retract is zero + * and that could also work [Absil07fcm] + */ +template +class LinearEqualityFactor1: public NoiseModelFactor1, + public ConstrainedFactor { + +public: + // typedefs for value types pulled from keys + typedef VALUE X; + +protected: + typedef NoiseModelFactor1 Base; + typedef LinearEqualityFactor1 This; + +public: + /** + * Default Constructor for I/O + */ + LinearEqualityFactor1() { + } + + /** + * Constructor + * @param j key of the variable + * @param constraintDim number of dimensions of the constraint error function + */ + LinearEqualityFactor1(Key key, Key dualKey, size_t constraintDim = 1) : + Base(noiseModel::Constrained::All(constraintDim), key), ConstrainedFactor( + dualKey) { + } + + virtual ~LinearEqualityFactor1() { + } + + /** + * Override this method to finish implementing a binary factor. + * If any of the optional Matrix reference arguments are specified, it should compute + * both the function evaluation and its derivative(s) in X1 (and/or X2). + */ + virtual Vector + evaluateError(const X&, boost::optional H1 = boost::none) const = 0; + +}; +// \class LinearEqualityFactor1 + +/* ************************************************************************* */ +/** A convenient base class for creating a linear equality constraint with 2 + * variables. To derive from this class, implement evaluateError(). + * Warning: It is the user's responsibility to make sure the Hessian is approximately zero + * + * TODO: Should we check Hessian = 0 automatically to make sure it's linear, like SNOPT? + * TODO: Sometimes the true Hessian is not 0, but an approximate one with a different retract is zero + * and that could also work [Absil07fcm] + */ +template +class LinearEqualityFactor2: public NoiseModelFactor2, + public ConstrainedFactor { + +public: + // typedefs for value types pulled from keys + typedef VALUE1 X1; + typedef VALUE2 X2; + +protected: + typedef NoiseModelFactor2 Base; + typedef LinearEqualityFactor2 This; + +public: + /** + * Default Constructor for I/O + */ + LinearEqualityFactor2() { + } + + /** + * Constructor + * @param j1 key of the first variable + * @param j2 key of the second variable + * @param constraintDim number of dimensions of the constraint error function + */ + LinearEqualityFactor2(Key j1, Key j2, Key dualKey, size_t constraintDim = 1) : + Base(noiseModel::Constrained::All(constraintDim), j1, j2), ConstrainedFactor( + dualKey) { + } + + virtual ~LinearEqualityFactor2() { + } + + /** + * Override this method to finish implementing a binary factor. + * If any of the optional Matrix reference arguments are specified, it should compute + * both the function evaluation and its derivative(s) in X1 (and/or X2). + */ + virtual Vector + evaluateError(const X1&, const X2&, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const = 0; + +}; +// \class LinearEqualityFactor2 + +/* ************************************************************************* */ +/** A convenient base class for creating a linear equality constraint with 3 + * variables. To derive from this class, implement evaluateError(). + * Warning: It is the user's responsibility to make sure the Hessian is approximately zero + * + * TODO: Should we check Hessian = 0 automatically to make sure it's linear, like SNOPT? + * TODO: Sometimes the true Hessian is not 0, but an approximate one with a different retract is zero + * and that could also work [Absil07fcm] + */ +template +class LinearEqualityFactor3: public NoiseModelFactor3, + public ConstrainedFactor { + +public: + // typedefs for value types pulled from keys + typedef VALUE1 X1; + typedef VALUE2 X2; + typedef VALUE3 X3; + +protected: + typedef NoiseModelFactor3 Base; + typedef LinearEqualityFactor3 This; + +public: + + /** + * Default Constructor for I/O + */ + LinearEqualityFactor3() { + } + + /** + * Constructor + * @param j1 key of the first variable + * @param j2 key of the second variable + * @param constraintDim number of dimensions of the constraint error function + */ + LinearEqualityFactor3(Key j1, Key j2, Key j3, Key dualKey, + size_t constraintDim = 1) : + Base(noiseModel::Constrained::All(constraintDim), j1, j2, j3), ConstrainedFactor( + dualKey) { + } + + virtual ~LinearEqualityFactor3() { + } + + /** + * Override this method to finish implementing a binary factor. + * If any of the optional Matrix reference arguments are specified, it should compute + * both the function evaluation and its derivative(s) in X1 (and/or X2). + */ + virtual Vector + evaluateError(const X1&, const X2&, const X3&, boost::optional H1 = + boost::none, boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const = 0; + +}; +// \class LinearEqualityFactor3 + +} /* namespace gtsam */ diff --git a/gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h b/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h similarity index 55% rename from gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h rename to gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h index b4dab147b..098aaae2c 100644 --- a/gtsam_unstable/nonlinear/NonlinearEqualityFactorGraph.h +++ b/gtsam_unstable/nonlinear/LinearEqualityFactorGraph.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearEqualityFactorGraph.h + * @file LinearEqualityFactorGraph.h * @author Duy-Nguyen Ta * @author Krunal Chande * @author Luca Carlone @@ -18,26 +18,29 @@ */ #pragma once -#include -#include +#include +#include namespace gtsam { -class NonlinearEqualityFactorGraph: public FactorGraph { +/** + * FactorGraph container for linear equality factors c(x)==0 at the nonlinear level + */ +class LinearEqualityFactorGraph: public FactorGraph { public: /// Default constructor - NonlinearEqualityFactorGraph() { + LinearEqualityFactorGraph() { } - /// Linearize to a LinearEqualityFactorGraph - LinearEqualityFactorGraph::shared_ptr linearize( + /// Linearize to a EqualityFactorGraph + EqualityFactorGraph::shared_ptr linearize( const Values& linearizationPoint) const { - LinearEqualityFactorGraph::shared_ptr linearGraph( - new LinearEqualityFactorGraph()); + EqualityFactorGraph::shared_ptr linearGraph( + new EqualityFactorGraph()); BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){ JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( factor->linearize(linearizationPoint)); - NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); + ConstrainedFactor::shared_ptr constraint = boost::dynamic_pointer_cast(factor); linearGraph->add(LinearEquality(*jacobian, constraint->dualKey())); } return linearGraph; @@ -58,18 +61,6 @@ public: return true; } -// /** -// * Additional cost for -lambda*ConstraintHessian for SQP -// */ -// GaussianFactorGraph::shared_ptr multipliedHessians(const Values& values, const VectorValues& duals) const { -// GaussianFactorGraph::shared_ptr constrainedHessians(new GaussianFactorGraph()); -// BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) { -// NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); -// constrainedHessians->push_back(constraint->multipliedHessian(values, duals)); -// } -// return constrainedHessians; -// } - }; } diff --git a/gtsam_unstable/nonlinear/NonlinearInequality.h b/gtsam_unstable/nonlinear/LinearInequalityFactor.h similarity index 66% rename from gtsam_unstable/nonlinear/NonlinearInequality.h rename to gtsam_unstable/nonlinear/LinearInequalityFactor.h index f7021416b..f1387bb42 100644 --- a/gtsam_unstable/nonlinear/NonlinearInequality.h +++ b/gtsam_unstable/nonlinear/LinearInequalityFactor.h @@ -9,7 +9,7 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearInequality.h + * @file LinearInequalityFactor.h * @brief * @author Duy-Nguyen Ta * @date Sep 30, 2013 @@ -17,15 +17,18 @@ #pragma once -#include +#include namespace gtsam { /* ************************************************************************* */ -/** A convenient base class for creating a nonlinear equality constraint with 1 - * variables. To derive from this class, implement evaluateError(). */ +/** + * A convenient base class for creating a linear inequality constraint, e.g., Ax <= 0, + * at the nonlinear level with 1 variable. + * To derive from this class, implement computeError(). + */ template -class NonlinearInequality1: public NonlinearConstraint1 { +class LinearInequalityFactor1: public LinearEqualityFactor1 { public: @@ -33,19 +36,15 @@ public: typedef VALUE X; protected: - - typedef NonlinearConstraint1 Base; - typedef NonlinearInequality1 This; - -private: - static const int X1Dim = traits::dimension; + typedef LinearEqualityFactor1 Base; + typedef LinearInequalityFactor1 This; public: /** * Default Constructor for I/O */ - NonlinearInequality1() { + LinearInequalityFactor1() { } /** @@ -53,11 +52,11 @@ public: * @param j key of the variable * @param constraintDim number of dimensions of the constraint error function */ - NonlinearInequality1(Key key, Key dualKey) : + LinearInequalityFactor1(Key key, Key dualKey) : Base(key, dualKey, 1) { } - virtual ~NonlinearInequality1() { + virtual ~LinearInequalityFactor1() { } /** @@ -73,20 +72,18 @@ public: evaluateError(const X& x, boost::optional H1 = boost::none) const { return (Vector(1) << computeError(x, H1)).finished(); } -// -// virtual GaussianFactor::shared_ptr multipliedHessian(const Values& x, -// const VectorValues& duals) const { -// return Base::multipliedHessian(x, duals); -// } }; -// \class NonlinearConstraint1 +// \class LinearEqualityFactor1 /* ************************************************************************* */ -/** A convenient base class for creating your own NonlinearConstraint with 2 - * variables. To derive from this class, implement evaluateError(). */ +/** + * A convenient base class for creating a linear inequality constraint, e.g., Ax <= 0, + * at the nonlinear level with 1 variable. + * To derive from this class, implement computeError(). + */ template -class NonlinearInequality2: public NonlinearConstraint2 { +class LinearInequalityFactor2: public LinearEqualityFactor2 { public: @@ -95,20 +92,15 @@ public: typedef VALUE2 X2; protected: - - typedef NonlinearConstraint2 Base; - typedef NonlinearInequality2 This; - -private: - static const int X1Dim = traits::dimension; - static const int X2Dim = traits::dimension; + typedef LinearEqualityFactor2 Base; + typedef LinearInequalityFactor2 This; public: /** * Default Constructor for I/O */ - NonlinearInequality2() { + LinearInequalityFactor2() { } /** @@ -117,11 +109,11 @@ public: * @param j2 key of the second variable * @param constraintDim number of dimensions of the constraint error function */ - NonlinearInequality2(Key j1, Key j2, Key dualKey) : + LinearInequalityFactor2(Key j1, Key j2, Key dualKey) : Base(j1, j2, 1) { } - virtual ~NonlinearInequality2() { + virtual ~LinearInequalityFactor2() { } /** @@ -140,7 +132,7 @@ public: return (Vector(1) << computeError(x1, x2, H1, H2)).finished(); } }; -// \class NonlinearConstraint2 +// \class LinearEqualityFactor2 } /* namespace gtsam */ diff --git a/gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h b/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h similarity index 51% rename from gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h rename to gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h index 45f23d310..5195ab86e 100644 --- a/gtsam_unstable/nonlinear/NonlinearInequalityFactorGraph.h +++ b/gtsam_unstable/nonlinear/LinearInequalityFactorGraph.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearInequalityFactorGraph.h + * @file LinearInequalityFactorGraph.h * @author Duy-Nguyen Ta * @author Krunal Chande * @author Luca Carlone @@ -19,26 +19,29 @@ #pragma once #include -#include -#include +#include +#include namespace gtsam { -class NonlinearInequalityFactorGraph : public FactorGraph { + +/** + * FactorGraph container for linear inequality factors c(x)<=0 at the nonlinear level + */ +class LinearInequalityFactorGraph: public FactorGraph { public: /// Default constructor - NonlinearInequalityFactorGraph() { + LinearInequalityFactorGraph() { } - /// Linearize to a LinearInequalityFactorGraph - LinearInequalityFactorGraph::shared_ptr linearize( - const Values& linearizationPoint) const { - LinearInequalityFactorGraph::shared_ptr linearGraph( - new LinearInequalityFactorGraph()); - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){ + /// Linearize to a InequalityFactorGraph + InequalityFactorGraph::shared_ptr linearize(const Values& linearizationPoint) const { + InequalityFactorGraph::shared_ptr linearGraph(new InequalityFactorGraph()); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) { JacobianFactor::shared_ptr jacobian = boost::dynamic_pointer_cast( factor->linearize(linearizationPoint)); - NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); + ConstrainedFactor::shared_ptr constraint + = boost::dynamic_pointer_cast(factor); linearGraph->add(LinearInequality(*jacobian, constraint->dualKey())); } return linearGraph; @@ -47,26 +50,32 @@ public: /** * Return true if the all errors are <= 0.0 */ - bool checkFeasibilityAndComplimentary(const Values& values, const VectorValues& duals, double tol) const { - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this){ - NoiseModelFactor::shared_ptr noiseModelFactor = boost::dynamic_pointer_cast( - factor); + bool checkFeasibilityAndComplimentary(const Values& values, + const VectorValues& dualValues, double tol) const { + + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, *this) { + NoiseModelFactor::shared_ptr noiseModelFactor + = boost::dynamic_pointer_cast(factor); Vector error = noiseModelFactor->unwhitenedError(values); // Primal feasibility condition: all constraints need to be <= 0.0 - if (error[0] > tol) { + if (error[0] > tol) return false; - } // Complimentary condition: errors of active constraints need to be 0.0 - NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast( - factor); + ConstrainedFactor::shared_ptr constraint + = boost::dynamic_pointer_cast(factor); Key dualKey = constraint->dualKey(); - if (!duals.exists(dualKey)) continue; // if dualKey doesn't exist, it is an inactive constraint! - if (fabs(error[0]) > tol) // for active constraint, the error should be 0.0 - return false; + // if dualKey doesn't exist in dualValues, it must be an inactive constraint! + if (!dualValues.exists(dualKey)) + continue; + + // for active constraint, the error should be 0.0 + if (fabs(error[0]) > tol) + return false; } + return true; } }; diff --git a/gtsam_unstable/nonlinear/NonlinearConstraint.h b/gtsam_unstable/nonlinear/NonlinearConstraint.h index 4043eb9b1..bf4b5fbb9 100644 --- a/gtsam_unstable/nonlinear/NonlinearConstraint.h +++ b/gtsam_unstable/nonlinear/NonlinearConstraint.h @@ -23,47 +23,15 @@ #include #include #include +#include namespace gtsam { -/** - * A base class for all NonlinearConstraint factors, - * containing additional information for the constraint, - * e.g., a unique key for the dual variable associated with it, - * and special treatments for nonlinear constraint linearization in SQP. - * - * Derived classes of NonlinearConstraint should also inherit from - * NoiseModelFactorX to reuse the normal linearization procedure in NonlinearFactor - */ -class NonlinearConstraint { - -protected: - Key dualKey_; //!< Unique key for the dual variable associated with this constraint - -public: - typedef boost::shared_ptr shared_ptr; -public: - /// Construct with dual key - NonlinearConstraint(Key dualKey) : dualKey_(dualKey) {} - - - /// Return the dual key - Key dualKey() const { return dualKey_; } - - /** - * Special linearization for nonlinear constraint in SQP - * Compute the HessianFactor of the (-dual * constraintHessian) - * for the qp subproblem's objective function - */ - virtual GaussianFactor::shared_ptr multipliedHessian(const Values& x, - const VectorValues& duals) const = 0; -}; - /* ************************************************************************* */ /** A convenient base class for creating a nonlinear equality constraint with 1 * variables. To derive from this class, implement evaluateError(). */ template -class NonlinearConstraint1: public NoiseModelFactor1, public NonlinearConstraint { +class NonlinearConstraint1: public NoiseModelFactor1, public ConstrainedFactor { public: @@ -92,7 +60,7 @@ public: * @param constraintDim number of dimensions of the constraint error function */ NonlinearConstraint1(Key key, Key dualKey, size_t constraintDim = 1) : - Base(noiseModel::Constrained::All(constraintDim), key), NonlinearConstraint(dualKey) { + Base(noiseModel::Constrained::All(constraintDim), key), ConstrainedFactor(dualKey) { } virtual ~NonlinearConstraint1() { @@ -179,10 +147,10 @@ private: // \class NonlinearConstraint1 /* ************************************************************************* */ -/** A convenient base class for creating your own NonlinearConstraint with 2 +/** A convenient base class for creating your own ConstrainedFactor with 2 * variables. To derive from this class, implement evaluateError(). */ template -class NonlinearConstraint2: public NoiseModelFactor2, public NonlinearConstraint { +class NonlinearConstraint2: public NoiseModelFactor2, public ConstrainedFactor { public: @@ -214,7 +182,7 @@ public: * @param constraintDim number of dimensions of the constraint error function */ NonlinearConstraint2(Key j1, Key j2, Key dualKey, size_t constraintDim = 1) : - Base(noiseModel::Constrained::All(constraintDim), j1, j2), NonlinearConstraint(dualKey) { + Base(noiseModel::Constrained::All(constraintDim), j1, j2), ConstrainedFactor(dualKey) { } virtual ~NonlinearConstraint2() { @@ -332,10 +300,10 @@ private: // \class NonlinearConstraint2 /* ************************************************************************* */ -/** A convenient base class for creating your own NonlinearConstraint with 3 +/** A convenient base class for creating your own ConstrainedFactor with 3 * variables. To derive from this class, implement evaluateError(). */ template -class NonlinearConstraint3: public NoiseModelFactor3, public NonlinearConstraint { +class NonlinearConstraint3: public NoiseModelFactor3, public ConstrainedFactor { public: @@ -369,7 +337,7 @@ public: * @param constraintDim number of dimensions of the constraint error function */ NonlinearConstraint3(Key j1, Key j2, Key j3, Key dualKey, size_t constraintDim = 1) : - Base(noiseModel::Constrained::All(constraintDim), j1, j2, j3), NonlinearConstraint(dualKey) { + Base(noiseModel::Constrained::All(constraintDim), j1, j2, j3), ConstrainedFactor(dualKey) { } virtual ~NonlinearConstraint3() { diff --git a/gtsam_unstable/nonlinear/tests/testLCNLPSolver.cpp b/gtsam_unstable/nonlinear/tests/testLCNLPSolver2.cpp similarity index 65% rename from gtsam_unstable/nonlinear/tests/testLCNLPSolver.cpp rename to gtsam_unstable/nonlinear/tests/testLCNLPSolver2.cpp index 9c1d8510b..88de347e3 100644 --- a/gtsam_unstable/nonlinear/tests/testLCNLPSolver.cpp +++ b/gtsam_unstable/nonlinear/tests/testLCNLPSolver2.cpp @@ -36,178 +36,6 @@ using namespace gtsam::symbol_shorthand; using namespace gtsam; const double tol = 1e-10; -//****************************************************************************** -// x + y - 1 = 0 -class ConstraintProblem1 : public NonlinearConstraint2 { - typedef NonlinearConstraint2 Base; - -public: - ConstraintProblem1(Key xK, Key yK, Key dualKey) : Base(xK, yK, dualKey, 1) {} - - // x + y - 1 - Vector evaluateError(const double& x, const double& y, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - if (H1) *H1 = eye(1); - if (H2) *H2 = eye(1); - return (Vector(1) << x + y - 1.0).finished(); - } -}; - -TEST_DISABLED(testlcnlpSolver, QPProblem) { - const Key dualKey = 0; - - // Simple quadratic cost: x1^2 + x2^2 - // 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 here we have G11 = 2, G12 = 0, G22 = 2, g1 = 0, g2 = 0, f = 0 - HessianFactor hf(X(1), Y(1), 2.0 * ones(1,1), zero(1), zero(1), - 2*ones(1,1), zero(1) , 0); - - LinearEqualityFactorGraph equalities; - LinearEquality linearConstraint(X(1), ones(1), Y(1), ones(1), 1*ones(1), dualKey); // x + y - 1 = 0 - equalities.push_back(linearConstraint); - - // Compare against QP - QP qp; - qp.cost.add(hf); - qp.equalities = equalities; - - // instantiate QPsolver - QPSolver qpSolver(qp); - // create initial values for optimization - VectorValues initialVectorValues; - initialVectorValues.insert(X(1), zero(1)); - initialVectorValues.insert(Y(1), ones(1)); - VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first; - - //Instantiate LCNLP - LCNLP lcnlp; - Values linPoint; - linPoint.insert(X(1), zero(1)); - linPoint.insert(Y(1), zero(1)); - lcnlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor - lcnlp.linearEqualities.add(ConstraintProblem1(X(1), Y(1), dualKey)); - - Values initialValues; - initialValues.insert(X(1), 0.0); - initialValues.insert(Y(1), 0.0); - - // Instantiate LCNLPSolver - LCNLPSolver lcnlpSolver(lcnlp); - Values actualValues = lcnlpSolver.optimize(initialValues).first; - - DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualValues.at(X(1)), 1e-100); - DOUBLES_EQUAL(expectedSolution.at(Y(1))[0], actualValues.at(Y(1)), 1e-100); - -} - -//****************************************************************************** -class LineConstraintX : public NonlinearConstraint1 { - typedef NonlinearConstraint1 Base; -public: - LineConstraintX(Key key, Key dualKey) : Base(key, dualKey, 1) { - } - - double computeError(const Pose3& pose) const { - return pose.x(); - } - - Vector evaluateError(const Pose3& pose, boost::optional H = boost::none) const { - if (H) - *H = (Matrix(1,6) << zeros(1,3), pose.rotation().matrix().row(0)).finished(); - return (Vector(1) << pose.x()).finished(); - } -}; - -TEST_DISABLED(testlcnlpSolver, poseOnALine) { - const Key dualKey = 0; - - - //Instantiate LCNLP - LCNLP lcnlp; - lcnlp.cost.add(PriorFactor(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6))); - LineConstraintX constraint(X(1), dualKey); - lcnlp.linearEqualities.add(constraint); - - Values initialValues; - initialValues.insert(X(1), Pose3(Rot3::ypr(0.3, 0.2, 0.3), Point3(1,0,0))); - - Values expectedSolution; - expectedSolution.insert(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3())); - - // Instantiate LCNLPSolver - LCNLPSolver lcnlpSolver(lcnlp); - Values actualSolution = lcnlpSolver.optimize(initialValues).first; - - CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); - Pose3 pose(Rot3::ypr(0.1, 0.2, 0.3), Point3()); - Matrix hessian = numericalHessian(boost::bind(&LineConstraintX::computeError, constraint, _1), pose, 1e-2); -} - -//****************************************************************************** -/// x + y - 1 <= 0 -class InequalityProblem1 : public NonlinearInequality2 { - typedef NonlinearInequality2 Base; -public: - InequalityProblem1(Key xK, Key yK, Key dualKey) : Base(xK, yK, dualKey) {} - - double computeError(const double& x, const double& y, - boost::optional H1 = boost::none, boost::optional H2 = - boost::none) const { - if (H1) *H1 = eye(1); - if (H2) *H2 = eye(1); - return x + y - 1.0; - } -}; - -TEST_DISABLED(testlcnlpSolver, inequalityConstraint) { - const Key dualKey = 0; - - // Simple quadratic cost: x^2 + y^2 - // 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 here we have G11 = 2, G12 = 0, G22 = 2, g1 = 0, g2 = 0, f = 0 - HessianFactor hf(X(1), Y(1), 2.0 * ones(1,1), zero(1), zero(1), - 2*ones(1,1), zero(1) , 0); - - LinearInequalityFactorGraph inequalities; - LinearInequality linearConstraint(X(1), ones(1), Y(1), ones(1), 1.0, dualKey); // x + y - 1 <= 0 - inequalities.push_back(linearConstraint); - - // Compare against QP - QP qp; - qp.cost.add(hf); - qp.inequalities = inequalities; - - // instantiate QPsolver - QPSolver qpSolver(qp); - // create initial values for optimization - VectorValues initialVectorValues; - initialVectorValues.insert(X(1), zero(1)); - initialVectorValues.insert(Y(1), zero(1)); - VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first; - - //Instantiate LCNLP - LCNLP lcnlp; - Values linPoint; - linPoint.insert(X(1), zero(1)); - linPoint.insert(Y(1), zero(1)); - lcnlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor - lcnlp.linearInequalities.add(InequalityProblem1(X(1), Y(1), dualKey)); - - Values initialValues; - initialValues.insert(X(1), 1.0); - initialValues.insert(Y(1), -10.0); - - // Instantiate LCNLPSolver - LCNLPSolver lcnlpSolver(lcnlp); - Values actualValues = lcnlpSolver.optimize(initialValues).first; - - DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualValues.at(X(1)), 1e-10); - DOUBLES_EQUAL(expectedSolution.at(Y(1))[0], actualValues.at(Y(1)), 1e-10); -} - //****************************************************************************** const size_t X_AXIS = 0; const size_t Y_AXIS = 1; @@ -253,7 +81,8 @@ public: } }; -TEST_DISABLED(testlcnlpSolver, poseWithABoundary) { +//****************************************************************************** +TEST(testlcnlpSolver, poseWithABoundary) { const Key dualKey = 0; //Instantiate LCNLP @@ -275,7 +104,8 @@ TEST_DISABLED(testlcnlpSolver, poseWithABoundary) { CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); } -TEST_DISABLED(testlcnlpSolver, poseWithNoConstraints) { +//****************************************************************************** +TEST(testlcnlpSolver, poseWithNoConstraints) { //Instantiate LCNLP LCNLP lcnlp; @@ -294,7 +124,8 @@ TEST_DISABLED(testlcnlpSolver, poseWithNoConstraints) { CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); } -TEST_DISABLED(testlcnlpSolver, poseWithinA2DBox) { +//****************************************************************************** +TEST(testlcnlpSolver, poseWithinA2DBox) { const Key dualKey = 0; //Instantiate LCNLP @@ -318,7 +149,8 @@ TEST_DISABLED(testlcnlpSolver, poseWithinA2DBox) { CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); } -TEST_DISABLED(testlcnlpSolver, posesInA2DBox) { +//****************************************************************************** +TEST(testlcnlpSolver, posesInA2DBox) { const double xLowerBound = -3.0, xUpperBound = 5.0, yLowerBound = -1.0, @@ -385,7 +217,8 @@ TEST_DISABLED(testlcnlpSolver, posesInA2DBox) { CHECK(assert_equal(expectedSolution, actualSolution, 1e-5)); } -TEST_DISABLED(testlcnlpSolver, iterativePoseinBox) { +//****************************************************************************** +TEST(testlcnlpSolver, iterativePoseinBox) { const double xLowerBound = -1.0, xUpperBound = 1.0, yLowerBound = -1.0, @@ -471,12 +304,13 @@ TEST_DISABLED(testlcnlpSolver, iterativePoseinBox) { } +//****************************************************************************** double testHessian(const Pose3& X) { return X.transform_from(Point3(0,0,0)).x(); } Pose3 pose(Rot3::ypr(0.1, 0.4, 0.7), Point3(1, 9.5, 7.3)); -TEST_DISABLED(testlcnlpSolver, testingHessian) { +TEST(testlcnlpSolver, testingHessian) { Matrix H = numericalHessian(testHessian, pose, 1e-5); } @@ -484,7 +318,7 @@ double h3(const Vector6& v) { return pose.retract(v).translation().x(); } -TEST_DISABLED(testlcnlpSolver, NumericalHessian3) { +TEST(testlcnlpSolver, NumericalHessian3) { Matrix6 expected; expected.setZero(); Vector6 z; diff --git a/gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp b/gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp new file mode 100644 index 000000000..3e96b1ce4 --- /dev/null +++ b/gtsam_unstable/nonlinear/tests/testLinearConstraintSQP.cpp @@ -0,0 +1,214 @@ +/* ---------------------------------------------------------------------------- + + * 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 testLinearConstraintSQP.cpp + * @brief Unit tests for testLinearConstraintSQP + * @author Duy-Nguyen Ta + * @author Krunal Chande + * @author Luca Carlone + * @date Dec 15, 2014 + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam::symbol_shorthand; +using namespace gtsam; +const double tol = 1e-10; + +//****************************************************************************** +// x + y - 1 = 0 +class ConstraintProblem1 : public LinearEqualityFactor2 { + typedef LinearEqualityFactor2 Base; + +public: + ConstraintProblem1(Key xK, Key yK, Key dualKey) : Base(xK, yK, dualKey, 1) {} + + // x + y - 1 + Vector evaluateError(const double& x, const double& y, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + if (H1) *H1 = eye(1); + if (H2) *H2 = eye(1); + return (Vector(1) << x + y - 1.0).finished(); + } +}; + +TEST(testlcnlpSolver, QPProblem) { + const Key dualKey = 0; + + // Simple quadratic cost: x1^2 + x2^2 + // 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 here we have G11 = 2, G12 = 0, G22 = 2, g1 = 0, g2 = 0, f = 0 + HessianFactor hf(X(1), Y(1), 2.0 * ones(1,1), zero(1), zero(1), + 2*ones(1,1), zero(1) , 0); + + EqualityFactorGraph equalities; + LinearEquality linearConstraint(X(1), ones(1), Y(1), ones(1), 1*ones(1), dualKey); // x + y - 1 = 0 + equalities.push_back(linearConstraint); + + // Compare against QP + QP qp; + qp.cost.add(hf); + qp.equalities = equalities; + + // instantiate QPsolver + QPSolver qpSolver(qp); + // create initial values for optimization + VectorValues initialVectorValues; + initialVectorValues.insert(X(1), zero(1)); + initialVectorValues.insert(Y(1), ones(1)); + VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first; + + //Instantiate LinearConstraintNLP + LinearConstraintNLP lcnlp; + Values linPoint; + linPoint.insert(X(1), zero(1)); + linPoint.insert(Y(1), zero(1)); + lcnlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor + lcnlp.linearEqualities.add(ConstraintProblem1(X(1), Y(1), dualKey)); + + Values initialValues; + initialValues.insert(X(1), 0.0); + initialValues.insert(Y(1), 0.0); + + // Instantiate LinearConstraintSQP + LinearConstraintSQP lcnlpSolver(lcnlp); + Values actualValues = lcnlpSolver.optimize(initialValues).first; + + DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualValues.at(X(1)), 1e-100); + DOUBLES_EQUAL(expectedSolution.at(Y(1))[0], actualValues.at(Y(1)), 1e-100); + +} + +//****************************************************************************** +/** + * A simple linear constraint on Pose3's x coordinate enforcing x==0 + */ +class LineConstraintX : public LinearEqualityFactor1 { + typedef LinearEqualityFactor1 Base; +public: + LineConstraintX(Key key, Key dualKey) : Base(key, dualKey, 1) { + } + + Vector evaluateError(const Pose3& pose, boost::optional H = boost::none) const { + if (H) + *H = (Matrix(1,6) << zeros(1,3), pose.rotation().matrix().row(0)).finished(); + return (Vector(1) << pose.x()).finished(); + } +}; + +TEST(testlcnlpSolver, poseOnALine) { + const Key dualKey = 0; + + + //Instantiate LinearConstraintNLP + LinearConstraintNLP lcnlp; + lcnlp.cost.add(PriorFactor(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6))); + LineConstraintX constraint(X(1), dualKey); + lcnlp.linearEqualities.add(constraint); + + Values initialValues; + initialValues.insert(X(1), Pose3(Rot3::ypr(0.3, 0.2, 0.3), Point3(1,0,0))); + + Values expectedSolution; + expectedSolution.insert(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3())); + + // Instantiate LinearConstraintSQP + LinearConstraintSQP lcnlpSolver(lcnlp); + Values actualSolution = lcnlpSolver.optimize(initialValues).first; + + CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); +} + +//****************************************************************************** +/// x + y - 1 <= 0 +class InequalityProblem1 : public LinearInequalityFactor2 { + typedef LinearInequalityFactor2 Base; +public: + InequalityProblem1(Key xK, Key yK, Key dualKey) : Base(xK, yK, dualKey) {} + + double computeError(const double& x, const double& y, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + if (H1) *H1 = eye(1); + if (H2) *H2 = eye(1); + return x + y - 1.0; + } +}; + +TEST(testlcnlpSolver, inequalityConstraint) { + const Key dualKey = 0; + + // Simple quadratic cost: x^2 + y^2 + // 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 here we have G11 = 2, G12 = 0, G22 = 2, g1 = 0, g2 = 0, f = 0 + HessianFactor hf(X(1), Y(1), 2.0 * ones(1,1), zero(1), zero(1), + 2*ones(1,1), zero(1) , 0); + + InequalityFactorGraph inequalities; + LinearInequality linearConstraint(X(1), ones(1), Y(1), ones(1), 1.0, dualKey); // x + y - 1 <= 0 + inequalities.push_back(linearConstraint); + + // Compare against QP + QP qp; + qp.cost.add(hf); + qp.inequalities = inequalities; + + // instantiate QPsolver + QPSolver qpSolver(qp); + // create initial values for optimization + VectorValues initialVectorValues; + initialVectorValues.insert(X(1), zero(1)); + initialVectorValues.insert(Y(1), zero(1)); + VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first; + + //Instantiate LinearConstraintNLP + LinearConstraintNLP lcnlp; + Values linPoint; + linPoint.insert(X(1), zero(1)); + linPoint.insert(Y(1), zero(1)); + lcnlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor + lcnlp.linearInequalities.add(InequalityProblem1(X(1), Y(1), dualKey)); + + Values initialValues; + initialValues.insert(X(1), 1.0); + initialValues.insert(Y(1), -10.0); + + // Instantiate LinearConstraintSQP + LinearConstraintSQP lcnlpSolver(lcnlp); + Values actualValues = lcnlpSolver.optimize(initialValues).first; + + DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualValues.at(X(1)), 1e-10); + DOUBLES_EQUAL(expectedSolution.at(Y(1))[0], actualValues.at(Y(1)), 1e-10); +} + +//****************************************************************************** +int main() { + cout<<"here: "<