[136] | 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
|
---|