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 
15 namespace Eigen {
16 
17 namespace internal {
18 
19   template <typename QRSolver, typename VectorType>
lmpar2(const QRSolver & qr,const VectorType & diag,const VectorType & qtb,typename VectorType::Scalar m_delta,typename VectorType::Scalar & par,VectorType & x)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