diff --git a/gtsam_unstable/linear/LinearInequality.h b/gtsam_unstable/linear/LinearInequality.h index 6aa03eaef..7eac21903 100644 --- a/gtsam_unstable/linear/LinearInequality.h +++ b/gtsam_unstable/linear/LinearInequality.h @@ -51,7 +51,7 @@ public: } /** Conversion from JacobianFactor */ - explicit LinearInequality(const JacobianFactor& jf) : Base(jf), dualKey_(dualKey), active_(true) { + explicit LinearInequality(const JacobianFactor& jf, Key dualKey) : Base(jf), dualKey_(dualKey), active_(true) { if (!jf.isConstrained()) { throw std::runtime_error("Cannot convert an unconstrained JacobianFactor to LinearEquality"); } diff --git a/gtsam_unstable/nonlinear/NonlinearConstraint.h b/gtsam_unstable/nonlinear/NonlinearConstraint.h index f24517983..af52ea178 100644 --- a/gtsam_unstable/nonlinear/NonlinearConstraint.h +++ b/gtsam_unstable/nonlinear/NonlinearConstraint.h @@ -70,7 +70,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), NonlinearConstraint(dualKey) { } virtual ~NonlinearConstraint1() { @@ -108,8 +108,12 @@ public: lG11sum += -lambda[i] * G11[i]; } - return boost::make_shared(Base::key(), lG11sum, - zero(X1Dim), 100.0); + + std::cout << "lG11sum: " << lG11sum << std::endl; + HessianFactor::shared_ptr hf(new HessianFactor(Base::key(), lG11sum, + zero(X1Dim), 100.0)); + hf->print("HessianFactor: "); + return hf; } /** evaluate Hessians for lambda factors */ @@ -149,8 +153,8 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { ar - & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); } }; // \class NonlinearConstraint1 @@ -191,7 +195,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), NonlinearConstraint(dualKey) { } virtual ~NonlinearConstraint2() { @@ -229,7 +233,7 @@ public: // Combine the Lagrange-multiplier part into this constraint factor Matrix lG11sum = zeros(G11[0].rows(), G11[0].cols()), lG12sum = zeros( G12[0].rows(), G12[0].cols()), lG22sum = zeros(G22[0].rows(), - G22[0].cols()); + G22[0].cols()); for (size_t i = 0; i < lambda.rows(); ++i) { lG11sum += -lambda[i] * G11[i]; lG12sum += -lambda[i] * G12[i]; @@ -249,7 +253,7 @@ public: boost::function vecH1( boost::bind(&This::vectorizeH1t, this, _1, _2)), vecH2( - boost::bind(&This::vectorizeH2t, this, _1, _2)); + boost::bind(&This::vectorizeH2t, this, _1, _2)); Matrix G11all = numericalDerivative21(vecH1, x1, x2, 1e-5); Matrix G12all = numericalDerivative22(vecH1, x1, x2, 1e-5); @@ -302,8 +306,8 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { ar - & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); } }; // \class NonlinearConstraint2 @@ -346,7 +350,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), NonlinearConstraint(dualKey) { } virtual ~NonlinearConstraint3() { @@ -387,9 +391,9 @@ public: // Combine the Lagrange-multiplier part into this constraint factor Matrix lG11sum = zeros(G11[0].rows(), G11[0].cols()), lG12sum = zeros( G12[0].rows(), G12[0].cols()), lG13sum = zeros(G13[0].rows(), - G13[0].cols()), lG22sum = zeros(G22[0].rows(), G22[0].cols()), lG23sum = - zeros(G23[0].rows(), G23[0].cols()), lG33sum = zeros(G33[0].rows(), - G33[0].cols()); + G13[0].cols()), lG22sum = zeros(G22[0].rows(), G22[0].cols()), lG23sum = + zeros(G23[0].rows(), G23[0].cols()), lG33sum = zeros(G33[0].rows(), + G33[0].cols()); for (size_t i = 0; i < lambda.rows(); ++i) { lG11sum += -lambda[i] * G11[i]; lG12sum += -lambda[i] * G12[i]; @@ -467,8 +471,8 @@ public: boost::function vecH1( boost::bind(&This::vectorizeH1t, this, _1, _2, _3)), vecH2( - boost::bind(&This::vectorizeH2t, this, _1, _2, _3)), vecH3( - boost::bind(&This::vectorizeH3t, this, _1, _2, _3)); + boost::bind(&This::vectorizeH2t, this, _1, _2, _3)), vecH3( + boost::bind(&This::vectorizeH3t, this, _1, _2, _3)); Matrix G11all = numericalDerivative31(vecH1, x1, x2, x3, 1e-5); Matrix G12all = numericalDerivative32(vecH1, x1, x2, x3, 1e-5); @@ -549,8 +553,8 @@ private: template void serialize(ARCHIVE & ar, const unsigned int version) { ar - & boost::serialization::make_nvp("NoiseModelFactor", - boost::serialization::base_object(*this)); + & boost::serialization::make_nvp("NoiseModelFactor", + boost::serialization::base_object(*this)); } }; // \class NonlinearConstraint3 diff --git a/gtsam_unstable/nonlinear/NonlinearInequality.h b/gtsam_unstable/nonlinear/NonlinearInequality.h index b9a9e418a..8d41b3ec0 100644 --- a/gtsam_unstable/nonlinear/NonlinearInequality.h +++ b/gtsam_unstable/nonlinear/NonlinearInequality.h @@ -11,30 +11,11 @@ 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 { +class NonlinearInequality1: public NonlinearConstraint1 { public: @@ -62,8 +43,8 @@ public: * @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) { + NonlinearInequality1(Key key, Key dualKey) : + Base(key, dualKey, 1) { } virtual ~NonlinearInequality1() { @@ -74,9 +55,63 @@ public: * 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; + + /** predefine evaluateError to return a 1-dimension vector */ virtual Vector - evaluateError(const X&, boost::optional H1 = boost::none) const { - return (Vector(1) << computeError(X, H1)); + 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 + +/* ************************************************************************* */ +/** A convenient base class for creating your own NonlinearConstraint with 2 + * variables. To derive from this class, implement evaluateError(). */ +template +class NonlinearInequality2: public NonlinearConstraint2 { + +public: + + // typedefs for value types pulled from keys + typedef VALUE1 X1; + typedef VALUE2 X2; + +protected: + + typedef NonlinearConstraint2 Base; + typedef NonlinearInequality2 This; + +private: + static const int X1Dim = traits::dimension::value; + static const int X2Dim = traits::dimension::value; + +public: + + /** + * Default Constructor for I/O + */ + NonlinearInequality2() { + } + + /** + * 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 + */ + NonlinearInequality2(Key j1, Key j2, Key dualKey) : + Base(j1, j2, 1) { + } + + virtual ~NonlinearInequality2() { } /** @@ -85,9 +120,17 @@ public: * 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; + computeError(const X1&, const X2&, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const = 0; + /** predefine evaluateError to return a 1-dimension vector */ + virtual Vector + evaluateError(const X1& x1, const X2& x2, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const { + return (Vector(1) << computeError(x1, x2, H1, H2)).finished(); + } }; -// \class NonlinearConstraint1 +// \class NonlinearConstraint2 + } /* namespace gtsam */ diff --git a/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp b/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp index 08bcfb614..d83312ee1 100644 --- a/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp +++ b/gtsam_unstable/nonlinear/tests/testSQPSimple.cpp @@ -23,6 +23,7 @@ #include #include #include +#include #include #include @@ -102,47 +103,31 @@ public: } /** - * Return true if the error is <= 0.0 + * Return true if the all errors are <= 0.0 */ - bool checkFeasibility(const Values& values, double tol) const { + 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); Vector error = noiseModelFactor->unwhitenedError(values); - // TODO: Do we need to check if it's active or not? + + // Primal feasibility condition: all constraints need to be <= 0.0 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( + // Complimentary condition: errors of active constraints need to be 0.0 + NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast( factor); - Vector error = noiseModelFactor->unwhitenedError(values); - if (error[0] > 0.0) { + 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; - } + } return true; } + }; struct NLP { @@ -181,22 +166,49 @@ public: } /// Check if \nabla f(x) - \lambda * \nabla c(x) == 0 - bool isDualFeasible(const VectorValues& delta) const { - return delta.vector().lpNorm() < errorTol - && nlp_.linearInequalities.checkDualFeasibility(errorTol); -// return false; + bool isStationary(const VectorValues& delta) const { + return delta.vector().lpNorm() < errorTol; } - /// Check if c(x) == 0 + /// 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) - && nlp_.linearInequalities.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 isPrimalFeasible(state) & isDualFeasible(delta); + return isStationary(delta) && isPrimalFeasible(state) && isDualFeasible(state.duals) && isComplementary(state); } /** @@ -275,7 +287,6 @@ public: #include #include #include -#include using namespace std; using namespace gtsam::symbol_shorthand; @@ -403,7 +414,6 @@ TEST_UNSAFE(testSQPSimple, quadraticCostNonlinearConstraint) { Values actualSolution = sqpSimple.optimize(initialValues).first; CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); - actualSolution.print("actualSolution: "); } //****************************************************************************** @@ -417,6 +427,13 @@ public: return pose.x(); } + void evaluateHessians(const Pose3& pose, std::vector& G11) const { + Matrix G11all = Z_6x6; + Vector rT1 = pose.rotation().matrix().row(0); + G11all.block<3,3>(3,0) = skewSymmetric(rT1); + G11.push_back(G11all); + } + 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(); @@ -445,54 +462,164 @@ TEST(testSQPSimple, poseOnALine) { Values actualSolution = sqpSimple.optimize(initialValues).first; CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); - actualSolution.print("actualSolution: "); Pose3 pose(Rot3::ypr(0.1, 0.2, 0.3), Point3()); Matrix hessian = numericalHessian(boost::bind(&LineConstraintX::computeError, constraint, _1), pose, 1e-2); - cout << "hessian: \n" << hessian << endl; +} + +//****************************************************************************** +/// 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(testSQPSimple, 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 NLP + NLP nlp; + Values linPoint; + linPoint.insert(X(1), zero(1)); + linPoint.insert(Y(1), zero(1)); + nlp.cost.add(LinearContainerFactor(hf, linPoint)); // wrap it using linearcontainerfactor + nlp.linearInequalities.add(InequalityProblem1(X(1), Y(1), dualKey)); + + Values initialValues; + initialValues.insert(X(1), 1.0); + initialValues.insert(Y(1), -10.0); + + // Instantiate SQP + SQPSimple sqpSimple(nlp); + Values actualValues = sqpSimple.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; +const size_t Z_AXIS = 2; + /** - * Inequality boundary constraint - * x <= bound + * Inequality boundary constraint on one axis (x, y or z) + * axis <= bound */ -class UpperBoundX : public NonlinearInequality1 { +class AxisUpperBound : public NonlinearInequality1 { typedef NonlinearInequality1 Base; + size_t axis_; double bound_; + public: - UpperBoundX(Key key, double bound, Key dualKey) : Base(key, dualKey, 1), bound_(bound) { + AxisUpperBound(Key key, size_t axis, double bound, Key dualKey) : Base(key, dualKey), axis_(axis), 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_; + *H = (Matrix(1,6) << zeros(1,3), pose.rotation().matrix().row(axis_)).finished(); + return pose.translation().vector()[axis_] - bound_; } }; -TEST(testSQPSimple, poseOnALine) { - const Key dualKey = 0; +/** + * Inequality boundary constraint on one axis (x, y or z) + * bound <= axis + */ +class AxisLowerBound : public NonlinearInequality1 { + typedef NonlinearInequality1 Base; + size_t axis_; + double bound_; +public: + AxisLowerBound(Key key, size_t axis, double bound, Key dualKey) : Base(key, dualKey), axis_(axis), 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(axis_)).finished(); + return -pose.translation().vector()[axis_] + bound_; + } +}; + +TEST(testSQPSimple, poseWithABoundary) { + 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); + nlp.cost.add(PriorFactor(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(1, 0, 0)), noiseModel::Unit::Create(6))); + AxisUpperBound constraint(X(1), X_AXIS, 0, dualKey); + nlp.linearInequalities.add(constraint); Values initialValues; - initialValues.insert(X(1), Pose3(Rot3::ypr(0.3, 0.2, 0.3), Point3(-1,0,0))); + 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())); + expectedSolution.insert(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(0, 0, 0))); // Instantiate SQP SQPSimple sqpSimple(nlp); Values actualSolution = sqpSimple.optimize(initialValues).first; CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); - actualSolution.print("actualSolution: "); +} + +TEST(testSQPSimple, poseWithinA2DBox) { + const Key dualKey = 0; + + //Instantiate NLP + NLP nlp; + nlp.cost.add(PriorFactor(X(1), Pose3(Rot3::ypr(0.1, 0.2, 0.3), Point3(10, 0.5, 0)), noiseModel::Unit::Create(6))); + nlp.linearInequalities.add(AxisLowerBound(X(1), X_AXIS, -1, dualKey)); + nlp.linearInequalities.add(AxisUpperBound(X(1), X_AXIS, 1, dualKey)); + nlp.linearInequalities.add(AxisLowerBound(X(1), Y_AXIS, -1, dualKey)); + nlp.linearInequalities.add(AxisUpperBound(X(1), Y_AXIS, 1, dualKey)); + + 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(1, 0.5, 0))); + + // Instantiate SQP + SQPSimple sqpSimple(nlp); + Values actualSolution = sqpSimple.optimize(initialValues).first; + + CHECK(assert_equal(expectedSolution, actualSolution, 1e-10)); + //TODO: remove printing, refactoring, } //******************************************************************************