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 
17 namespace Eigen {
18 
19 template<typename FunctorType>
20 LevenbergMarquardtSpace::Status
minimizeOneStep(FVectorType & x)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