src/LevenbergMarquardt/LMonestep.h

0001 // This file is part of Eigen, a lightweight C++ template library
0002 // for linear algebra.
0003 //
0004 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
0005 //
0006 // This code initially comes from MINPACK whose original authors are:
0007 // Copyright Jorge More - Argonne National Laboratory
0008 // Copyright Burt Garbow - Argonne National Laboratory
0009 // Copyright Ken Hillstrom - Argonne National Laboratory
0010 //
0011 // This Source Code Form is subject to the terms of the Minpack license
0012 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
0013
0014 #ifndef EIGEN_LMONESTEP_H
0015 #define EIGEN_LMONESTEP_H
0016
0017 namespace Eigen {
0018
0019 template<typename FunctorType>
0020 LevenbergMarquardtSpace::Status
0021 LevenbergMarquardt<FunctorType>::minimizeOneStep(FVectorType  &x)
0022 {
0023   using std::abs;
0024   using std::sqrt;
0025   RealScalar temp, temp1,temp2;
0026   RealScalar ratio;
0027   RealScalar pnorm, xnorm, fnorm1, actred, dirder, prered;
0028   eigen_assert(x.size()==n); // check the caller is not cheating us
0029
0030   temp = 0.0; xnorm = 0.0;
0031   /* calculate the jacobian matrix. */
0032   Index df_ret = m_functor.df(x, m_fjac);
0033   if (df_ret<0)
0034       return LevenbergMarquardtSpace::UserAsked;
0035   if (df_ret>0)
0036       // numerical diff, we evaluated the function df_ret times
0037       m_nfev += df_ret;
0038   else m_njev++;
0039
0040   /* compute the qr factorization of the jacobian. */
0041   for (int j = 0; j < x.size(); ++j)
0042     m_wa2(j) = m_fjac.col(j).blueNorm();
0043   QRSolver qrfac(m_fjac);
0044   if(qrfac.info() != Success) {
0045     m_info = NumericalIssue;
0046     return LevenbergMarquardtSpace::ImproperInputParameters;
0047   }
0048   // Make a copy of the first factor with the associated permutation
0049   m_rfactor = qrfac.matrixR();
0050   m_permutation = (qrfac.colsPermutation());
0051
0052   /* on the first iteration and if external scaling is not used, scale according */
0053   /* to the norms of the columns of the initial jacobian. */
0054   if (m_iter == 1) {
0055       if (!m_useExternalScaling)
0056           for (Index j = 0; j < n; ++j)
0057               m_diag[j] = (m_wa2[j]==0.)? 1. : m_wa2[j];
0058
0059       /* on the first iteration, calculate the norm of the scaled x */
0060       /* and initialize the step bound m_delta. */
0061       xnorm = m_diag.cwiseProduct(x).stableNorm();
0062       m_delta = m_factor * xnorm;
0063       if (m_delta == 0.)
0064           m_delta = m_factor;
0065   }
0066
0067   /* form (q transpose)*m_fvec and store the first n components in */
0068   /* m_qtf. */
0069   m_wa4 = m_fvec;
0070   m_wa4 = qrfac.matrixQ().adjoint() * m_fvec;
0071   m_qtf = m_wa4.head(n);
0072
0073   /* compute the norm of the scaled gradient. */
0074   m_gnorm = 0.;
0075   if (m_fnorm != 0.)
0076       for (Index j = 0; j < n; ++j)
0077           if (m_wa2[m_permutation.indices()[j]] != 0.)
0078               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]]));
0079
0080   /* test for convergence of the gradient norm. */
0081   if (m_gnorm <= m_gtol) {
0082     m_info = Success;
0083     return LevenbergMarquardtSpace::CosinusTooSmall;
0084   }
0085
0086   /* rescale if necessary. */
0087   if (!m_useExternalScaling)
0088       m_diag = m_diag.cwiseMax(m_wa2);
0089
0090   do {
0091     /* determine the levenberg-marquardt parameter. */
0092     internal::lmpar2(qrfac, m_diag, m_qtf, m_delta, m_par, m_wa1);
0093
0094     /* store the direction p and x + p. calculate the norm of p. */
0095     m_wa1 = -m_wa1;
0096     m_wa2 = x + m_wa1;
0097     pnorm = m_diag.cwiseProduct(m_wa1).stableNorm();
0098
0099     /* on the first iteration, adjust the initial step bound. */
0100     if (m_iter == 1)
0101         m_delta = (std::min)(m_delta,pnorm);
0102
0103     /* evaluate the function at x + p and calculate its norm. */
0104     if ( m_functor(m_wa2, m_wa4) < 0)
0105         return LevenbergMarquardtSpace::UserAsked;
0106     ++m_nfev;
0107     fnorm1 = m_wa4.stableNorm();
0108
0109     /* compute the scaled actual reduction. */
0110     actred = -1.;
0111     if (Scalar(.1) * fnorm1 < m_fnorm)
0112         actred = 1. - numext::abs2(fnorm1 / m_fnorm);
0113
0114     /* compute the scaled predicted reduction and */
0115     /* the scaled directional derivative. */
0116     m_wa3 = m_rfactor.template triangularView<Upper>() * (m_permutation.inverse() *m_wa1);
0117     temp1 = numext::abs2(m_wa3.stableNorm() / m_fnorm);
0118     temp2 = numext::abs2(sqrt(m_par) * pnorm / m_fnorm);
0119     prered = temp1 + temp2 / Scalar(.5);
0120     dirder = -(temp1 + temp2);
0121
0122     /* compute the ratio of the actual to the predicted */
0123     /* reduction. */
0124     ratio = 0.;
0125     if (prered != 0.)
0126         ratio = actred / prered;
0127
0128     /* update the step bound. */
0129     if (ratio <= Scalar(.25)) {
0130         if (actred >= 0.)
0131             temp = RealScalar(.5);
0132         if (actred < 0.)
0133             temp = RealScalar(.5) * dirder / (dirder + RealScalar(.5) * actred);
0134         if (RealScalar(.1) * fnorm1 >= m_fnorm || temp < RealScalar(.1))
0135             temp = Scalar(.1);
0136         /* Computing MIN */
0137         m_delta = temp * (std::min)(m_delta, pnorm / RealScalar(.1));
0138         m_par /= temp;
0139     } else if (!(m_par != 0. && ratio < RealScalar(.75))) {
0140         m_delta = pnorm / RealScalar(.5);
0141         m_par = RealScalar(.5) * m_par;
0142     }
0143
0144     /* test for successful iteration. */
0145     if (ratio >= RealScalar(1e-4)) {
0146         /* successful iteration. update x, m_fvec, and their norms. */
0147         x = m_wa2;
0148         m_wa2 = m_diag.cwiseProduct(x);
0149         m_fvec = m_wa4;
0150         xnorm = m_wa2.stableNorm();
0151         m_fnorm = fnorm1;
0152         ++m_iter;
0153     }
0154
0155     /* tests for convergence. */
0156     if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1. && m_delta <= m_xtol * xnorm)
0157     {
0158        m_info = Success;
0159       return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall;
0160     }
0161     if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1.)
0162     {
0163       m_info = Success;
0164       return LevenbergMarquardtSpace::RelativeReductionTooSmall;
0165     }
0166     if (m_delta <= m_xtol * xnorm)
0167     {
0168       m_info = Success;
0169       return LevenbergMarquardtSpace::RelativeErrorTooSmall;
0170     }
0171
0172     /* tests for termination and stringent tolerances. */
0173     if (m_nfev >= m_maxfev)
0174     {
0175       m_info = NoConvergence;
0176       return LevenbergMarquardtSpace::TooManyFunctionEvaluation;
0177     }
0178     if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
0179     {
0180       m_info = Success;
0181       return LevenbergMarquardtSpace::FtolTooSmall;
0182     }
0183     if (m_delta <= NumTraits<Scalar>::epsilon() * xnorm)
0184     {
0185       m_info = Success;
0186       return LevenbergMarquardtSpace::XtolTooSmall;
0187     }
0188     if (m_gnorm <= NumTraits<Scalar>::epsilon())
0189     {
0190       m_info = Success;
0191       return LevenbergMarquardtSpace::GtolTooSmall;
0192     }
0193
0194   } while (ratio < Scalar(1e-4));
0195
0196   return LevenbergMarquardtSpace::Running;
0197 }
0198
0199
0200 } // end namespace Eigen
0201
0202 #endif // EIGEN_LMONESTEP_H