diff --git a/src/micpsolver/micpsolver.inl b/src/micpsolver/micpsolver.inl index 4ae390e..409607f 100644 --- a/src/micpsolver/micpsolver.inl +++ b/src/micpsolver/micpsolver.inl @@ -1,470 +1,473 @@ /*------------------------------------------------------- - Module : micpsolver - File : micpsolver.inl - Author : Fabien Georget Copyright (c) 2014, Fabien Georget , Princeton University All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of the Princeton University nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ---------------------------------------------------------*/ #include "micpsolver.hpp" // for syntaxic coloration... #include "estimate_cond_number.hpp" #include "utils/log.hpp" #include //! \file micpsolver.inl implementation of the MiCP solver namespace specmicp { namespace micpsolver { // Main algorithm // ############## template MiCPSolverReturnCode MiCPSolver::solve(Vector &x) { int cnt = 0; if (get_options().use_crashing) crashing(x); else setup_residuals(x); MiCPSolverReturnCode retcode = MiCPSolverReturnCode::NotConvergedYet; Eigen::VectorXd update(get_neq()); while (retcode == MiCPSolverReturnCode::NotConvergedYet) { DEBUG << "Iteration : " << cnt; SPAM << "Solution : \n" << x; // (S.0) this is a hook for simultaneous fixed-point iterations bool may_have_converged = get_program()->hook_start_iteration(x, m_phi_residuals.norm()); // (S.1) Check the convergence setup_residuals(x); get_perf().current_residual = m_phi_residuals.norm(); SPAM << "Residuals : \n ----- \n" << m_phi_residuals << "\n ----- \n"; retcode = base::check_convergence(cnt, update, x, m_phi_residuals, may_have_converged); get_perf().return_code = retcode; if (retcode != MiCPSolverReturnCode::NotConvergedYet) break; ++cnt; // (S.2) Compute the jacobian and solve the linear problem setup_jacobian(x); if(get_options().use_scaling) search_direction_calculation(update); else search_direction_calculation_no_scaling(x, update); // (S.3) Linesearch int termcode = linesearch(update, x); if (termcode != 0) retcode = MiCPSolverReturnCode::LinesearchFailure; get_perf().current_update = update.norm(); DEBUG << "Return LineSearch : " << termcode; base::projection(x); // project x over the positive quadrant get_perf().nb_iterations = cnt; } return retcode; } template MiCPSolverReturnCode MiCPSolver::search_direction_calculation(Eigen::VectorXd& update) { Vector rscaler(Vector::Ones(m_jacobian.cols())); Vector cscaler(Vector::Ones(m_jacobian.rows())); base::scaling_jacobian(m_jacobian, m_phi_residuals, rscaler, cscaler); m_jacobian = rscaler.asDiagonal() * (m_jacobian) * cscaler.asDiagonal(); Eigen::ColPivHouseholderQR solver(m_jacobian); // it needs to be correctly initialized m_gradient_step_taken = false; int m; for (m=0; m()); if (cond > get_options().condition_limit) { m_gradient_step_taken = true; m_jacobian += rscaler.asDiagonal() * ( lambda*Eigen::MatrixXd::Identity(m_jacobian.rows(),m_jacobian.cols())) * cscaler.asDiagonal(); continue; } update = solver.solve(-rscaler.cwiseProduct(m_phi_residuals + m*lambda*m_grad_phi)); update = cscaler.cwiseProduct(update); scalar_t descent_cond = m_grad_phi.dot(update); scalar_t norm_grad = m_grad_phi.norm(); scalar_t norm_update = update.norm(); if ( ((descent_cond <= -get_options().factor_descent_condition*std::min( std::pow(norm_update,2),std::pow(norm_update,3))) and (descent_cond <= -get_options().factor_descent_condition*std::min( std::pow(norm_grad,2),std::pow(norm_grad,3))) )) break; // we have a solution ! m_gradient_step_taken = true; m_jacobian += rscaler.asDiagonal() * ( lambda* Eigen::MatrixXd::Identity(m_jacobian.rows(),m_jacobian.cols()) ) * cscaler.asDiagonal(); } DEBUG << "Gradient step : m = " << m; if (m == get_options().max_factorization_step) { INFO << "Full gradient step taken !"; update = -m_grad_phi; } return MiCPSolverReturnCode::NotConvergedYet; } template MiCPSolverReturnCode MiCPSolver::search_direction_calculation_no_scaling( Vector& x, Vector& update ) { DEBUG << "Solving linear system"; Eigen::ColPivHouseholderQR solver(m_jacobian); // it needs to be correctly initialized // if everything is ok, this is the only decomposition m_gradient_step_taken = false; int m; // this is an index that increase the contribution of the gradient in the solution // the pure Newton solution is when m=0 for (m=0; m()); if (cond < get_options().condition_limit) // if ok { update = solver.solve(-(m_phi_residuals + m*lambda*m_grad_phi)); scalar_t descent_cond = m_grad_phi.dot(update); scalar_t norm_grad = m_grad_phi.norm(); scalar_t norm_update = update.norm(); if (( (descent_cond <= -get_options().factor_descent_condition*std::min( std::pow(norm_update,2),std::pow(norm_update,3))) and (descent_cond <= -get_options().factor_descent_condition*std::min( std::pow(norm_grad,2),std::pow(norm_grad,3))) )) break; // we have a solution ! } // add the perturbation m_gradient_step_taken = true; m_jacobian += lambda*Eigen::MatrixXd::Identity(m_jacobian.rows(),m_jacobian.cols()); } DEBUG << "Gradient step : m = " << m; if (m_gradient_step_taken && m == get_options().max_factorization_step) { INFO << "Full gradient step taken !"; update = -m_grad_phi; } return MiCPSolverReturnCode::NotConvergedYet; } template void MiCPSolver::crashing(Vector& x) { // steepest descent direction algorithm DEBUG << "Crashing "; const scalar_t beta = 0.5; const scalar_t sigma = 1e-5; int cnt = 0; while (cnt < 10) { setup_residuals(x); setup_jacobian(x); m_grad_phi = m_jacobian.transpose()*m_phi_residuals; Vector xp(get_neq()); int l=0; const int maxl = 10; while (l void MiCPSolver::reformulate_residuals( const Vector &x, const Vector &r, Vector &r_phi ) { // reformulation with copy from r to r_phi r_phi.resizeLike(r); r_phi.block(0, 0, get_neq_free(), 1) = r.block(0, 0, get_neq_free(), 1); for (index_t i = get_neq_free(); i void MiCPSolver::reformulate_residuals_inplace( const Vector& x, Vector& r ) { for (index_t i = get_neq_free(); i int MiCPSolver::linesearch(Eigen::VectorXd& p, Eigen::VectorXd& x) { // References // ---------- // - Algo A6.3.1 : Dennis and Schnabel (1983) // - Munson et al. (2001) // - Facchinei (2003) // - Nocedal & Wrigth (2006) DEBUG << "Linesearch"; Eigen::VectorXd xp(get_neq()); Eigen::VectorXd new_res(get_neq()); double fcp; get_perf().max_taken = false; int retcode = 2; const double alpha = 1e-4; double newtlen = base::is_step_too_long(p); double init_slope = m_grad_phi.dot(p); double rellength = std::abs(p(0)); for (int i=1; imax_lambda(x, p); double lambda_prev = lambda; // non monotone linesearch // ======================= double merit_value = 0.5*m_phi_residuals.squaredNorm(); // new residual xp = x + lambda*p; base::compute_residuals(xp, new_res); reformulate_residuals_inplace(xp, new_res); fcp = 0.5*new_res.squaredNorm(); // Skip linesearch if enough progress is done // ------------------------------------------ if (fcp < get_options().coeff_accept_newton_step *merit_value) { if (m_max_merits.size() > 0) m_max_merits[m_max_merits.size()-1] = merit_value; else m_max_merits.push_back(merit_value); x = xp; return 0; } // Select the merit value of reference // ----------------------------------- - double mmax = merit_value; - if (m_max_merits.size() > 0) + if (get_options().non_monotone_linesearch) { - mmax = m_max_merits[m_max_merits.size()-1]; - // check for cycling - // - - - - - - - - - - if ( m_max_merits.size() ==4 - && std::fabs(mmax - merit_value) < get_options().threshold_cycling_linesearch*get_options().fvectol) + double mmax = merit_value; + if (m_max_merits.size() > 0) { - //std::cout << merit_value << std::endl; - WARNING << "Cycling has been detected by the linesearch - Taking the full Newton step"; - x = xp; - p = lambda*p; - return 3; + mmax = m_max_merits[m_max_merits.size()-1]; + // check for cycling + // - - - - - - - - - + if ( m_max_merits.size() ==4 + && std::fabs(mmax - merit_value) < get_options().threshold_cycling_linesearch*get_options().fvectol) + { + //std::cout << merit_value << std::endl; + WARNING << "Cycling has been detected by the linesearch - Taking the full Newton step"; + x = xp; + p = lambda*p; + return 3; + } + } + if (m_max_merits.size() < 4) + { + m_max_merits.push_back(merit_value); + if (merit_value < mmax) merit_value = (3*merit_value + mmax)/4; + } + else if (merit_value < mmax) + { + m_max_merits[3] = merit_value; + merit_value = mmax; + } + if (m_gradient_step_taken) + { + merit_value *= 100; } - } - if (m_max_merits.size() < 4) - { - m_max_merits.push_back(merit_value); - if (merit_value < mmax) merit_value = (3*merit_value + mmax)/4; - } - else if (merit_value < mmax) - { - m_max_merits[3] = merit_value; - merit_value = mmax; - } - if (m_gradient_step_taken) - { - merit_value *= 100; } // The linesearch // -------------- double fc = merit_value; double fcp_prev; int cnt = 0; do { fcp = 0.5*new_res.squaredNorm(); if (fcp <= fc - std::min(-alpha*lambda*init_slope,(1-alpha)*fc)) //pg760 Fachinei2003 { retcode = 0; if (lambda ==1 and (newtlen > 0.99 * get_options().maxstep)) { get_perf().max_taken = true; } break; } else if (lambda < minlambda) { retcode = 1; break; } else { // Select a new step length // - - - - - - - - - - - - double lambdatmp; if (cnt == 0) { // only a quadratic at the first lambdatmp = - init_slope / (2*(fcp - fc -init_slope)); } else { const double factor = 1 /(lambda - lambda_prev); const double x1 = fcp - fc - lambda*init_slope; const double x2 = fcp_prev - fc - lambda_prev*init_slope; const double a = factor * ( x1/(lambda*lambda) - x2/(lambda_prev*lambda_prev)); const double b = factor * ( -x1*lambda_prev/(lambda*lambda) + x2*lambda/(lambda_prev*lambda_prev)); if (a == 0) { // cubic interpolation is in fact a quadratic interpolation lambdatmp = - init_slope/(2*b); } else { const double disc = b*b-3*a*init_slope; lambdatmp = (-b+std::sqrt(disc))/(3*a); } if (lambdatmp > 0.5*lambda ) lambdatmp = 0.5*lambda; } lambda_prev = lambda; fcp_prev = fcp; if (lambdatmp < 0.1*lambda) { lambda = 0.1 * lambda; } else { lambda = lambdatmp; } if (not std::isfinite(lambda)) { ERROR << "Lambda is non finite in micpsolver linesearch - we stop"; return 3; } } xp = x + lambda*p; base::compute_residuals(xp, new_res); reformulate_residuals_inplace(xp, new_res); ++cnt; } while(retcode == 2 and cnt < 100); DEBUG << "Lambda : " << lambda; if (cnt == 100) { ERROR << "Too much linesearch iterations ! We stop"; } x = xp; p = lambda*p; return retcode; } template void MiCPSolver::reformulate_jacobian(const Vector &x) { base::reformulate_jacobian_cck(x, m_residuals, m_jacobian); // // see Facchinei and Pang for more details // // set the z vector : contains 1 for degenerate points // Eigen::VectorXd z(Eigen::VectorXd::Zero(get_neq())); // for (int i=get_neq_free(); i 0) and (x(i) >0)) // { // c -= (1-lambda)* m_residuals(i); // d -= (1-lambda)*x(i); // } // grad_fi = d*grad_fi; // grad_fi(i) += c; // } // m_jacobian.block(i, 0, 1, get_neq()) = grad_fi.transpose(); // } } } // end namespace micpsolver } // end namespace specmicp diff --git a/src/micpsolver/micpsolver_structs.hpp b/src/micpsolver/micpsolver_structs.hpp index 1b2497b..8740a02 100644 --- a/src/micpsolver/micpsolver_structs.hpp +++ b/src/micpsolver/micpsolver_structs.hpp @@ -1,166 +1,168 @@ /*------------------------------------------------------- - Module : micpsolver - File : micpsolver_structs.hpp - Author : Fabien Georget Copyright (c) 2014, Fabien Georget , Princeton University All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of the Princeton University nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ---------------------------------------------------------*/ #ifndef SPECMICP_MICPSOLVER_MICPSOLVERSTRUCTS_HPP #define SPECMICP_MICPSOLVER_MICPSOLVERSTRUCTS_HPP //! \file micpsolver_structs.hpp structures and enum used by the micpsolver // this file exists to reduce the header dependancy #include namespace specmicp { namespace micpsolver { //! \brief Options for the MiCPSolver struct MiCPSolverOptions { int max_iter; //!< Maximum number of iterations allowed // tolerances double fvectol; //!< Tolerance for minimization of residuals double steptol; //!< Tolerance for minimization of error // misc ... double condition_limit; //!< Condition number limit double penalization_factor; //!< Penalization factor for the penalized Fisher-Burmeister function double maxstep; //!< the maximum step allowed int maxiter_maxstep; //!< the maximum number of step of length maxstep allowed double factor_descent_condition; //!< factor in front of the descent condition > 0 double power_descent_condition; //!< power used in the traditional format of the descent condition > 2 bool use_crashing; //! Use a crashing step to improve the starting guess double coeff_accept_newton_step; //! Accept the newton step without line search if merit(x+d)threshold_stationary_point and ErrorMinimized -> stationary point double threshold_cycling_linesearch; //! factor to detect cycling in linesearch int max_factorization_step; //! Max number of factorization in an iteration + bool non_monotone_linesearch; //! Use non monotone linesearch //! \brief Constructor - also defines the default value used by the algorithm MiCPSolverOptions(): max_iter(100), fvectol(1e-8), steptol(1e-10), condition_limit(1e6), penalization_factor(0.8), maxstep(100), // this is quite restrictif maxiter_maxstep(20), factor_descent_condition(1e-4), power_descent_condition(2), use_crashing(false), coeff_accept_newton_step(0.95), use_scaling(true), factor_gradient_search_direction(5.0), projection_min_variable(DBL_EPSILON), threshold_stationary_point(1e-6), threshold_cycling_linesearch(1e-5), - max_factorization_step(4) + max_factorization_step(4), + non_monotone_linesearch(true) {} }; //! \brief Return code of the MiCP solver //! //! A positive return code means that the algorithm converged (but maybe to a stationnary points) //! A negative return code means that something wrong happened and was detected enum class MiCPSolverReturnCode { LolItsNotSupposedToHappen = -10, //!< bummer... LinesearchFailure = -5, //!< Failure in linesearch MaxStepTakenTooManyTimes = -4, //!< Probably detect a divergence FailedToSolveLinearSystem = -3, //!< Problem in the decomposition... shouldn't be raised, use of the gradient step MaxIterations = -2, //!< Algorithm has reached the maximum number of iterations StationaryPoint = -1, //!< Algorithm is stuck in a stationnary point of the merit function NotConvergedYet = 0, //!< Keep going... Success = 1, //!< Test success (well be careful of stationnary points) ResidualMinimized = 2, //!< The residual is minimized (i.e. close to zero) ErrorMinimized = 3 //!< The error is minimized, may indicate a stationary point }; //! \brief This structure contains counter to check/query the performance of the algorithm //! //! It should be updated after each operations, not at the end struct MiCPPerformance { int nb_call_residuals; //! Number of calls of the residual int nb_call_jacobian; //! Number of calls of the jacobian int nb_factorization; //! Number of factorization performed (may be > number of iterations) int nb_gradient_step; //! Number of gradient steps (too many gradient steps indicate a bad starting guess) int nb_crashing_iterations; //! Number of crashing iterations int nb_iterations; //! Number of iterations bool max_taken; //! Maximum step has been taken int nb_max_taken; //! Number of time the maximum has been taken int nb_consecutive_max_taken; //! Number of consecutive step at max length double current_residual; //! Current residual double current_update; //! Current update MiCPSolverReturnCode return_code; //! Return code MiCPPerformance(): nb_call_residuals(0), nb_call_jacobian(0), nb_factorization(0), nb_gradient_step(0), nb_crashing_iterations(0), nb_iterations(0), max_taken(false), nb_max_taken(0), nb_consecutive_max_taken(0), current_residual(INFINITY), current_update(INFINITY), return_code(MiCPSolverReturnCode::NotConvergedYet) {} //! \brief add two performance instances - useful when restarting a computation in case of failure MiCPPerformance& operator+=(const MiCPPerformance& other) { nb_call_residuals += other.nb_call_residuals; nb_call_jacobian += other.nb_call_jacobian; nb_factorization += other.nb_factorization; nb_gradient_step += other.nb_gradient_step; nb_crashing_iterations += other.nb_crashing_iterations; nb_iterations += other.nb_iterations; return_code = other.return_code; // we take the last one return *this; } }; //! The NCP function that are implemented enum class NCPfunction { penalizedFB, // Penalized Fischer-Burmeister function min // The minimum function }; } // end namespace micpsolver } // end namespace specmicp #endif //SPECMICP_MICPSOLVER_MICPSOLVERSTRUCTS_HPP