lmpar.h revision 7faaa9f3f0df9d23790277834d426c3d992ac3ba 
1namespace Eigen {
2
3namespace internal {
4
5template <typename Scalar>
6void lmpar(
7        Matrix< Scalar, Dynamic, Dynamic > &r,
8        const VectorXi &ipvt,
9        const Matrix< Scalar, Dynamic, 1 >  &diag,
10        const Matrix< Scalar, Dynamic, 1 >  &qtb,
11        Scalar delta,
12        Scalar &par,
13        Matrix< Scalar, Dynamic, 1 >  &x)
14{
15    using std::abs;
16    using std::sqrt;
17    typedef DenseIndex Index;
18
19    /* Local variables */
20    Index i, j, l;
21    Scalar fp;
22    Scalar parc, parl;
23    Index iter;
24    Scalar temp, paru;
25    Scalar gnorm;
26    Scalar dxnorm;
27
28
29    /* Function Body */
30    const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
31    const Index n = r.cols();
32    eigen_assert(n==diag.size());
33    eigen_assert(n==qtb.size());
34    eigen_assert(n==x.size());
35
36    Matrix< Scalar, Dynamic, 1 >  wa1, wa2;
37
38    /* compute and store in x the gauss-newton direction. if the */
39    /* jacobian is rank-deficient, obtain a least squares solution. */
40    Index nsing = n-1;
41    wa1 = qtb;
42    for (j = 0; j < n; ++j) {
43        if (r(j,j) == 0. && nsing == n-1)
44            nsing = j - 1;
45        if (nsing < n-1)
46            wa1[j] = 0.;
47    }
48    for (j = nsing; j>=0; --j) {
49        wa1[j] /= r(j,j);
50        temp = wa1[j];
51        for (i = 0; i < j ; ++i)
52            wa1[i] -= r(i,j) * temp;
53    }
54
55    for (j = 0; j < n; ++j)
56        x[ipvt[j]] = wa1[j];
57
58    /* initialize the iteration counter. */
59    /* evaluate the function at the origin, and test */
60    /* for acceptance of the gauss-newton direction. */
61    iter = 0;
62    wa2 = diag.cwiseProduct(x);
63    dxnorm = wa2.blueNorm();
64    fp = dxnorm - delta;
65    if (fp <= Scalar(0.1) * delta) {
66        par = 0;
67        return;
68    }
69
70    /* if the jacobian is not rank deficient, the newton */
71    /* step provides a lower bound, parl, for the zero of */
72    /* the function. otherwise set this bound to zero. */
73    parl = 0.;
74    if (nsing >= n-1) {
75        for (j = 0; j < n; ++j) {
76            l = ipvt[j];
77            wa1[j] = diag[l] * (wa2[l] / dxnorm);
78        }
79        // it's actually a triangularView.solveInplace(), though in a weird
80        // way:
81        for (j = 0; j < n; ++j) {
82            Scalar sum = 0.;
83            for (i = 0; i < j; ++i)
84                sum += r(i,j) * wa1[i];
85            wa1[j] = (wa1[j] - sum) / r(j,j);
86        }
87        temp = wa1.blueNorm();
88        parl = fp / delta / temp / temp;
89    }
90
91    /* calculate an upper bound, paru, for the zero of the function. */
92    for (j = 0; j < n; ++j)
93        wa1[j] = r.col(j).head(j+1).dot(qtb.head(j+1)) / diag[ipvt[j]];
94
95    gnorm = wa1.stableNorm();
96    paru = gnorm / delta;
97    if (paru == 0.)
98        paru = dwarf / (std::min)(delta,Scalar(0.1));
99
100    /* if the input par lies outside of the interval (parl,paru), */
101    /* set par to the closer endpoint. */
102    par = (std::max)(par,parl);
103    par = (std::min)(par,paru);
104    if (par == 0.)
105        par = gnorm / dxnorm;
106
107    /* beginning of an iteration. */
108    while (true) {
109        ++iter;
110
111        /* evaluate the function at the current value of par. */
112        if (par == 0.)
113            par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */
114        wa1 = sqrt(par)* diag;
115
116        Matrix< Scalar, Dynamic, 1 > sdiag(n);
117        qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag);
118
119        wa2 = diag.cwiseProduct(x);
120        dxnorm = wa2.blueNorm();
121        temp = fp;
122        fp = dxnorm - delta;
123
124        /* if the function is small enough, accept the current value */
125        /* of par. also test for the exceptional cases where parl */
126        /* is zero or the number of iterations has reached 10. */
127        if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
128            break;
129
130        /* compute the newton correction. */
131        for (j = 0; j < n; ++j) {
132            l = ipvt[j];
133            wa1[j] = diag[l] * (wa2[l] / dxnorm);
134        }
135        for (j = 0; j < n; ++j) {
136            wa1[j] /= sdiag[j];
137            temp = wa1[j];
138            for (i = j+1; i < n; ++i)
139                wa1[i] -= r(i,j) * temp;
140        }
141        temp = wa1.blueNorm();
142        parc = fp / delta / temp / temp;
143
144        /* depending on the sign of the function, update parl or paru. */
145        if (fp > 0.)
146            parl = (std::max)(parl,par);
147        if (fp < 0.)
148            paru = (std::min)(paru,par);
149
150        /* compute an improved estimate for par. */
151        /* Computing MAX */
152        par = (std::max)(parl,par+parc);
153
154        /* end of an iteration. */
155    }
156
157    /* termination. */
158    if (iter == 0)
159        par = 0.;
160    return;
161}
162
163template <typename Scalar>
164void lmpar2(
165        const ColPivHouseholderQR<Matrix< Scalar, Dynamic, Dynamic> > &qr,
166        const Matrix< Scalar, Dynamic, 1 >  &diag,
167        const Matrix< Scalar, Dynamic, 1 >  &qtb,
168        Scalar delta,
169        Scalar &par,
170        Matrix< Scalar, Dynamic, 1 >  &x)
171
172{
173    using std::sqrt;
174    using std::abs;
175    typedef DenseIndex Index;
176
177    /* Local variables */
178    Index j;
179    Scalar fp;
180    Scalar parc, parl;
181    Index iter;
182    Scalar temp, paru;
183    Scalar gnorm;
184    Scalar dxnorm;
185
186
187    /* Function Body */
188    const Scalar dwarf = (std::numeric_limits<Scalar>::min)();
189    const Index n = qr.matrixQR().cols();
190    eigen_assert(n==diag.size());
191    eigen_assert(n==qtb.size());
192
193    Matrix< Scalar, Dynamic, 1 >  wa1, wa2;
194
195    /* compute and store in x the gauss-newton direction. if the */
196    /* jacobian is rank-deficient, obtain a least squares solution. */
197
198//    const Index rank = qr.nonzeroPivots(); // exactly double(0.)
199    const Index rank = qr.rank(); // use a threshold
200    wa1 = qtb;
201    wa1.tail(n-rank).setZero();
202    qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank));
203
204    x = qr.colsPermutation()*wa1;
205
206    /* initialize the iteration counter. */
207    /* evaluate the function at the origin, and test */
208    /* for acceptance of the gauss-newton direction. */
209    iter = 0;
210    wa2 = diag.cwiseProduct(x);
211    dxnorm = wa2.blueNorm();
212    fp = dxnorm - delta;
213    if (fp <= Scalar(0.1) * delta) {
214        par = 0;
215        return;
216    }
217
218    /* if the jacobian is not rank deficient, the newton */
219    /* step provides a lower bound, parl, for the zero of */
220    /* the function. otherwise set this bound to zero. */
221    parl = 0.;
222    if (rank==n) {
223        wa1 = qr.colsPermutation().inverse() *  diag.cwiseProduct(wa2)/dxnorm;
224        qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
225        temp = wa1.blueNorm();
226        parl = fp / delta / temp / temp;
227    }
228
229    /* calculate an upper bound, paru, for the zero of the function. */
230    for (j = 0; j < n; ++j)
231        wa1[j] = qr.matrixQR().col(j).head(j+1).dot(qtb.head(j+1)) / diag[qr.colsPermutation().indices()(j)];
232
233    gnorm = wa1.stableNorm();
234    paru = gnorm / delta;
235    if (paru == 0.)
236        paru = dwarf / (std::min)(delta,Scalar(0.1));
237
238    /* if the input par lies outside of the interval (parl,paru), */
239    /* set par to the closer endpoint. */
240    par = (std::max)(par,parl);
241    par = (std::min)(par,paru);
242    if (par == 0.)
243        par = gnorm / dxnorm;
244
245    /* beginning of an iteration. */
246    Matrix< Scalar, Dynamic, Dynamic > s = qr.matrixQR();
247    while (true) {
248        ++iter;
249
250        /* evaluate the function at the current value of par. */
251        if (par == 0.)
252            par = (std::max)(dwarf,Scalar(.001) * paru); /* Computing MAX */
253        wa1 = sqrt(par)* diag;
254
255        Matrix< Scalar, Dynamic, 1 > sdiag(n);
256        qrsolv<Scalar>(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag);
257
258        wa2 = diag.cwiseProduct(x);
259        dxnorm = wa2.blueNorm();
260        temp = fp;
261        fp = dxnorm - delta;
262
263        /* if the function is small enough, accept the current value */
264        /* of par. also test for the exceptional cases where parl */
265        /* is zero or the number of iterations has reached 10. */
266        if (abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
267            break;
268
269        /* compute the newton correction. */
270        wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm);
271        // we could almost use this here, but the diagonal is outside qr, in sdiag[]
272        // qr.matrixQR().topLeftCorner(n, n).transpose().template triangularView<Lower>().solveInPlace(wa1);
273        for (j = 0; j < n; ++j) {
274            wa1[j] /= sdiag[j];
275            temp = wa1[j];
276            for (Index i = j+1; i < n; ++i)
277                wa1[i] -= s(i,j) * temp;
278        }
279        temp = wa1.blueNorm();
280        parc = fp / delta / temp / temp;
281
282        /* depending on the sign of the function, update parl or paru. */
283        if (fp > 0.)
284            parl = (std::max)(parl,par);
285        if (fp < 0.)
286            paru = (std::min)(paru,par);
287
288        /* compute an improved estimate for par. */
289        par = (std::max)(parl,par+parc);
290    }
291    if (iter == 0)
292        par = 0.;
293    return;
294}
295
296} // end namespace internal
297
298} // end namespace Eigen
299