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