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
92
|
template <typename Scalar>
void ei_qrsolv(
Matrix< Scalar, Dynamic, Dynamic > &r,
VectorXi &ipvt, // TODO : const once ipvt mess fixed
const Matrix< Scalar, Dynamic, 1 > &diag,
const Matrix< Scalar, Dynamic, 1 > &qtb,
Matrix< Scalar, Dynamic, 1 > &x,
Matrix< Scalar, Dynamic, 1 > &sdiag)
{
/* Local variables */
int i, j, k, l;
Scalar sum, temp;
int n = r.cols();
Matrix< Scalar, Dynamic, 1 > wa(n);
/* Function Body */
/* copy r and (q transpose)*b to preserve input and initialize s. */
/* in particular, save the diagonal elements of r in x. */
x = r.diagonal();
wa = qtb;
for (j = 0; j < n; ++j)
for (i = j+1; i < n; ++i)
r(i,j) = r(j,i);
/* 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.segment(j,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. */
PlanarRotation<Scalar> givens;
givens.makeGivens(-r(k,k), sdiag[k]);
/* compute the modified diagonal element of r and */
/* the modified element of ((q transpose)*b,0). */
r(k,k) = givens.c() * r(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() * r(i,k) + givens.s() * sdiag[i];
sdiag[i] = -givens.s() * r(i,k) + givens.c() * sdiag[i];
r(i,k) = temp;
}
}
}
// restore
sdiag = r.diagonal();
r.diagonal() = x;
/* solve the triangular system for z. if the system is */
/* singular, then obtain a least squares solution. */
int nsing;
for (nsing=0; nsing<n && sdiag[nsing]!=0; nsing++);
wa.segment(nsing,n-nsing).setZero();
nsing--; // nsing is the last nonsingular index
for (j = nsing; j>=0; j--) {
sum = 0.;
for (i = j+1; i <= nsing; ++i)
sum += r(i,j) * wa[i];
wa[j] = (wa[j] - sum) / sdiag[j];
}
/* permute the components of z back to components of x. */
for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j];
}
|