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