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
|
---|
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
|
---|