src/LevenbergMarquardt/LMpar.h

0001 // This file is part of Eigen, a lightweight C++ template library
0002 // for linear algebra.
0003 //
0004 // This code initially comes from MINPACK whose original authors are:
0005 // Copyright Jorge More - Argonne National Laboratory
0006 // Copyright Burt Garbow - Argonne National Laboratory
0007 // Copyright Ken Hillstrom - Argonne National Laboratory
0008 //
0009 // This Source Code Form is subject to the terms of the Minpack license
0010 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
0011
0012 #ifndef EIGEN_LMPAR_H
0013 #define EIGEN_LMPAR_H
0014
0015 namespace Eigen {
0016
0017 namespace internal {
0018
0019   template <typename QRSolver, typename VectorType>
0020     void lmpar2(
0021     const QRSolver &qr,
0022     const VectorType  &diag,
0023     const VectorType  &qtb,
0024     typename VectorType::Scalar m_delta,
0025     typename VectorType::Scalar &par,
0026     VectorType  &x)
0027
0028   {
0029     using std::sqrt;
0030     using std::abs;
0031     typedef typename QRSolver::MatrixType MatrixType;
0032     typedef typename QRSolver::Scalar Scalar;
0033 //    typedef typename QRSolver::StorageIndex StorageIndex;
0034
0035     /* Local variables */
0036     Index j;
0037     Scalar fp;
0038     Scalar parc, parl;
0039     Index iter;
0040     Scalar temp, paru;
0041     Scalar gnorm;
0042     Scalar dxnorm;
0043
0044     // Make a copy of the triangular factor.
0045     // This copy is modified during call the qrsolv
0046     MatrixType s;
0047     s = qr.matrixR();
0048
0049     /* Function Body */
0050     const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
0051     const Index n = qr.matrixR().cols();
0052     eigen_assert(n==diag.size());
0053     eigen_assert(n==qtb.size());
0054
0055     VectorType  wa1, wa2;
0056
0057     /* compute and store in x the gauss-newton direction. if the */
0058     /* jacobian is rank-deficient, obtain a least squares solution. */
0059
0060     //    const Index rank = qr.nonzeroPivots(); // exactly double(0.)
0061     const Index rank = qr.rank(); // use a threshold
0062     wa1 = qtb;
0063     wa1.tail(n-rank).setZero();
0064     //FIXME There is no solve in place for sparse triangularView
0065     wa1.head(rank) = s.topLeftCorner(rank,rank).template triangularView<Upper>().solve(qtb.head(rank));
0066
0067     x = qr.colsPermutation()*wa1;
0068
0069     /* initialize the iteration counter. */
0070     /* evaluate the function at the origin, and test */
0071     /* for acceptance of the gauss-newton direction. */
0072     iter = 0;
0073     wa2 = diag.cwiseProduct(x);
0074     dxnorm = wa2.blueNorm();
0075     fp = dxnorm - m_delta;
0076     if (fp <= Scalar(0.1) * m_delta) {
0077       par = 0;
0078       return;
0079     }
0080
0081     /* if the jacobian is not rank deficient, the newton */
0082     /* step provides a lower bound, parl, for the zero of */
0083     /* the function. otherwise set this bound to zero. */
0084     parl = 0.;
0085     if (rank==n) {
0086       wa1 = qr.colsPermutation().inverse() *  diag.cwiseProduct(wa2)/dxnorm;
0087       s.topLeftCorner(n,n).transpose().template triangularView<Lower>().solveInPlace(wa1);
0088       temp = wa1.blueNorm();
0089       parl = fp / m_delta / temp / temp;
0090     }
0091
0092     /* calculate an upper bound, paru, for the zero of the function. */
0093     for (j = 0; j < n; ++j)
0094       wa1[j] = s.col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)];
0095
0096     gnorm = wa1.stableNorm();
0097     paru = gnorm / m_delta;
0098     if (paru == 0.)
0099       paru = dwarf / (std::min)(m_delta,Scalar(0.1));
0100
0101     /* if the input par lies outside of the interval (parl,paru), */
0102     /* set par to the closer endpoint. */
0103     par = (std::max)(par,parl);
0104     par = (std::min)(par,paru);
0105     if (par == 0.)
0106       par = gnorm / dxnorm;
0107
0108     /* beginning of an iteration. */
0109     while (true) {
0110       ++iter;
0111
0112       /* evaluate the function at the current value of par. */
0113       if (par == 0.)
0114         par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */
0115       wa1 = sqrt(par)* diag;
0116
0117       VectorType sdiag(n);
0118       lmqrsolv(s, qr.colsPermutation(), wa1, qtb, x, sdiag);
0119
0120       wa2 = diag.cwiseProduct(x);
0121       dxnorm = wa2.blueNorm();
0122       temp = fp;
0123       fp = dxnorm - m_delta;
0124
0125       /* if the function is small enough, accept the current value */
0126       /* of par. also test for the exceptional cases where parl */
0127       /* is zero or the number of iterations has reached 10. */
0128       if (abs(fp) <= Scalar(0.1) * m_delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
0129         break;
0130
0131       /* compute the newton correction. */
0132       wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm);
0133       // we could almost use this here, but the diagonal is outside qr, in sdiag[]
0134       for (j = 0; j < n; ++j) {
0135         wa1[j] /= sdiag[j];
0136         temp = wa1[j];
0137         for (Index i = j+1; i < n; ++i)
0138           wa1[i] -= s.coeff(i,j) * temp;
0139       }
0140       temp = wa1.blueNorm();
0141       parc = fp / m_delta / temp / temp;
0142
0143       /* depending on the sign of the function, update parl or paru. */
0144       if (fp > 0.)
0145         parl = (std::max)(parl,par);
0146       if (fp < 0.)
0147         paru = (std::min)(paru,par);
0148
0149       /* compute an improved estimate for par. */
0150       par = (std::max)(parl,par+parc);
0151     }
0152     if (iter == 0)
0153       par = 0.;
0154     return;
0155   }
0156 } // end namespace internal
0157
0158 } // end namespace Eigen
0159
0160 #endif // EIGEN_LMPAR_H