LMonestep.h revision 7faaa9f3f0df9d23790277834d426c3d992ac3ba 
1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// Copyright (C) 2009 Thomas Capricelli <orzel@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
17namespace Eigen {
18
19template<typename FunctorType>
20LevenbergMarquardtSpace::Status
21LevenbergMarquardt<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