Home | History | Annotate | Download | only in LevenbergMarquardt 
      1 // This file is part of Eigen, a lightweight C++ template library
      2 // for linear algebra.
      3 //
      4 // Copyright (C) 2009 Thomas Capricelli <orzel (at) freehackers.org>
      5 //
      6 // This code initially comes from MINPACK whose original authors are:
      7 // Copyright Jorge More - Argonne National Laboratory
      8 // Copyright Burt Garbow - Argonne National Laboratory
      9 // Copyright Ken Hillstrom - Argonne National Laboratory
     10 //
     11 // This Source Code Form is subject to the terms of the Minpack license
     12 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
     13 
     14 #ifndef EIGEN_LMONESTEP_H
     15 #define EIGEN_LMONESTEP_H
     16 
     17 namespace Eigen {
     18 
     19 template<typename FunctorType>
     20 LevenbergMarquardtSpace::Status
     21 LevenbergMarquardt<FunctorType>::minimizeOneStep(FVectorType  &x)
     22 {
     23   using std::abs;
     24   using std::sqrt;
     25   RealScalar temp, temp1,temp2;
     26   RealScalar ratio;
     27   RealScalar pnorm, xnorm, fnorm1, actred, dirder, prered;
     28   eigen_assert(x.size()==n); // check the caller is not cheating us
     29 
     30   temp = 0.0; xnorm = 0.0;
     31   /* calculate the jacobian matrix. */
     32   Index df_ret = m_functor.df(x, m_fjac);
     33   if (df_ret<0)
     34       return LevenbergMarquardtSpace::UserAsked;
     35   if (df_ret>0)
     36       // numerical diff, we evaluated the function df_ret times
     37       m_nfev += df_ret;
     38   else m_njev++;
     39 
     40   /* compute the qr factorization of the jacobian. */
     41   for (int j = 0; j < x.size(); ++j)
     42     m_wa2(j) = m_fjac.col(j).blueNorm();
     43   QRSolver qrfac(m_fjac);
     44   if(qrfac.info() != Success) {
     45     m_info = NumericalIssue;
     46     return LevenbergMarquardtSpace::ImproperInputParameters;
     47   }
     48   // Make a copy of the first factor with the associated permutation
     49   m_rfactor = qrfac.matrixR();
     50   m_permutation = (qrfac.colsPermutation());
     51 
     52   /* on the first iteration and if external scaling is not used, scale according */
     53   /* to the norms of the columns of the initial jacobian. */
     54   if (m_iter == 1) {
     55       if (!m_useExternalScaling)
     56           for (Index j = 0; j < n; ++j)
     57               m_diag[j] = (m_wa2[j]==0.)? 1. : m_wa2[j];
     58 
     59       /* on the first iteration, calculate the norm of the scaled x */
     60       /* and initialize the step bound m_delta. */
     61       xnorm = m_diag.cwiseProduct(x).stableNorm();
     62       m_delta = m_factor * xnorm;
     63       if (m_delta == 0.)
     64           m_delta = m_factor;
     65   }
     66 
     67   /* form (q transpose)*m_fvec and store the first n components in */
     68   /* m_qtf. */
     69   m_wa4 = m_fvec;
     70   m_wa4 = qrfac.matrixQ().adjoint() * m_fvec;
     71   m_qtf = m_wa4.head(n);
     72 
     73   /* compute the norm of the scaled gradient. */
     74   m_gnorm = 0.;
     75   if (m_fnorm != 0.)
     76       for (Index j = 0; j < n; ++j)
     77           if (m_wa2[m_permutation.indices()[j]] != 0.)
     78               m_gnorm = (std::max)(m_gnorm, abs( m_rfactor.col(j).head(j+1).dot(m_qtf.head(j+1)/m_fnorm) / m_wa2[m_permutation.indices()[j]]));
     79 
     80   /* test for convergence of the gradient norm. */
     81   if (m_gnorm <= m_gtol) {
     82     m_info = Success;
     83     return LevenbergMarquardtSpace::CosinusTooSmall;
     84   }
     85 
     86   /* rescale if necessary. */
     87   if (!m_useExternalScaling)
     88       m_diag = m_diag.cwiseMax(m_wa2);
     89 
     90   do {
     91     /* determine the levenberg-marquardt parameter. */
     92     internal::lmpar2(qrfac, m_diag, m_qtf, m_delta, m_par, m_wa1);
     93 
     94     /* store the direction p and x + p. calculate the norm of p. */
     95     m_wa1 = -m_wa1;
     96     m_wa2 = x + m_wa1;
     97     pnorm = m_diag.cwiseProduct(m_wa1).stableNorm();
     98 
     99     /* on the first iteration, adjust the initial step bound. */
    100     if (m_iter == 1)
    101         m_delta = (std::min)(m_delta,pnorm);
    102 
    103     /* evaluate the function at x + p and calculate its norm. */
    104     if ( m_functor(m_wa2, m_wa4) < 0)
    105         return LevenbergMarquardtSpace::UserAsked;
    106     ++m_nfev;
    107     fnorm1 = m_wa4.stableNorm();
    108 
    109     /* compute the scaled actual reduction. */
    110     actred = -1.;
    111     if (Scalar(.1) * fnorm1 < m_fnorm)
    112         actred = 1. - numext::abs2(fnorm1 / m_fnorm);
    113 
    114     /* compute the scaled predicted reduction and */
    115     /* the scaled directional derivative. */
    116     m_wa3 = m_rfactor.template triangularView<Upper>() * (m_permutation.inverse() *m_wa1);
    117     temp1 = numext::abs2(m_wa3.stableNorm() / m_fnorm);
    118     temp2 = numext::abs2(sqrt(m_par) * pnorm / m_fnorm);
    119     prered = temp1 + temp2 / Scalar(.5);
    120     dirder = -(temp1 + temp2);
    121 
    122     /* compute the ratio of the actual to the predicted */
    123     /* reduction. */
    124     ratio = 0.;
    125     if (prered != 0.)
    126         ratio = actred / prered;
    127 
    128     /* update the step bound. */
    129     if (ratio <= Scalar(.25)) {
    130         if (actred >= 0.)
    131             temp = RealScalar(.5);
    132         if (actred < 0.)
    133             temp = RealScalar(.5) * dirder / (dirder + RealScalar(.5) * actred);
    134         if (RealScalar(.1) * fnorm1 >= m_fnorm || temp < RealScalar(.1))
    135             temp = Scalar(.1);
    136         /* Computing MIN */
    137         m_delta = temp * (std::min)(m_delta, pnorm / RealScalar(.1));
    138         m_par /= temp;
    139     } else if (!(m_par != 0. && ratio < RealScalar(.75))) {
    140         m_delta = pnorm / RealScalar(.5);
    141         m_par = RealScalar(.5) * m_par;
    142     }
    143 
    144     /* test for successful iteration. */
    145     if (ratio >= RealScalar(1e-4)) {
    146         /* successful iteration. update x, m_fvec, and their norms. */
    147         x = m_wa2;
    148         m_wa2 = m_diag.cwiseProduct(x);
    149         m_fvec = m_wa4;
    150         xnorm = m_wa2.stableNorm();
    151         m_fnorm = fnorm1;
    152         ++m_iter;
    153     }
    154 
    155     /* tests for convergence. */
    156     if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1. && m_delta <= m_xtol * xnorm)
    157     {
    158        m_info = Success;
    159       return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
    160     }
    161     if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1.)
    162     {
    163       m_info = Success;
    164       return LevenbergMarquardtSpace::RelativeReductionTooSmall;
    165     }
    166     if (m_delta <= m_xtol * xnorm)
    167     {
    168       m_info = Success;
    169       return LevenbergMarquardtSpace::RelativeErrorTooSmall;
    170     }
    171 
    172     /* tests for termination and stringent tolerances. */
    173     if (m_nfev >= m_maxfev)
    174     {
    175       m_info = NoConvergence;
    176       return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
    177     }
    178     if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
    179     {
    180       m_info = Success;
    181       return LevenbergMarquardtSpace::FtolTooSmall;
    182     }
    183     if (m_delta <= NumTraits<Scalar>::epsilon() * xnorm)
    184     {
    185       m_info = Success;
    186       return LevenbergMarquardtSpace::XtolTooSmall;
    187     }
    188     if (m_gnorm <= NumTraits<Scalar>::epsilon())
    189     {
    190       m_info = Success;
    191       return LevenbergMarquardtSpace::GtolTooSmall;
    192     }
    193 
    194   } while (ratio < Scalar(1e-4));
    195 
    196   return LevenbergMarquardtSpace::Running;
    197 }
    198 
    199 
    200 } // end namespace Eigen
    201 
    202 #endif // EIGEN_LMONESTEP_H
    203