![]() |
Eigen-unsupported
3.3.3
|
00001 // This file is part of Eigen, a lightweight C++ template library 00002 // for linear algebra. 00003 // 00004 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org> 00005 // 00006 // This code initially comes from MINPACK whose original authors are: 00007 // Copyright Jorge More - Argonne National Laboratory 00008 // Copyright Burt Garbow - Argonne National Laboratory 00009 // Copyright Ken Hillstrom - Argonne National Laboratory 00010 // 00011 // This Source Code Form is subject to the terms of the Minpack license 00012 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file. 00013 00014 #ifndef EIGEN_LMONESTEP_H 00015 #define EIGEN_LMONESTEP_H 00016 00017 namespace Eigen { 00018 00019 template<typename FunctorType> 00020 LevenbergMarquardtSpace::Status 00021 LevenbergMarquardt<FunctorType>::minimizeOneStep(FVectorType &x) 00022 { 00023 using std::abs; 00024 using std::sqrt; 00025 RealScalar temp, temp1,temp2; 00026 RealScalar ratio; 00027 RealScalar pnorm, xnorm, fnorm1, actred, dirder, prered; 00028 eigen_assert(x.size()==n); // check the caller is not cheating us 00029 00030 temp = 0.0; xnorm = 0.0; 00031 /* calculate the jacobian matrix. */ 00032 Index df_ret = m_functor.df(x, m_fjac); 00033 if (df_ret<0) 00034 return LevenbergMarquardtSpace::UserAsked; 00035 if (df_ret>0) 00036 // numerical diff, we evaluated the function df_ret times 00037 m_nfev += df_ret; 00038 else m_njev++; 00039 00040 /* compute the qr factorization of the jacobian. */ 00041 for (int j = 0; j < x.size(); ++j) 00042 m_wa2(j) = m_fjac.col(j).blueNorm(); 00043 QRSolver qrfac(m_fjac); 00044 if(qrfac.info() != Success) { 00045 m_info = NumericalIssue; 00046 return LevenbergMarquardtSpace::ImproperInputParameters; 00047 } 00048 // Make a copy of the first factor with the associated permutation 00049 m_rfactor = qrfac.matrixR(); 00050 m_permutation = (qrfac.colsPermutation()); 00051 00052 /* on the first iteration and if external scaling is not used, scale according */ 00053 /* to the norms of the columns of the initial jacobian. */ 00054 if (m_iter == 1) { 00055 if (!m_useExternalScaling) 00056 for (Index j = 0; j < n; ++j) 00057 m_diag[j] = (m_wa2[j]==0.)? 1. : m_wa2[j]; 00058 00059 /* on the first iteration, calculate the norm of the scaled x */ 00060 /* and initialize the step bound m_delta. */ 00061 xnorm = m_diag.cwiseProduct(x).stableNorm(); 00062 m_delta = m_factor * xnorm; 00063 if (m_delta == 0.) 00064 m_delta = m_factor; 00065 } 00066 00067 /* form (q transpose)*m_fvec and store the first n components in */ 00068 /* m_qtf. */ 00069 m_wa4 = m_fvec; 00070 m_wa4 = qrfac.matrixQ().adjoint() * m_fvec; 00071 m_qtf = m_wa4.head(n); 00072 00073 /* compute the norm of the scaled gradient. */ 00074 m_gnorm = 0.; 00075 if (m_fnorm != 0.) 00076 for (Index j = 0; j < n; ++j) 00077 if (m_wa2[m_permutation.indices()[j]] != 0.) 00078 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]])); 00079 00080 /* test for convergence of the gradient norm. */ 00081 if (m_gnorm <= m_gtol) { 00082 m_info = Success; 00083 return LevenbergMarquardtSpace::CosinusTooSmall; 00084 } 00085 00086 /* rescale if necessary. */ 00087 if (!m_useExternalScaling) 00088 m_diag = m_diag.cwiseMax(m_wa2); 00089 00090 do { 00091 /* determine the levenberg-marquardt parameter. */ 00092 internal::lmpar2(qrfac, m_diag, m_qtf, m_delta, m_par, m_wa1); 00093 00094 /* store the direction p and x + p. calculate the norm of p. */ 00095 m_wa1 = -m_wa1; 00096 m_wa2 = x + m_wa1; 00097 pnorm = m_diag.cwiseProduct(m_wa1).stableNorm(); 00098 00099 /* on the first iteration, adjust the initial step bound. */ 00100 if (m_iter == 1) 00101 m_delta = (std::min)(m_delta,pnorm); 00102 00103 /* evaluate the function at x + p and calculate its norm. */ 00104 if ( m_functor(m_wa2, m_wa4) < 0) 00105 return LevenbergMarquardtSpace::UserAsked; 00106 ++m_nfev; 00107 fnorm1 = m_wa4.stableNorm(); 00108 00109 /* compute the scaled actual reduction. */ 00110 actred = -1.; 00111 if (Scalar(.1) * fnorm1 < m_fnorm) 00112 actred = 1. - numext::abs2(fnorm1 / m_fnorm); 00113 00114 /* compute the scaled predicted reduction and */ 00115 /* the scaled directional derivative. */ 00116 m_wa3 = m_rfactor.template triangularView<Upper>() * (m_permutation.inverse() *m_wa1); 00117 temp1 = numext::abs2(m_wa3.stableNorm() / m_fnorm); 00118 temp2 = numext::abs2(sqrt(m_par) * pnorm / m_fnorm); 00119 prered = temp1 + temp2 / Scalar(.5); 00120 dirder = -(temp1 + temp2); 00121 00122 /* compute the ratio of the actual to the predicted */ 00123 /* reduction. */ 00124 ratio = 0.; 00125 if (prered != 0.) 00126 ratio = actred / prered; 00127 00128 /* update the step bound. */ 00129 if (ratio <= Scalar(.25)) { 00130 if (actred >= 0.) 00131 temp = RealScalar(.5); 00132 if (actred < 0.) 00133 temp = RealScalar(.5) * dirder / (dirder + RealScalar(.5) * actred); 00134 if (RealScalar(.1) * fnorm1 >= m_fnorm || temp < RealScalar(.1)) 00135 temp = Scalar(.1); 00136 /* Computing MIN */ 00137 m_delta = temp * (std::min)(m_delta, pnorm / RealScalar(.1)); 00138 m_par /= temp; 00139 } else if (!(m_par != 0. && ratio < RealScalar(.75))) { 00140 m_delta = pnorm / RealScalar(.5); 00141 m_par = RealScalar(.5) * m_par; 00142 } 00143 00144 /* test for successful iteration. */ 00145 if (ratio >= RealScalar(1e-4)) { 00146 /* successful iteration. update x, m_fvec, and their norms. */ 00147 x = m_wa2; 00148 m_wa2 = m_diag.cwiseProduct(x); 00149 m_fvec = m_wa4; 00150 xnorm = m_wa2.stableNorm(); 00151 m_fnorm = fnorm1; 00152 ++m_iter; 00153 } 00154 00155 /* tests for convergence. */ 00156 if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1. && m_delta <= m_xtol * xnorm) 00157 { 00158 m_info = Success; 00159 return LevenbergMarquardtSpace::RelativeErrorAndReductionTooSmall; 00160 } 00161 if (abs(actred) <= m_ftol && prered <= m_ftol && Scalar(.5) * ratio <= 1.) 00162 { 00163 m_info = Success; 00164 return LevenbergMarquardtSpace::RelativeReductionTooSmall; 00165 } 00166 if (m_delta <= m_xtol * xnorm) 00167 { 00168 m_info = Success; 00169 return LevenbergMarquardtSpace::RelativeErrorTooSmall; 00170 } 00171 00172 /* tests for termination and stringent tolerances. */ 00173 if (m_nfev >= m_maxfev) 00174 { 00175 m_info = NoConvergence; 00176 return LevenbergMarquardtSpace::TooManyFunctionEvaluation; 00177 } 00178 if (abs(actred) <= NumTraits<Scalar>::epsilon() && prered <= NumTraits<Scalar>::epsilon() && Scalar(.5) * ratio <= 1.) 00179 { 00180 m_info = Success; 00181 return LevenbergMarquardtSpace::FtolTooSmall; 00182 } 00183 if (m_delta <= NumTraits<Scalar>::epsilon() * xnorm) 00184 { 00185 m_info = Success; 00186 return LevenbergMarquardtSpace::XtolTooSmall; 00187 } 00188 if (m_gnorm <= NumTraits<Scalar>::epsilon()) 00189 { 00190 m_info = Success; 00191 return LevenbergMarquardtSpace::GtolTooSmall; 00192 } 00193 00194 } while (ratio < Scalar(1e-4)); 00195 00196 return LevenbergMarquardtSpace::Running; 00197 } 00198 00199 00200 } // end namespace Eigen 00201 00202 #endif // EIGEN_LMONESTEP_H