qrsolv.h
Go to the documentation of this file.
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>
7 void qrsolv(
9  // TODO : use a PermutationMatrix once lmpar is no more:
10  const VectorXi &ipvt,
11  const Matrix< Scalar, Dynamic, 1 > &diag,
15 
16 {
17  typedef DenseIndex Index;
18 
19  /* Local variables */
20  Index i, j, k, l;
21  Scalar temp;
22  Index n = s.cols();
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
Scalar & c()
Definition: Jacobi.h:45
void makeGivens(const Scalar &p, const Scalar &q, Scalar *z=0)
Definition: Jacobi.h:148
iterative scaling algorithm to equilibrate rows and column norms in matrices
Definition: matrix.hpp:471
Rotation given by a cosine-sine pair.
Derived & setZero(Index size)
EIGEN_DEFAULT_DENSE_INDEX_TYPE DenseIndex
Definition: XprHelper.h:27
Scalar & s()
Definition: Jacobi.h:47
void 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)
Definition: qrsolv.h:7
EIGEN_STRONG_INLINE Index cols() const


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Mon Jun 10 2019 12:35:02