From 29e6e67de774740fb9240afe6a7dfadc2b96fcd8 Mon Sep 17 00:00:00 2001 From: krunalchande Date: Fri, 19 Dec 2014 18:07:11 -0500 Subject: [PATCH] added debug info, fixed unit test, added nonlinear constraint (circle) test. Doesn't work because of negative definite hessian obtained from multiplying the dual with the constraint hessian. --- .../nonlinear/tests/testSQPSimple.cpp | 220 ++++++++++++------ 1 file changed, 154 insertions(+), 66 deletions(-) diff --git a/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp b/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp index 923d7be4c..16bc17b03 100644 --- a/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp +++ b/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp @@ -91,13 +91,14 @@ struct SQPSimpleState { Values values; VectorValues duals; bool converged; + size_t iterations; /// Default constructor - SQPSimpleState() : values(), duals(), converged(false) {} + SQPSimpleState() : values(), duals(), converged(false), iterations(0) {} /// Constructor with an initialValues SQPSimpleState(const Values& initialValues) : - values(initialValues), duals(VectorValues()), converged(false) { + values(initialValues), duals(VectorValues()), converged(false), iterations(0) { } }; @@ -117,6 +118,7 @@ public: /// Check if \nabla f(x) - \lambda * \nabla c(x) == 0 bool isDualFeasible(const VectorValues& delta) const { return delta.vector().lpNorm() < errorTol; +// return false; } /// Check if c(x) == 0 @@ -134,6 +136,8 @@ public: * Single iteration of SQP */ SQPSimpleState iterate(const SQPSimpleState& state) const { + static const bool debug = true; + // construct the qp subproblem QP qp; qp.cost = *nlp_.cost.linearize(state.values); @@ -143,30 +147,56 @@ public: qp.equalities.add(*nlp_.linearEqualities.linearize(state.values)); qp.equalities.add(*nlp_.nonlinearEqualities.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); - while (!state.converged) { + state.duals = initializeDuals(); + + while (!state.converged && state.iterations < 100) { state = iterate(state); } - return std::make_pair(initialValues, VectorValues()); + return std::make_pair(state.values, state.duals); } @@ -177,17 +207,38 @@ using namespace std; using namespace gtsam::symbol_shorthand; using namespace gtsam; const double tol = 1e-10; + //****************************************************************************** -TEST(testSQPSimple, Problem2) { +// 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(testSQPSimple, 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), X(2), 2.0 * ones(1,1), zero(1), zero(1), + HessianFactor hf(X(1), Y(1), 2.0 * ones(1,1), zero(1), zero(1), 2*ones(1,1), zero(1) , 0); LinearEqualityFactorGraph equalities; - equalities.push_back(LinearEquality(X(1), ones(1), X(2), ones(1), -1*ones(1), 0)); // x + y - 1 = 0 + 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; @@ -199,77 +250,114 @@ TEST(testSQPSimple, Problem2) { // create initial values for optimization VectorValues initialVectorValues; initialVectorValues.insert(X(1), zero(1)); - initialVectorValues.insert(X(2), zero(1)); + initialVectorValues.insert(Y(1), ones(1)); VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first; - cout<<"expectedSolution.at(X(1))[0]: "<(X(1)), 1e-100); + DOUBLES_EQUAL(expectedSolution.at(Y(1))[0], actualValues.at(Y(1)), 1e-100); + } +//****************************************************************************** +class CircleConstraint : public NonlinearConstraint2 { + typedef NonlinearConstraint2 Base; +public: + CircleConstraint(Key xK, Key yK, Key dualKey) : Base(xK, yK, dualKey, 1) {} + + Vector evaluateError(const double& x, const double& y, + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + if (H1) *H1 = (Matrix(1,1) << 2*(x-1)).finished(); + if (H2) *H2 = (Matrix(1,1) << 2*y).finished(); + return (Vector(1) << (x-1)*(x-1) + y*y - 0.25).finished(); + } + + void evaluateHessians(const double& x, const double& y, + std::vector& G11, std::vector& G12, + std::vector& G22) const { + G11.push_back((Matrix(1,1) << 2).finished()); + G12.push_back((Matrix(1,1) << 0).finished()); + G22.push_back((Matrix(1,1) << 2).finished()); + } + +}; + +TEST_UNSAFE(testSQPSimple, quadraticCostNonlinearConstraint) { + const Key dualKey = 0; + + //Instantiate NLP + NLP nlp; + + // 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 + Values linPoint; + linPoint.insert(X(1), zero(1)); + linPoint.insert(Y(1), zero(1)); + HessianFactor hf(X(1), Y(1), 2.0 * ones(1,1), zero(1), zero(1), + 2*ones(1,1), zero(1) , 0); + nlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor + nlp.nonlinearEqualities.add(CircleConstraint(X(1), Y(1), dualKey)); + + Values initialValues; + initialValues.insert(X(1), 0.0); + initialValues.insert(Y(1), 2.0); + + Values expectedSolution; + expectedSolution.insert(X(1), 0.5); + expectedSolution.insert(Y(1), 0.0); + + // Instantiate SQP + SQPSimple sqpSimple(nlp); + Values actualSolution = sqpSimple.optimize(initialValues).first; + + CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); + actualSolution.print("actualSolution: "); +} + +//****************************************************************************** +// +//TEST_UNSAFE(testSQPSimple, poseOnALine) { +// const Key dualKey = 0; +// +// //Instantiate NLP +// NLP nlp; +// nlp.cost.add(PriorFactor(X(1), Pose3(), noiseModel::Unit::Create(6))); // wrap it using linearcontainerfactor +// nlp.nonlinearEqualities.add(CircleConstraint(X(1), Y(1), dualKey)); +// +// Values initialValues; +// initialValues.insert(X(1), 0.0); +// initialValues.insert(Y(1), 0.0); +// +// Values expectedSolution; +// expectedSolution.insert(X(1), 0.5); +// expectedSolution.insert(Y(1), 0.0); +// +// // 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; return TestRegistry::runAllTests(tr); } //****************************************************************************** - -//TEST(testSQPSimple, Problem1 ) { -// -// // build a quadratic Objective function x1^2 - x1*x2 + x2^2 - 3*x1 + 5 -// // 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 -// HessianFactor lf(X(1), X(2), 2.0 * ones(1, 1), -ones(1, 1), 3.0 * ones(1), -// 2.0 * ones(1, 1), zero(1), 10.0); -// -// // build linear inequalities -// LinearInequalityFactorGraph inequalities; -// inequalities.push_back( -// LinearInequality(X(1), ones(1, 1), X(2), ones(1, 1), 2, 0)); // x1 + x2 <= 2 --> x1 + x2 -2 <= 0, --> b=2 -// inequalities.push_back(LinearInequality(X(1), -ones(1, 1), 0, 1)); // -x1 <= 0 -// inequalities.push_back(LinearInequality(X(2), -ones(1, 1), 0, 2)); // -x2 <= 0 -// inequalities.push_back(LinearInequality(X(1), ones(1, 1), 1.5, 3)); // x1 <= 3/2 -// -// // Compare against a QP -// QP qp; -// qp.cost.add(lf); -// qp.inequalities = inequalities; -// -// // instantiate QPsolver -// QPSolver qpSolver(qp); -// // create initial values for optimization -// VectorValues initialVectorValues; -// initialVectorValues.insert(X(1), zero(1)); -// initialVectorValues.insert(X(2), zero(1)); -// VectorValues expectedSolution = qpSolver.optimize(initialVectorValues).first; -// -// -// NonlinearEqualityFactorGraph linearEqualities; -// NonlinearEqualityFactorGraph nonlinearEqualities; -// nonlinearEqualities.push_back(NonlinearEquality(X(1), ones(1, 1), X(2), ones(1, 1), 2, 0))); -// -// NLP nlp; -// nlp.cost.add(lf); -// nlp.linearEqualities.push_back(NonlinearEqualityFactorGraph()); -// // instantiate QPsolver -// SQPSimple sqpSolver(nlp); -// // create initial values for optimization -// Values initialValues; -// initialValues.insert(X(1), zero(1)); -// initialValues.insert(X(2), zero(1)); -// -// std::pair actualSolution = sqpSolver.optimize(initialValues); -// -// DOUBLES_EQUAL(expectedSolution.at(X(1))[0], actualSolution.at(X(1)), -// tol); -// DOUBLES_EQUAL(expectedSolution.at(X(2))[0], actualSolution.at(X(2)), -// tol); -//}