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 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
163 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)164 void 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