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