lmpar.h
1 namespace Eigen {
2 
3 namespace internal {
4 
5 template <typename Scalar>
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>
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