From ba4698bf515e03099a04f921aca0b48fb40bf7b1 Mon Sep 17 00:00:00 2001 From: thduynguyen Date: Thu, 19 Feb 2015 08:51:54 -0500 Subject: [PATCH] add comments, fix format, make test compiled --- gtsam_unstable/nonlinear/LCNLPSolver.cpp | 50 ++++++++++--------- gtsam_unstable/nonlinear/LCNLPSolver.h | 1 + .../nonlinear/NonlinearConstraint.h | 22 ++++++-- .../nonlinear/NonlinearInequality.h | 8 +-- .../nonlinear/tests/testLCNLPSolver.cpp | 16 +++--- 5 files changed, 56 insertions(+), 41 deletions(-) diff --git a/gtsam_unstable/nonlinear/LCNLPSolver.cpp b/gtsam_unstable/nonlinear/LCNLPSolver.cpp index fe96878fb..722576eba 100644 --- a/gtsam_unstable/nonlinear/LCNLPSolver.cpp +++ b/gtsam_unstable/nonlinear/LCNLPSolver.cpp @@ -23,7 +23,6 @@ namespace gtsam { - /* ************************************************************************* */ bool LCNLPSolver::isStationary(const VectorValues& delta) const { return delta.vector().lpNorm() < errorTol; @@ -36,12 +35,13 @@ bool LCNLPSolver::isPrimalFeasible(const LCNLPState& state) const { /* ************************************************************************* */ bool LCNLPSolver::isDualFeasible(const VectorValues& duals) const { - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearInequalities) { - NonlinearConstraint::shared_ptr inequality = boost::dynamic_pointer_cast(factor); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.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) { // See the explanation in QPSolver::identifyLeavingConstraint, we want dual < 0 ? + double dual = duals.at(dualKey)[0];// because we only support single-valued inequalities + if (dual > 0.0) { // See the explanation in QPSolver::identifyLeavingConstraint, we want dual < 0 ? return false; } } @@ -50,19 +50,23 @@ bool LCNLPSolver::isDualFeasible(const VectorValues& duals) const { /* ************************************************************************* */ bool LCNLPSolver::isComplementary(const LCNLPState& state) const { - return lcnlp_.linearInequalities.checkFeasibilityAndComplimentary(state.values, state.duals, errorTol); + return lcnlp_.linearInequalities.checkFeasibilityAndComplimentary( + state.values, state.duals, errorTol); } /* ************************************************************************* */ -bool LCNLPSolver::checkConvergence(const LCNLPState& state, const VectorValues& delta) const { - return isStationary(delta) && isPrimalFeasible(state) && isDualFeasible(state.duals) && isComplementary(state); +bool LCNLPSolver::checkConvergence(const LCNLPState& state, + const VectorValues& delta) const { + return isStationary(delta) && isPrimalFeasible(state) + && isDualFeasible(state.duals) && isComplementary(state); } /* ************************************************************************* */ VectorValues LCNLPSolver::initializeDuals() const { VectorValues duals; - BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearEqualities) { - NonlinearConstraint::shared_ptr constraint = boost::dynamic_pointer_cast(factor); + BOOST_FOREACH(const NonlinearFactor::shared_ptr& factor, lcnlp_.linearEqualities){ + NonlinearConstraint::shared_ptr constraint + = boost::dynamic_pointer_cast(factor); duals.insert(constraint->dualKey(), zero(factor->dim())); } @@ -70,21 +74,23 @@ VectorValues LCNLPSolver::initializeDuals() const { } /* ************************************************************************* */ -std::pair LCNLPSolver::optimize(const Values& initialValues, bool useWarmStart, bool debug) 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; + std::cout << "state: iteration " << state.iterations << std::endl; state = iterate(state, useWarmStart, debug); } if (debug) - std::cout << "Number of iterations: " << state.iterations << std::endl; + 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, bool debug) const { +LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart, + bool debug) const { // construct the qp subproblem QP qp; @@ -98,15 +104,12 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart, bool // solve the QP subproblem VectorValues delta, duals; QPSolver qpSolver(qp); - if (useWarmStart == false || state.iterations == 0) - boost::tie(delta, duals) = qpSolver.optimize(); - else { - VectorValues zeroInitialValues; - BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, state.values) { - zeroInitialValues.insert(key_value.key, zero(key_value.value.dim())); - } - boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals, useWarmStart); + VectorValues zeroInitialValues; + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, state.values) { + zeroInitialValues.insert(key_value.key, zero(key_value.value.dim())); } + boost::tie(delta, duals) = qpSolver.optimize(zeroInitialValues, state.duals, + useWarmStart); if (debug) state.values.print("state.values: "); @@ -128,7 +131,6 @@ LCNLPState LCNLPSolver::iterate(const LCNLPState& state, bool useWarmStart, bool return newState; } -} - +} // namespace gtsam diff --git a/gtsam_unstable/nonlinear/LCNLPSolver.h b/gtsam_unstable/nonlinear/LCNLPSolver.h index 47f6f86ae..b05ddf8bd 100644 --- a/gtsam_unstable/nonlinear/LCNLPSolver.h +++ b/gtsam_unstable/nonlinear/LCNLPSolver.h @@ -107,6 +107,7 @@ public: LCNLPState iterate(const LCNLPState& state, bool useWarmStart = true, bool debug = false) const; VectorValues initializeDuals() const; + /** * Main optimization function. new */ diff --git a/gtsam_unstable/nonlinear/NonlinearConstraint.h b/gtsam_unstable/nonlinear/NonlinearConstraint.h index bc0bc04b5..4043eb9b1 100644 --- a/gtsam_unstable/nonlinear/NonlinearConstraint.h +++ b/gtsam_unstable/nonlinear/NonlinearConstraint.h @@ -26,10 +26,19 @@ 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_; + Key dualKey_; //!< Unique key for the dual variable associated with this constraint public: typedef boost::shared_ptr shared_ptr; @@ -37,14 +46,17 @@ public: /// Construct with dual key NonlinearConstraint(Key dualKey) : dualKey_(dualKey) {} + + /// Return the dual key + Key dualKey() const { return dualKey_; } + /** - * compute the HessianFactor of the (-dual * constraintHessian) for the qp subproblem's objective function + * 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; - - /// return the dual key - Key dualKey() const { return dualKey_; } }; /* ************************************************************************* */ diff --git a/gtsam_unstable/nonlinear/NonlinearInequality.h b/gtsam_unstable/nonlinear/NonlinearInequality.h index 439d5e070..f7021416b 100644 --- a/gtsam_unstable/nonlinear/NonlinearInequality.h +++ b/gtsam_unstable/nonlinear/NonlinearInequality.h @@ -9,7 +9,7 @@ * -------------------------------------------------------------------------- */ /** - * @file NonlinearConstraint.h + * @file NonlinearInequality.h * @brief * @author Duy-Nguyen Ta * @date Sep 30, 2013 @@ -38,7 +38,7 @@ protected: typedef NonlinearInequality1 This; private: - static const int X1Dim = traits::dimension::value; + static const int X1Dim = traits::dimension; public: @@ -100,8 +100,8 @@ protected: typedef NonlinearInequality2 This; private: - static const int X1Dim = traits::dimension::value; - static const int X2Dim = traits::dimension::value; + static const int X1Dim = traits::dimension; + static const int X2Dim = traits::dimension; public: diff --git a/gtsam_unstable/nonlinear/tests/testLCNLPSolver.cpp b/gtsam_unstable/nonlinear/tests/testLCNLPSolver.cpp index d9c6a2ba8..9c1d8510b 100644 --- a/gtsam_unstable/nonlinear/tests/testLCNLPSolver.cpp +++ b/gtsam_unstable/nonlinear/tests/testLCNLPSolver.cpp @@ -436,15 +436,15 @@ TEST_DISABLED(testlcnlpSolver, iterativePoseinBox) { // Instantiate LCNLPSolver LCNLPSolver lcnlpSolver(lcnlp); - if (firstTime) { +// if (firstTime) { solutionAndDuals = lcnlpSolver.optimize(isamValues, useWarmStart); - firstTime = false; - } - else { - cout << " using this \n "; - solutionAndDuals = lcnlpSolver.optimize(isamValues, solutionAndDuals.second, useWarmStart); - - } +// firstTime = false; +// } +// else { +// cout << " using this \n "; +// solutionAndDuals = lcnlpSolver.optimize(isamValues, solutionAndDuals.second, useWarmStart); +// +// } cout << " ************************** \n"; } actualSolution = solutionAndDuals.first;