![]() |
Eigen-unsupported
3.3.3
|
00001 namespace Eigen { 00002 00003 namespace internal { 00004 00005 // TODO : once qrsolv2 is removed, use ColPivHouseholderQR or PermutationMatrix instead of ipvt 00006 template <typename Scalar> 00007 void qrsolv( 00008 Matrix< Scalar, Dynamic, Dynamic > &s, 00009 // TODO : use a PermutationMatrix once lmpar is no more: 00010 const VectorXi &ipvt, 00011 const Matrix< Scalar, Dynamic, 1 > &diag, 00012 const Matrix< Scalar, Dynamic, 1 > &qtb, 00013 Matrix< Scalar, Dynamic, 1 > &x, 00014 Matrix< Scalar, Dynamic, 1 > &sdiag) 00015 00016 { 00017 typedef DenseIndex Index; 00018 00019 /* Local variables */ 00020 Index i, j, k, l; 00021 Scalar temp; 00022 Index n = s.cols(); 00023 Matrix< Scalar, Dynamic, 1 > wa(n); 00024 JacobiRotation<Scalar> givens; 00025 00026 /* Function Body */ 00027 // the following will only change the lower triangular part of s, including 00028 // the diagonal, though the diagonal is restored afterward 00029 00030 /* copy r and (q transpose)*b to preserve input and initialize s. */ 00031 /* in particular, save the diagonal elements of r in x. */ 00032 x = s.diagonal(); 00033 wa = qtb; 00034 00035 s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose(); 00036 00037 /* eliminate the diagonal matrix d using a givens rotation. */ 00038 for (j = 0; j < n; ++j) { 00039 00040 /* prepare the row of d to be eliminated, locating the */ 00041 /* diagonal element using p from the qr factorization. */ 00042 l = ipvt[j]; 00043 if (diag[l] == 0.) 00044 break; 00045 sdiag.tail(n-j).setZero(); 00046 sdiag[j] = diag[l]; 00047 00048 /* the transformations to eliminate the row of d */ 00049 /* modify only a single element of (q transpose)*b */ 00050 /* beyond the first n, which is initially zero. */ 00051 Scalar qtbpj = 0.; 00052 for (k = j; k < n; ++k) { 00053 /* determine a givens rotation which eliminates the */ 00054 /* appropriate element in the current row of d. */ 00055 givens.makeGivens(-s(k,k), sdiag[k]); 00056 00057 /* compute the modified diagonal element of r and */ 00058 /* the modified element of ((q transpose)*b,0). */ 00059 s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k]; 00060 temp = givens.c() * wa[k] + givens.s() * qtbpj; 00061 qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj; 00062 wa[k] = temp; 00063 00064 /* accumulate the tranformation in the row of s. */ 00065 for (i = k+1; i<n; ++i) { 00066 temp = givens.c() * s(i,k) + givens.s() * sdiag[i]; 00067 sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i]; 00068 s(i,k) = temp; 00069 } 00070 } 00071 } 00072 00073 /* solve the triangular system for z. if the system is */ 00074 /* singular, then obtain a least squares solution. */ 00075 Index nsing; 00076 for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {} 00077 00078 wa.tail(n-nsing).setZero(); 00079 s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing)); 00080 00081 // restore 00082 sdiag = s.diagonal(); 00083 s.diagonal() = x; 00084 00085 /* permute the components of z back to components of x. */ 00086 for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j]; 00087 } 00088 00089 } // end namespace internal 00090 00091 } // end namespace Eigen