1 // This file is part of Eigen, a lightweight C++ template library
2 // for linear algebra.
3 //
4 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
5 // Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
6 //
7 // This code initially comes from MINPACK whose original authors are:
8 // Copyright Jorge More - Argonne National Laboratory
9 // Copyright Burt Garbow - Argonne National Laboratory
10 // Copyright Ken Hillstrom - Argonne National Laboratory
11 //
12 // This Source Code Form is subject to the terms of the Minpack license
13 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
14
15 #ifndef EIGEN_LMQRSOLV_H
16 #define EIGEN_LMQRSOLV_H
17
18 namespace Eigen {
19
20 namespace internal {
21
22 template <typename Scalar,int Rows, int Cols, typename PermIndex>
lmqrsolv(Matrix<Scalar,Rows,Cols> & s,const PermutationMatrix<Dynamic,Dynamic,PermIndex> & iPerm,const Matrix<Scalar,Dynamic,1> & diag,const Matrix<Scalar,Dynamic,1> & qtb,Matrix<Scalar,Dynamic,1> & x,Matrix<Scalar,Dynamic,1> & sdiag)23 void lmqrsolv(
24 Matrix<Scalar,Rows,Cols> &s,
25 const PermutationMatrix<Dynamic,Dynamic,PermIndex> &iPerm,
26 const Matrix<Scalar,Dynamic,1> &diag,
27 const Matrix<Scalar,Dynamic,1> &qtb,
28 Matrix<Scalar,Dynamic,1> &x,
29 Matrix<Scalar,Dynamic,1> &sdiag)
30 {
31 /* Local variables */
32 Index i, j, k;
33 Scalar temp;
34 Index n = s.cols();
35 Matrix<Scalar,Dynamic,1> wa(n);
36 JacobiRotation<Scalar> givens;
37
38 /* Function Body */
39 // the following will only change the lower triangular part of s, including
40 // the diagonal, though the diagonal is restored afterward
41
42 /* copy r and (q transpose)*b to preserve input and initialize s. */
43 /* in particular, save the diagonal elements of r in x. */
44 x = s.diagonal();
45 wa = qtb;
46
47
48 s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose();
49 /* eliminate the diagonal matrix d using a givens rotation. */
50 for (j = 0; j < n; ++j) {
51
52 /* prepare the row of d to be eliminated, locating the */
53 /* diagonal element using p from the qr factorization. */
54 const PermIndex l = iPerm.indices()(j);
55 if (diag[l] == 0.)
56 break;
57 sdiag.tail(n-j).setZero();
58 sdiag[j] = diag[l];
59
60 /* the transformations to eliminate the row of d */
61 /* modify only a single element of (q transpose)*b */
62 /* beyond the first n, which is initially zero. */
63 Scalar qtbpj = 0.;
64 for (k = j; k < n; ++k) {
65 /* determine a givens rotation which eliminates the */
66 /* appropriate element in the current row of d. */
67 givens.makeGivens(-s(k,k), sdiag[k]);
68
69 /* compute the modified diagonal element of r and */
70 /* the modified element of ((q transpose)*b,0). */
71 s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k];
72 temp = givens.c() * wa[k] + givens.s() * qtbpj;
73 qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
74 wa[k] = temp;
75
76 /* accumulate the tranformation in the row of s. */
77 for (i = k+1; i<n; ++i) {
78 temp = givens.c() * s(i,k) + givens.s() * sdiag[i];
79 sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i];
80 s(i,k) = temp;
81 }
82 }
83 }
84
85 /* solve the triangular system for z. if the system is */
86 /* singular, then obtain a least squares solution. */
87 Index nsing;
88 for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {}
89
90 wa.tail(n-nsing).setZero();
91 s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
92
93 // restore
94 sdiag = s.diagonal();
95 s.diagonal() = x;
96
97 /* permute the components of z back to components of x. */
98 x = iPerm * wa;
99 }
100
101 template <typename Scalar, int _Options, typename Index>
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)102 void lmqrsolv(
103 SparseMatrix<Scalar,_Options,Index> &s,
104 const PermutationMatrix<Dynamic,Dynamic> &iPerm,
105 const Matrix<Scalar,Dynamic,1> &diag,
106 const Matrix<Scalar,Dynamic,1> &qtb,
107 Matrix<Scalar,Dynamic,1> &x,
108 Matrix<Scalar,Dynamic,1> &sdiag)
109 {
110 /* Local variables */
111 typedef SparseMatrix<Scalar,RowMajor,Index> FactorType;
112 Index i, j, k, l;
113 Scalar temp;
114 Index n = s.cols();
115 Matrix<Scalar,Dynamic,1> wa(n);
116 JacobiRotation<Scalar> givens;
117
118 /* Function Body */
119 // the following will only change the lower triangular part of s, including
120 // the diagonal, though the diagonal is restored afterward
121
122 /* copy r and (q transpose)*b to preserve input and initialize R. */
123 wa = qtb;
124 FactorType R(s);
125 // Eliminate the diagonal matrix d using a givens rotation
126 for (j = 0; j < n; ++j)
127 {
128 // Prepare the row of d to be eliminated, locating the
129 // diagonal element using p from the qr factorization
130 l = iPerm.indices()(j);
131 if (diag(l) == Scalar(0))
132 break;
133 sdiag.tail(n-j).setZero();
134 sdiag[j] = diag[l];
135 // the transformations to eliminate the row of d
136 // modify only a single element of (q transpose)*b
137 // beyond the first n, which is initially zero.
138
139 Scalar qtbpj = 0;
140 // Browse the nonzero elements of row j of the upper triangular s
141 for (k = j; k < n; ++k)
142 {
143 typename FactorType::InnerIterator itk(R,k);
144 for (; itk; ++itk){
145 if (itk.index() < k) continue;
146 else break;
147 }
148 //At this point, we have the diagonal element R(k,k)
149 // Determine a givens rotation which eliminates
150 // the appropriate element in the current row of d
151 givens.makeGivens(-itk.value(), sdiag(k));
152
153 // Compute the modified diagonal element of r and
154 // the modified element of ((q transpose)*b,0).
155 itk.valueRef() = givens.c() * itk.value() + givens.s() * sdiag(k);
156 temp = givens.c() * wa(k) + givens.s() * qtbpj;
157 qtbpj = -givens.s() * wa(k) + givens.c() * qtbpj;
158 wa(k) = temp;
159
160 // Accumulate the transformation in the remaining k row/column of R
161 for (++itk; itk; ++itk)
162 {
163 i = itk.index();
164 temp = givens.c() * itk.value() + givens.s() * sdiag(i);
165 sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i);
166 itk.valueRef() = temp;
167 }
168 }
169 }
170
171 // Solve the triangular system for z. If the system is
172 // singular, then obtain a least squares solution
173 Index nsing;
174 for(nsing = 0; nsing<n && sdiag(nsing) !=0; nsing++) {}
175
176 wa.tail(n-nsing).setZero();
177 // x = wa;
178 wa.head(nsing) = R.topLeftCorner(nsing,nsing).template triangularView<Upper>().solve/*InPlace*/(wa.head(nsing));
179
180 sdiag = R.diagonal();
181 // Permute the components of z back to components of x
182 x = iPerm * wa;
183 }
184 } // end namespace internal
185
186 } // end namespace Eigen
187
188 #endif // EIGEN_LMQRSOLV_H
189