From fd461a1c15552eec52273b9baeeb78078db8770a Mon Sep 17 00:00:00 2001 From: krunalchande Date: Mon, 22 Dec 2014 14:41:22 -0500 Subject: [PATCH] [unfinished] prototyping inequality SQP with Luca. --- gtsam_unstable/linear/LinearInequality.h | 13 +- .../nonlinear/NonlinearConstraint.h | 3 +- .../nonlinear/NonlinearInequality.h | 93 ++++++++++++++ .../nonlinear/tests/testSQPSimple.cpp | 121 +++++++++++++++++- 4 files changed, 225 insertions(+), 5 deletions(-) create mode 100644 gtsam_unstable/nonlinear/NonlinearInequality.h diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h index 7c62c3d54..6aa03eaef 100644 --- a/gtsam_unstable/linear/LinearInequality.h +++ b/gtsam_unstable/linear/LinearInequality.h @@ -44,12 +44,23 @@ 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) : Base(jf), dualKey_(dualKey), active_(true) { + if (!jf.isConstrained()) { + throw std::runtime_error("Cannot convert an unconstrained JacobianFactor to LinearEquality"); + } + + 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_( diff --git a/gtsam_unstable/nonlinear/NonlinearConstraint.h b/gtsam_unstable/nonlinear/NonlinearConstraint.h index ce01f89e8..f24517983 100644 --- a/gtsam_unstable/nonlinear/NonlinearConstraint.h +++ b/gtsam_unstable/nonlinear/NonlinearConstraint.h @@ -13,11 +13,12 @@ #include #include #include -//#include "DualKeyGenerator.h" namespace gtsam { class NonlinearConstraint { + +protected: Key dualKey_; public: diff --git a/gtsam_unstable/nonlinear/NonlinearInequality.h b/gtsam_unstable/nonlinear/NonlinearInequality.h new file mode 100644 index 000000000..b9a9e418a --- /dev/null +++ b/gtsam_unstable/nonlinear/NonlinearInequality.h @@ -0,0 +1,93 @@ +/** + * @file NonlinearConstraint.h + * @brief + * @author Duy-Nguyen Ta + * @date Sep 30, 2013 + */ + +#pragma once + +#include + +namespace gtsam { + +class NonlinearInequality : public NonlinearConstraint { + bool active_; + + typedef NonlinearConstraint Base; + +public: + typedef boost::shared_ptr shared_ptr; + +public: + /// Construct with dual key + NonlinearInequality(Key dualKey) : Base(dualKey), active_(true) {} + + /** + * 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 NonlinearInequality1: public NonlinearConstraint1, public NonlinearInequality { + +public: + + // typedefs for value types pulled from keys + typedef VALUE X; + +protected: + + typedef NonlinearConstraint1 Base; + typedef NonlinearInequality1 This; + +private: + static const int X1Dim = traits::dimension::value; + +public: + + /** + * Default Constructor for I/O + */ + NonlinearInequality1() { + } + + /** + * Constructor + * @param j key of the variable + * @param constraintDim number of dimensions of the constraint error function + */ + NonlinearInequality1(Key key, Key dualKey, size_t constraintDim = 1) : + Base(noiseModel::Constrained::All(constraintDim), key), NonlinearConstraint(dualKey) { + } + + virtual ~NonlinearInequality1() { + } + + /** + * 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 { + return (Vector(1) << computeError(X, H1)); + } + + /** + * 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 double + computeError(const X&, boost::optional H1 = boost::none) const = 0; + +}; +// \class NonlinearConstraint1 + +} /* namespace gtsam */ diff --git a/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp b/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp index aa8de00e8..08bcfb614 100644 --- a/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp +++ b/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp @@ -80,11 +80,76 @@ public: } }; +class NonlinearInequalityFactorGraph : public FactorGraph { + +public: + /// default constructor + NonlinearInequalityFactorGraph() { + } + + /// linearize to a LinearEqualityFactorGraph + LinearInequalityFactorGraph::shared_ptr linearize( + const Values& linearizationPoint) const { + LinearInequalityFactorGraph::shared_ptr linearGraph( + new LinearInequalityFactorGraph()); + 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); + linearGraph->add(LinearInequality(*jacobian, constraint->dualKey())); + } + return linearGraph; + } + + /** + * Return true if the error is <= 0.0 + */ + bool checkFeasibility(const Values& values, 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); + // TODO: Do we need to check if it's active or not? + if (error[0] > tol) { + return false; + } + } + return true; + } + + /** + * Return true if the max absolute error all factors is less than a tolerance + */ + bool checkDualFeasibility(const VectorValues& duals, double tol) const { + BOOST_FOREACH(const Vector& dual, duals){ + if (dual[0] < 0.0) { + return false; + } + } + return true; + } + + /** + * Return true if the max absolute error all factors is less than a tolerance + */ + bool checkComplimentaryCondition(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); + Vector error = noiseModelFactor->unwhitenedError(values); + if (error[0] > 0.0) { + return false; + } + } + return true; + } +}; struct NLP { NonlinearFactorGraph cost; NonlinearEqualityFactorGraph linearEqualities; NonlinearEqualityFactorGraph nonlinearEqualities; + NonlinearInequalityFactorGraph linearInequalities; }; struct SQPSimpleState { @@ -117,14 +182,16 @@ public: /// Check if \nabla f(x) - \lambda * \nabla c(x) == 0 bool isDualFeasible(const VectorValues& delta) const { - return delta.vector().lpNorm() < errorTol; + return delta.vector().lpNorm() < errorTol + && nlp_.linearInequalities.checkDualFeasibility(errorTol); // return false; } /// Check if c(x) == 0 bool isPrimalFeasible(const SQPSimpleState& state) const { return nlp_.linearEqualities.checkFeasibility(state.values, errorTol) - && nlp_.nonlinearEqualities.checkFeasibility(state.values, errorTol); + && nlp_.nonlinearEqualities.checkFeasibility(state.values, errorTol) + && nlp_.linearInequalities.checkFeasibility(state.values, errorTol); } /// Check convergence @@ -147,6 +214,8 @@ public: 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:"); @@ -206,6 +275,7 @@ public: #include #include #include +#include using namespace std; using namespace gtsam::symbol_shorthand; @@ -353,7 +423,8 @@ public: return (Vector(1) << pose.x()).finished(); } }; -TEST_UNSAFE(testSQPSimple, poseOnALine) { + +TEST(testSQPSimple, poseOnALine) { const Key dualKey = 0; @@ -380,6 +451,50 @@ TEST_UNSAFE(testSQPSimple, poseOnALine) { cout << "hessian: \n" << hessian << endl; } + +//****************************************************************************** +/** + * Inequality boundary constraint + * x <= bound + */ +class UpperBoundX : public NonlinearInequality1 { + typedef NonlinearInequality1 Base; + double bound_; +public: + UpperBoundX(Key key, double bound, Key dualKey) : Base(key, dualKey, 1), bound_(bound) { + } + + double computeError(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 pose.x() - bound_; + } +}; + +TEST(testSQPSimple, poseOnALine) { + const Key dualKey = 0; + + + //Instantiate NLP + NLP nlp; + nlp.cost.add(PriorFactor(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(-1, 0, 0)), noiseModel::Unit::Create(6))); + UpperBoundX constraint(X(1), 0, dualKey); + nlp.nonlinearInequalities.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 SQP + SQPSimple sqpSimple(nlp); + Values actualSolution = sqpSimple.optimize(initialValues).first; + + CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); + actualSolution.print("actualSolution: "); +} + //****************************************************************************** int main() { TestResult tr;