Carlos Hernandez | 7faaa9f | 2014-08-05 17:53:32 -0700 | [diff] [blame] | 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 |