1 namespace Eigen { 2 3 namespace internal { 4 5 // TODO : once qrsolv2 is removed, use ColPivHouseholderQR or PermutationMatrix instead of ipvt 6 template <typename Scalar> 7 void qrsolv( 8 Matrix< Scalar, Dynamic, Dynamic > &s, 9 // TODO : use a PermutationMatrix once lmpar is no more: 10 const VectorXi &ipvt, 11 const Matrix< Scalar, Dynamic, 1 > &diag, 12 const Matrix< Scalar, Dynamic, 1 > &qtb, 13 Matrix< Scalar, Dynamic, 1 > &x, 14 Matrix< Scalar, Dynamic, 1 > &sdiag) 15 16 { 17 typedef DenseIndex Index; 18 19 /* Local variables */ 20 Index i, j, k, l; 21 Scalar temp; 22 Index n = s.cols(); 23 Matrix< Scalar, Dynamic, 1 > wa(n); 24 JacobiRotation<Scalar> givens; 25 26 /* Function Body */ 27 // the following will only change the lower triangular part of s, including 28 // the diagonal, though the diagonal is restored afterward 29 30 /* copy r and (q transpose)*b to preserve input and initialize s. */ 31 /* in particular, save the diagonal elements of r in x. */ 32 x = s.diagonal(); 33 wa = qtb; 34 35 s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose(); 36 37 /* eliminate the diagonal matrix d using a givens rotation. */ 38 for (j = 0; j < n; ++j) { 39 40 /* prepare the row of d to be eliminated, locating the */ 41 /* diagonal element using p from the qr factorization. */ 42 l = ipvt[j]; 43 if (diag[l] == 0.) 44 break; 45 sdiag.tail(n-j).setZero(); 46 sdiag[j] = diag[l]; 47 48 /* the transformations to eliminate the row of d */ 49 /* modify only a single element of (q transpose)*b */ 50 /* beyond the first n, which is initially zero. */ 51 Scalar qtbpj = 0.; 52 for (k = j; k < n; ++k) { 53 /* determine a givens rotation which eliminates the */ 54 /* appropriate element in the current row of d. */ 55 givens.makeGivens(-s(k,k), sdiag[k]); 56 57 /* compute the modified diagonal element of r and */ 58 /* the modified element of ((q transpose)*b,0). */ 59 s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k]; 60 temp = givens.c() * wa[k] + givens.s() * qtbpj; 61 qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj; 62 wa[k] = temp; 63 64 /* accumulate the tranformation in the row of s. */ 65 for (i = k+1; i<n; ++i) { 66 temp = givens.c() * s(i,k) + givens.s() * sdiag[i]; 67 sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i]; 68 s(i,k) = temp; 69 } 70 } 71 } 72 73 /* solve the triangular system for z. if the system is */ 74 /* singular, then obtain a least squares solution. */ 75 Index nsing; 76 for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {} 77 78 wa.tail(n-nsing).setZero(); 79 s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing)); 80 81 // restore 82 sdiag = s.diagonal(); 83 s.diagonal() = x; 84 85 /* permute the components of z back to components of x. */ 86 for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j]; 87 } 88 89 } // end namespace internal 90 91 } // end namespace Eigen 92
