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>
qrsolv(Matrix<Scalar,Dynamic,Dynamic> & s,const VectorXi & ipvt,const Matrix<Scalar,Dynamic,1> & diag,const Matrix<Scalar,Dynamic,1> & qtb,Matrix<Scalar,Dynamic,1> & x,Matrix<Scalar,Dynamic,1> & sdiag)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