lmpar.h
Go to the documentation of this file.
1 namespace Eigen {
2 
3 namespace internal {
4 
5 template <typename Scalar>
6 void lmpar(
8  const VectorXi &ipvt,
11  Scalar delta,
12  Scalar &par,
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 
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 
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>
164 void lmpar2(
166  const Matrix< Scalar, Dynamic, 1 > &diag,
167  const Matrix< Scalar, Dynamic, 1 > &qtb,
168  Scalar delta,
169  Scalar &par,
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 
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 
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
IntermediateState sqrt(const Expression &arg)
iterative scaling algorithm to equilibrate rows and column norms in matrices
Definition: matrix.hpp:471
EIGEN_STRONG_INLINE const CwiseUnaryOp< internal::scalar_abs_op< Scalar >, const Derived > abs() const
Householder rank-revealing QR decomposition of a matrix with column-pivoting.
Derived & setZero(Index size)
EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex
Definition: XprHelper.h:27
static Parameters par
void 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)
Definition: lmpar.h:164
void 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)
Definition: lmpar.h:6
EIGEN_STRONG_INLINE Index cols() const
#define eigen_assert(x)


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:34:48