LMqrsolv.h
00001 // This file is part of Eigen, a lightweight C++ template library
00002 // for linear algebra.
00003 //
00004 // Copyright (C) 2009 Thomas Capricelli <orzel@freehackers.org>
00005 // Copyright (C) 2012 Desire Nuentsa <desire.nuentsa_wakam@inria.fr>
00006 //
00007 // This code initially comes from MINPACK whose original authors are:
00008 // Copyright Jorge More - Argonne National Laboratory
00009 // Copyright Burt Garbow - Argonne National Laboratory
00010 // Copyright Ken Hillstrom - Argonne National Laboratory
00011 //
00012 // This Source Code Form is subject to the terms of the Minpack license
00013 // (a BSD-like license) described in the campaigned CopyrightMINPACK.txt file.
00014 
00015 #ifndef EIGEN_LMQRSOLV_H
00016 #define EIGEN_LMQRSOLV_H
00017 
00018 namespace Eigen { 
00019 
00020 namespace internal {
00021 
00022 template <typename Scalar,int Rows, int Cols, typename PermIndex>
00023 void lmqrsolv(
00024   Matrix<Scalar,Rows,Cols> &s,
00025   const PermutationMatrix<Dynamic,Dynamic,PermIndex> &iPerm,
00026   const Matrix<Scalar,Dynamic,1> &diag,
00027   const Matrix<Scalar,Dynamic,1> &qtb,
00028   Matrix<Scalar,Dynamic,1> &x,
00029   Matrix<Scalar,Dynamic,1> &sdiag)
00030 {
00031     /* Local variables */
00032     Index i, j, k;
00033     Scalar temp;
00034     Index n = s.cols();
00035     Matrix<Scalar,Dynamic,1>  wa(n);
00036     JacobiRotation<Scalar> givens;
00037 
00038     /* Function Body */
00039     // the following will only change the lower triangular part of s, including
00040     // the diagonal, though the diagonal is restored afterward
00041 
00042     /*     copy r and (q transpose)*b to preserve input and initialize s. */
00043     /*     in particular, save the diagonal elements of r in x. */
00044     x = s.diagonal();
00045     wa = qtb;
00046     
00047    
00048     s.topLeftCorner(n,n).template triangularView<StrictlyLower>() = s.topLeftCorner(n,n).transpose();
00049     /*     eliminate the diagonal matrix d using a givens rotation. */
00050     for (j = 0; j < n; ++j) {
00051 
00052         /*        prepare the row of d to be eliminated, locating the */
00053         /*        diagonal element using p from the qr factorization. */
00054         const PermIndex l = iPerm.indices()(j);
00055         if (diag[l] == 0.)
00056             break;
00057         sdiag.tail(n-j).setZero();
00058         sdiag[j] = diag[l];
00059 
00060         /*        the transformations to eliminate the row of d */
00061         /*        modify only a single element of (q transpose)*b */
00062         /*        beyond the first n, which is initially zero. */
00063         Scalar qtbpj = 0.;
00064         for (k = j; k < n; ++k) {
00065             /*           determine a givens rotation which eliminates the */
00066             /*           appropriate element in the current row of d. */
00067             givens.makeGivens(-s(k,k), sdiag[k]);
00068 
00069             /*           compute the modified diagonal element of r and */
00070             /*           the modified element of ((q transpose)*b,0). */
00071             s(k,k) = givens.c() * s(k,k) + givens.s() * sdiag[k];
00072             temp = givens.c() * wa[k] + givens.s() * qtbpj;
00073             qtbpj = -givens.s() * wa[k] + givens.c() * qtbpj;
00074             wa[k] = temp;
00075 
00076             /*           accumulate the tranformation in the row of s. */
00077             for (i = k+1; i<n; ++i) {
00078                 temp = givens.c() * s(i,k) + givens.s() * sdiag[i];
00079                 sdiag[i] = -givens.s() * s(i,k) + givens.c() * sdiag[i];
00080                 s(i,k) = temp;
00081             }
00082         }
00083     }
00084   
00085     /*     solve the triangular system for z. if the system is */
00086     /*     singular, then obtain a least squares solution. */
00087     Index nsing;
00088     for(nsing=0; nsing<n && sdiag[nsing]!=0; nsing++) {}
00089 
00090     wa.tail(n-nsing).setZero();
00091     s.topLeftCorner(nsing, nsing).transpose().template triangularView<Upper>().solveInPlace(wa.head(nsing));
00092   
00093     // restore
00094     sdiag = s.diagonal();
00095     s.diagonal() = x;
00096 
00097     /* permute the components of z back to components of x. */
00098     x = iPerm * wa; 
00099 }
00100 
00101 template <typename Scalar, int _Options, typename Index>
00102 void lmqrsolv(
00103   SparseMatrix<Scalar,_Options,Index> &s,
00104   const PermutationMatrix<Dynamic,Dynamic> &iPerm,
00105   const Matrix<Scalar,Dynamic,1> &diag,
00106   const Matrix<Scalar,Dynamic,1> &qtb,
00107   Matrix<Scalar,Dynamic,1> &x,
00108   Matrix<Scalar,Dynamic,1> &sdiag)
00109 {
00110   /* Local variables */
00111   typedef SparseMatrix<Scalar,RowMajor,Index> FactorType;
00112     Index i, j, k, l;
00113     Scalar temp;
00114     Index n = s.cols();
00115     Matrix<Scalar,Dynamic,1>  wa(n);
00116     JacobiRotation<Scalar> givens;
00117 
00118     /* Function Body */
00119     // the following will only change the lower triangular part of s, including
00120     // the diagonal, though the diagonal is restored afterward
00121 
00122     /*     copy r and (q transpose)*b to preserve input and initialize R. */
00123     wa = qtb;
00124     FactorType R(s);
00125     // Eliminate the diagonal matrix d using a givens rotation
00126     for (j = 0; j < n; ++j)
00127     {
00128       // Prepare the row of d to be eliminated, locating the 
00129       // diagonal element using p from the qr factorization
00130       l = iPerm.indices()(j);
00131       if (diag(l) == Scalar(0)) 
00132         break; 
00133       sdiag.tail(n-j).setZero();
00134       sdiag[j] = diag[l];
00135       // the transformations to eliminate the row of d
00136       // modify only a single element of (q transpose)*b
00137       // beyond the first n, which is initially zero. 
00138       
00139       Scalar qtbpj = 0; 
00140       // Browse the nonzero elements of row j of the upper triangular s
00141       for (k = j; k < n; ++k)
00142       {
00143         typename FactorType::InnerIterator itk(R,k);
00144         for (; itk; ++itk){
00145           if (itk.index() < k) continue;
00146           else break;
00147         }
00148         //At this point, we have the diagonal element R(k,k)
00149         // Determine a givens rotation which eliminates 
00150         // the appropriate element in the current row of d
00151         givens.makeGivens(-itk.value(), sdiag(k));
00152         
00153         // Compute the modified diagonal element of r and 
00154         // the modified element of ((q transpose)*b,0).
00155         itk.valueRef() = givens.c() * itk.value() + givens.s() * sdiag(k);
00156         temp = givens.c() * wa(k) + givens.s() * qtbpj; 
00157         qtbpj = -givens.s() * wa(k) + givens.c() * qtbpj;
00158         wa(k) = temp;
00159         
00160         // Accumulate the transformation in the remaining k row/column of R
00161         for (++itk; itk; ++itk)
00162         {
00163           i = itk.index();
00164           temp = givens.c() *  itk.value() + givens.s() * sdiag(i);
00165           sdiag(i) = -givens.s() * itk.value() + givens.c() * sdiag(i);
00166           itk.valueRef() = temp;
00167         }
00168       }
00169     }
00170     
00171     // Solve the triangular system for z. If the system is 
00172     // singular, then obtain a least squares solution
00173     Index nsing;
00174     for(nsing = 0; nsing<n && sdiag(nsing) !=0; nsing++) {}
00175     
00176     wa.tail(n-nsing).setZero();
00177 //     x = wa; 
00178     wa.head(nsing) = R.topLeftCorner(nsing,nsing).template triangularView<Upper>().solve/*InPlace*/(wa.head(nsing));
00179     
00180     sdiag = R.diagonal();
00181     // Permute the components of z back to components of x
00182     x = iPerm * wa; 
00183 }
00184 } // end namespace internal
00185 
00186 } // end namespace Eigen
00187 
00188 #endif // EIGEN_LMQRSOLV_H
 All Classes Functions Variables Typedefs Enumerator