LMonestep.h
Go to the documentation of this file.
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>
22 {
23  using std::abs;
24  using std::sqrt;
25  RealScalar temp, temp1,temp2;
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)
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;
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;
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)
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;
160  }
161  if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1.)
162  {
163  m_info = Success;
165  }
166  if (m_delta <= m_xtol * xnorm)
167  {
168  m_info = Success;
170  }
171 
172  /* tests for termination and stringent tolerances. */
173  if (m_nfev >= m_maxfev)
174  {
175  m_info = NoConvergence;
177  }
178  if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.)
179  {
180  m_info = Success;
182  }
183  if (m_delta <= NumTraits<Scalar>::epsilon() * xnorm)
184  {
185  m_info = Success;
187  }
188  if (m_gnorm <= NumTraits<Scalar>::epsilon())
189  {
190  m_info = Success;
192  }
193 
194  } while (ratio < Scalar(1e-4));
195 
197 }
198 
199 
200 } // end namespace Eigen
201 
202 #endif // EIGEN_LMONESTEP_H
SCALAR Scalar
Definition: bench_gemm.cpp:46
#define max(a, b)
Definition: datatypes.h:20
LevenbergMarquardtSpace::Status minimizeOneStep(FVectorType &x)
Definition: LMonestep.h:21
#define min(a, b)
Definition: datatypes.h:19
void lmpar2(const QRSolver &qr, const VectorType &diag, const VectorType &qtb, typename VectorType::Scalar m_delta, typename VectorType::Scalar &par, VectorType &x)
Definition: LMpar.h:20
int n
Namespace containing all symbols from the Eigen library.
Definition: jet.h:637
Holds information about the various numeric (i.e. scalar) types allowed by Eigen. ...
Definition: NumTraits.h:232
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size ratio
#define eigen_assert(x)
Definition: Macros.h:1037
Array< double, 1, 3 > e(1./3., 0.5, 2.)
NumTraits< Scalar >::Real RealScalar
Definition: bench_gemm.cpp:47
Jet< T, N > sqrt(const Jet< T, N > &f)
Definition: jet.h:418
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
#define abs(x)
Definition: datatypes.h:17
EIGEN_DEVICE_FUNC bool abs2(bool x)
std::ptrdiff_t j


gtsam
Author(s):
autogenerated on Tue Jul 4 2023 02:34:33