src/NonLinearOptimization/qrsolv.h

0001 namespace Eigen {
0002
0003 namespace internal {
0004
0005 // TODO : once qrsolv2 is removed, use ColPivHouseholderQR or PermutationMatrix instead of ipvt
0006 template <typename Scalar>
0007 void qrsolv(
0008         Matrix< Scalar, Dynamic, Dynamic > &s,
0009         // TODO : use a PermutationMatrix once lmpar is no more:
0010         const VectorXi &ipvt,
0011         const Matrix< Scalar, Dynamic, 1 >  &diag,
0012         const Matrix< Scalar, Dynamic, 1 >  &qtb,
0013         Matrix< Scalar, Dynamic, 1 >  &x,
0014         Matrix< Scalar, Dynamic, 1 >  &sdiag)
0015
0016 {
0017     typedef DenseIndex Index;
0018
0019     /* Local variables */
0020     Index i, j, k, l;
0021     Scalar temp;
0022     Index n = s.cols();
0023     Matrix< Scalar, Dynamic, 1 >  wa(n);
0024     JacobiRotation<Scalar> givens;
0025
0026     /* Function Body */
0027     // the following will only change the lower triangular part of s, including
0028     // the diagonal, though the diagonal is restored afterward
0029
0030     /*     copy r and (q transpose)*b to preserve input and initialize s. */
0031     /*     in particular, save the diagonal elements of r in x. */
0032     x = s.diagonal();
0033     wa = qtb;
0034
0035     s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose();
0036
0037     /*     eliminate the diagonal matrix d using a givens rotation. */
0038     for (j = 0; j < n; ++j) {
0039
0040         /*        prepare the row of d to be eliminated, locating the */
0041         /*        diagonal element using p from the qr factorization. */
0042         l = ipvt[j];
0043         if (diag[l] == 0.)
0044             break;
0045         sdiag.tail(n-j).setZero();
0046         sdiag[j] = diag[l];
0047
0048         /*        the transformations to eliminate the row of d */
0049         /*        modify only a single element of (q transpose)*b */
0050         /*        beyond the first n, which is initially zero. */
0051         Scalar qtbpj = 0.;
0052         for (k = j; k < n; ++k) {
0053             /*           determine a givens rotation which eliminates the */
0054             /*           appropriate element in the current row of d. */
0055             givens.makeGivens(-s(k,k), sdiag[k]);
0056
0057             /*           compute the modified diagonal element of r and */
0058             /*           the modified element of ((q transpose)*b,0). */
0059             s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k];
0060             temp = givens.c() * wa[k] + givens.s() * qtbpj;
0061             qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
0062             wa[k] = temp;
0063
0064             /*           accumulate the transformation in the row of s. */
0065             for (i = k+1; i<n; ++i) {
0066                 temp = givens.c() * s(i,k) + givens.s() * sdiag[i];
0067                 sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i];
0068                 s(i,k) = temp;
0069             }
0070         }
0071     }
0072
0073     /*     solve the triangular system for z. if the system is */
0074     /*     singular, then obtain a least squares solution. */
0075     Index nsing;
0076     for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {}
0077
0078     wa.tail(n-nsing).setZero();
0079     s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
0080
0081     // restore
0082     sdiag = s.diagonal();
0083     s.diagonal() = x;
0084
0085     /*     permute the components of z back to components of x. */
0086     for (j = 0; j < n; ++j) x[ipvt[j]] = wa[j];
0087 }
0088
0089 } // end namespace internal
0090
0091 } // end namespace Eigen