5 template <
typename Scalar>
7 Matrix< Scalar, Dynamic, Dynamic > &r,
9 const Matrix< Scalar, Dynamic, 1 > &diag,
10 const Matrix< Scalar, Dynamic, 1 > &qtb,
13 Matrix< Scalar, Dynamic, 1 > &x)
15 typedef DenseIndex Index;
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());
34 Matrix< Scalar, Dynamic, 1 > wa1, wa2;
40 for (j = 0; j < n; ++j) {
41 if (r(j,j) == 0. && nsing == n-1)
46 for (j = nsing; j>=0; --j) {
49 for (i = 0; i < j ; ++i)
50 wa1[i] -= r(i,j) * temp;
53 for (j = 0; j < n; ++j)
60 wa2 = diag.cwiseProduct(x);
61 dxnorm = wa2.blueNorm();
63 if (fp <= Scalar(0.1) * delta) {
73 for (j = 0; j < n; ++j) {
75 wa1[j] = diag[l] * (wa2[l] / dxnorm);
79 for (j = 0; j < n; ++j) {
81 for (i = 0; i < j; ++i)
82 sum += r(i,j) * wa1[i];
83 wa1[j] = (wa1[j] - sum) / r(j,j);
85 temp = wa1.blueNorm();
86 parl = fp / delta / temp / temp;
90 for (j = 0; j < n; ++j)
91 wa1[j] = r.col(j).head(j+1).dot(qtb.head(j+1)) / diag[ipvt[j]];
93 gnorm = wa1.stableNorm();
96 paru = dwarf / (std::min)(delta,Scalar(0.1));
100 par = (std::max)(par,parl);
101 par = (std::min)(par,paru);
103 par = gnorm / dxnorm;
111 par = (std::max)(dwarf,Scalar(.001) * paru);
112 wa1 =
sqrt(par)* diag;
114 Matrix< Scalar, Dynamic, 1 > sdiag(n);
115 qrsolv<Scalar>(r, ipvt, wa1, qtb, x, sdiag);
117 wa2 = diag.cwiseProduct(x);
118 dxnorm = wa2.blueNorm();
125 if (
abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
129 for (j = 0; j < n; ++j) {
131 wa1[j] = diag[l] * (wa2[l] / dxnorm);
133 for (j = 0; j < n; ++j) {
136 for (i = j+1; i < n; ++i)
137 wa1[i] -= r(i,j) * temp;
139 temp = wa1.blueNorm();
140 parc = fp / delta / temp / temp;
144 parl = (std::max)(parl,par);
146 paru = (std::min)(paru,par);
150 par = (std::max)(parl,par+parc);
161 template <
typename Scalar>
163 const ColPivHouseholderQR<Matrix< Scalar, Dynamic, Dynamic> > &qr,
164 const Matrix< Scalar, Dynamic, 1 > &diag,
165 const Matrix< Scalar, Dynamic, 1 > &qtb,
168 Matrix< Scalar, Dynamic, 1 > &x)
171 typedef DenseIndex Index;
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());
189 Matrix< Scalar, Dynamic, 1 > wa1, wa2;
195 const Index rank = qr.rank();
197 wa1.tail(n-rank).setZero();
198 qr.matrixQR().topLeftCorner(rank, rank).template triangularView<Upper>().solveInPlace(wa1.head(rank));
200 x = qr.colsPermutation()*wa1;
206 wa2 = diag.cwiseProduct(x);
207 dxnorm = wa2.blueNorm();
209 if (fp <= Scalar(0.1) * delta) {
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;
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)];
229 gnorm = wa1.stableNorm();
230 paru = gnorm / delta;
232 paru = dwarf / (std::min)(delta,Scalar(0.1));
236 par = (std::max)(par,parl);
237 par = (std::min)(par,paru);
239 par = gnorm / dxnorm;
242 Matrix< Scalar, Dynamic, Dynamic > s = qr.matrixQR();
248 par = (std::max)(dwarf,Scalar(.001) * paru);
249 wa1 =
sqrt(par)* diag;
251 Matrix< Scalar, Dynamic, 1 > sdiag(n);
252 qrsolv<Scalar>(s, qr.colsPermutation().indices(), wa1, qtb, x, sdiag);
254 wa2 = diag.cwiseProduct(x);
255 dxnorm = wa2.blueNorm();
262 if (
abs(fp) <= Scalar(0.1) * delta || (parl == 0. && fp <= temp && temp < 0.) || iter == 10)
266 wa1 = qr.colsPermutation().inverse() * diag.cwiseProduct(wa2/dxnorm);
269 for (j = 0; j < n; ++j) {
272 for (Index i = j+1; i < n; ++i)
273 wa1[i] -= s(i,j) * temp;
275 temp = wa1.blueNorm();
276 parc = fp / delta / temp / temp;
280 parl = (std::max)(parl,par);
282 paru = (std::min)(paru,par);
285 par = (std::max)(parl,par+parc);