qrsolv.h
Go to the documentation of this file.
00001 namespace Eigen { 
00002 
00003 namespace internal {
00004 
00005 // TODO : once qrsolv2 is removed, use ColPivHouseholderQR or PermutationMatrix instead of ipvt
00006 template <typename Scalar>
00007 void qrsolv(
00008         Matrix< Scalar, Dynamic, Dynamic > &s,
00009         // TODO : use a PermutationMatrix once lmpar is no more:
00010         const VectorXi &ipvt,
00011         const Matrix< Scalar, Dynamic, 1 >  &diag,
00012         const Matrix< Scalar, Dynamic, 1 >  &qtb,
00013         Matrix< Scalar, Dynamic, 1 >  &x,
00014         Matrix< Scalar, Dynamic, 1 >  &sdiag)
00015 
00016 {
00017     typedef DenseIndex Index;
00018 
00019     /* Local variables */
00020     Index i, j, k, l;
00021     Scalar temp;
00022     Index n = s.cols();
00023     Matrix< Scalar, Dynamic, 1 >  wa(n);
00024     JacobiRotation<Scalar> givens;
00025 
00026     /* Function Body */
00027     // the following will only change the lower triangular part of s, including
00028     // the diagonal, though the diagonal is restored afterward
00029 
00030     /*     copy r and (q transpose)*b to preserve input and initialize s. */
00031     /*     in particular, save the diagonal elements of r in x. */
00032     x = s.diagonal();
00033     wa = qtb;
00034 
00035     s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose();
00036 
00037     /*     eliminate the diagonal matrix d using a givens rotation. */
00038     for (j = 0; j < n; ++j) {
00039 
00040         /*        prepare the row of d to be eliminated, locating the */
00041         /*        diagonal element using p from the qr factorization. */
00042         l = ipvt[j];
00043         if (diag[l] == 0.)
00044             break;
00045         sdiag.tail(n-j).setZero();
00046         sdiag[j] = diag[l];
00047 
00048         /*        the transformations to eliminate the row of d */
00049         /*        modify only a single element of (q transpose)*b */
00050         /*        beyond the first n, which is initially zero. */
00051         Scalar qtbpj = 0.;
00052         for (k = j; k < n; ++k) {
00053             /*           determine a givens rotation which eliminates the */
00054             /*           appropriate element in the current row of d. */
00055             givens.makeGivens(-s(k,k), sdiag[k]);
00056 
00057             /*           compute the modified diagonal element of r and */
00058             /*           the modified element of ((q transpose)*b,0). */
00059             s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k];
00060             temp = givens.c() * wa[k] + givens.s() * qtbpj;
00061             qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
00062             wa[k] = temp;
00063 
00064             /*           accumulate the tranformation in the row of s. */
00065             for (i = k+1; i<n; ++i) {
00066                 temp = givens.c() * s(i,k) + givens.s() * sdiag[i];
00067                 sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i];
00068                 s(i,k) = temp;
00069             }
00070         }
00071     }
00072 
00073     /*     solve the triangular system for z. if the system is */
00074     /*     singular, then obtain a least squares solution. */
00075     Index nsing;
00076     for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {}
00077 
00078     wa.tail(n-nsing).setZero();
00079     s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
00080 
00081     // restore
00082     sdiag = s.diagonal();
00083     s.diagonal() = x;
00084 
00085     /*     permute the components of z back to components of x. */
00086     for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j];
00087 }
00088 
00089 } // end namespace internal
00090 
00091 } // end namespace Eigen


acado
Author(s): Milan Vukov, Rien Quirynen
autogenerated on Sat Jun 8 2019 19:38:44