1// This file is part of Eigen, a lightweight C++ template library
2// for linear algebra.
3//
4// This code initially comes from MINPACK whose original authors are:
5// Copyright Jorge More - Argonne National Laboratory
6// Copyright Burt Garbow - Argonne National Laboratory
7// Copyright Ken Hillstrom - Argonne National Laboratory
8//
9// This Source Code Form is subject to the terms of the Minpack license
10// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
11
12#ifndef EIGEN_LMPAR_H
13#define EIGEN_LMPAR_H
14
15namespace Eigen {
16
17namespace internal {
18
19  template <typename QRSolver, typename VectorType>
20    void lmpar2(
21    const QRSolver &qr,
22    const VectorType  &diag,
23    const VectorType  &qtb,
24    typename VectorType::Scalar m_delta,
25    typename VectorType::Scalar &par,
26    VectorType  &x)
27
28  {
29    using std::sqrt;
30    using std::abs;
31    typedef typename QRSolver::MatrixType MatrixType;
32    typedef typename QRSolver::Scalar Scalar;
33//    typedef typename QRSolver::StorageIndex StorageIndex;
34
35    /* Local variables */
36    Index j;
37    Scalar fp;
38    Scalar parc, parl;
39    Index iter;
40    Scalar temp, paru;
41    Scalar gnorm;
42    Scalar dxnorm;
43
44    // Make a copy of the triangular factor.
45    // This copy is modified during call the qrsolv
46    MatrixType s;
47    s = qr.matrixR();
48
49    /* Function Body */
50    const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
51    const Index n = qr.matrixR().cols();
52    eigen_assert(n==diag.size());
53    eigen_assert(n==qtb.size());
54
55    VectorType  wa1, wa2;
56
57    /* compute and store in x the gauss-newton direction. if the */
58    /* jacobian is rank-deficient, obtain a least squares solution. */
59
60    //    const Index rank = qr.nonzeroPivots(); // exactly double(0.)
61    const Index rank = qr.rank(); // use a threshold
62    wa1 = qtb;
63    wa1.tail(n-rank).setZero();
64    //FIXME There is no solve in place for sparse triangularView
65    wa1.head(rank) = s.topLeftCorner(rank,rank).template triangularView<Upper>().solve(qtb.head(rank));
66
67    x = qr.colsPermutation()*wa1;
68
69    /* initialize the iteration counter. */
70    /* evaluate the function at the origin, and test */
71    /* for acceptance of the gauss-newton direction. */
72    iter = 0;
73    wa2 = diag.cwiseProduct(x);
74    dxnorm = wa2.blueNorm();
75    fp = dxnorm - m_delta;
76    if (fp <= Scalar(0.1) * m_delta) {
77      par = 0;
78      return;
79    }
80
81    /* if the jacobian is not rank deficient, the newton */
82    /* step provides a lower bound, parl, for the zero of */
83    /* the function. otherwise set this bound to zero. */
84    parl = 0.;
85    if (rank==n) {
86      wa1 = qr.colsPermutation().inverse() *  diag.cwiseProduct(wa2)/dxnorm;
87      s.topLeftCorner(n,n).transpose().template triangularView<Lower>().solveInPlace(wa1);
88      temp = wa1.blueNorm();
89      parl = fp / m_delta / temp / temp;
90    }
91
92    /* calculate an upper bound, paru, for the zero of the function. */
93    for (j = 0; j < n; ++j)
94      wa1[j] = s.col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)];
95
96    gnorm = wa1.stableNorm();
97    paru = gnorm / m_delta;
98    if (paru == 0.)
99      paru = dwarf / (std::min)(m_delta,Scalar(0.1));
100
101    /* if the input par lies outside of the interval (parl,paru), */
102    /* set par to the closer endpoint. */
103    par = (std::max)(par,parl);
104    par = (std::min)(par,paru);
105    if (par == 0.)
106      par = gnorm / dxnorm;
107
108    /* beginning of an iteration. */
109    while (true) {
110      ++iter;
111
112      /* evaluate the function at the current value of par. */
113      if (par == 0.)
114        par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */
115      wa1 = sqrt(par)* diag;
116
117      VectorType sdiag(n);
118      lmqrsolv(s, qr.colsPermutation(), wa1, qtb, x, sdiag);
119
120      wa2 = diag.cwiseProduct(x);
121      dxnorm = wa2.blueNorm();
122      temp = fp;
123      fp = dxnorm - m_delta;
124
125      /* if the function is small enough, accept the current value */
126      /* of par. also test for the exceptional cases where parl */
127      /* is zero or the number of iterations has reached 10. */
128      if (abs(fp) <= Scalar(0.1) * m_delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
129        break;
130
131      /* compute the newton correction. */
132      wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm);
133      // we could almost use this here, but the diagonal is outside qr, in sdiag[]
134      for (j = 0; j < n; ++j) {
135        wa1[j] /= sdiag[j];
136        temp = wa1[j];
137        for (Index i = j+1; i < n; ++i)
138          wa1[i] -= s.coeff(i,j) * temp;
139      }
140      temp = wa1.blueNorm();
141      parc = fp / m_delta / temp / temp;
142
143      /* depending on the sign of the function, update parl or paru. */
144      if (fp > 0.)
145        parl = (std::max)(parl,par);
146      if (fp < 0.)
147        paru = (std::min)(paru,par);
148
149      /* compute an improved estimate for par. */
150      par = (std::max)(parl,par+parc);
151    }
152    if (iter == 0)
153      par = 0.;
154    return;
155  }
156} // end namespace internal
157
158} // end namespace Eigen
159
160#endif // EIGEN_LMPAR_H
161