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
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
|
// This file is part of Eigen, a lightweight C++ template library
// for linear algebra.
//
// Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
// Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
//
// This code initially comes from MINPACK whose original authors are:
// Copyright Jorge More - Argonne National Laboratory
// Copyright Burt Garbow - Argonne National Laboratory
// Copyright Ken Hillstrom - Argonne National Laboratory
//
// This Source Code Form is subject to the terms of the Minpack license
// (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
#ifndef EIGEN_LMQRSOLV_H
#define EIGEN_LMQRSOLV_H
namespace Eigen {
namespace internal {
template <typename Scalar,int Rows, int Cols, typename Index>
void lmqrsolv(
Matrix<Scalar,Rows,Cols> &s,
const PermutationMatrix<Dynamic,Dynamic,Index> &iPerm,
const Matrix<Scalar,Dynamic,1> &diag,
const Matrix<Scalar,Dynamic,1> &qtb,
Matrix<Scalar,Dynamic,1> &x,
Matrix<Scalar,Dynamic,1> &sdiag)
{
/* 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 = iPerm.indices()(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. */
x = iPerm * wa;
}
template <typename Scalar, int _Options, typename Index>
void lmqrsolv(
SparseMatrix<Scalar,_Options,Index> &s,
const PermutationMatrix<Dynamic,Dynamic> &iPerm,
const Matrix<Scalar,Dynamic,1> &diag,
const Matrix<Scalar,Dynamic,1> &qtb,
Matrix<Scalar,Dynamic,1> &x,
Matrix<Scalar,Dynamic,1> &sdiag)
{
/* Local variables */
typedef SparseMatrix<Scalar,RowMajor,Index> FactorType;
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 R. */
wa = qtb;
FactorType R(s);
// 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 = iPerm.indices()(j);
if (diag(l) == Scalar(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;
// Browse the nonzero elements of row j of the upper triangular s
for (k = j; k < n; ++k)
{
typename FactorType::InnerIterator itk(R,k);
for (; itk; ++itk){
if (itk.index() < k) continue;
else break;
}
//At this point, we have the diagonal element R(k,k)
// Determine a givens rotation which eliminates
// the appropriate element in the current row of d
givens.makeGivens(-itk.value(), sdiag(k));
// Compute the modified diagonal element of r and
// the modified element of ((q transpose)*b,0).
itk.valueRef() = givens.c() * itk.value() + 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 transformation in the remaining k row/column of R
for (++itk; itk; ++itk)
{
i = itk.index();
temp = givens.c() * itk.value() + givens.s() * sdiag(i);
sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i);
itk.valueRef() = 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();
// x = wa;
wa.head(nsing) = R.topLeftCorner(nsing,nsing).template triangularView<Upper>().solve/*InPlace*/(wa.head(nsing));
sdiag = R.diagonal();
// Permute the components of z back to components of x
x = iPerm * wa;
}
} // end namespace internal
} // end namespace Eigen
#endif // EIGEN_LMQRSOLV_H
|
