5 template <
typename Scalar>
7 const Matrix< Scalar, Dynamic, Dynamic > &qrfac,
8 const Matrix< Scalar, Dynamic, 1 > &diag,
9 const Matrix< Scalar, Dynamic, 1 > &qtb,
11 Matrix< Scalar, Dynamic, 1 > &x)
13 typedef DenseIndex Index;
17 Scalar sum, temp, alpha, bnorm;
22 const Scalar epsmch = NumTraits<Scalar>::epsilon();
23 const Index n = qrfac.cols();
24 assert(n==qtb.size());
26 assert(n==diag.size());
27 Matrix< Scalar, Dynamic, 1 > wa1(n), wa2(n);
30 for (j = n-1; j >=0; --j) {
33 temp = epsmch * qrfac.col(j).head(j+1).maxCoeff();
40 x[j] = (qtb[j] - qrfac.row(j).tail(n-j-1).dot(x.tail(n-j-1))) / temp;
44 qnorm = diag.cwiseProduct(x).stableNorm();
54 for (j = 0; j < n; ++j) {
55 wa1.tail(n-j) += qrfac.row(j).tail(n-j) * qtb[j];
61 gnorm = wa1.stableNorm();
63 alpha = delta / qnorm;
69 wa1.array() /= (diag*gnorm).array();
72 for (j = 0; j < n; ++j) {
74 for (i = j; i < n; ++i) {
75 sum += qrfac(j,i) * wa1[i];
79 temp = wa2.stableNorm();
80 sgnorm = gnorm / temp / temp;
90 bnorm = qtb.stableNorm();
91 temp = bnorm / gnorm * (bnorm / qnorm) * (sgnorm / delta);
92 temp = temp - delta / qnorm * abs2(sgnorm / delta) + sqrt(abs2(temp - delta / qnorm) + (1.-abs2(delta / qnorm)) * (1.-abs2(sgnorm / delta)));
93 alpha = delta / qnorm * (1. - abs2(sgnorm / delta)) / temp;
98 temp = (1.-alpha) * (std::min)(sgnorm,delta);
99 x = temp * wa1 + alpha * x;