← Back to branch summary

~yade-dev/yade/eigen3

« back to all changes in this revision

Viewing changes to unsupported/Eigen/src/NonLinearOptimization/lmpar.h

  • Committer: Anton Gladky
  • Date: 2011-12-06 16:25:13 UTC
  • Revision ID: gladky.anton@gmail.com-20111206162513-pnl0n3q3yurpg9p4
Initial import of eigen3, version 3.0.4

Show diffs side-by-side

added added

removed removed

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