GPS ToolKit Software Library: MatrixFunctors.hpp Source File

00001 #pragma ident "$Id: MatrixFunctors.hpp 3140 2012-06-18 15:03:02Z susancummins $"
00002 
00008 #ifndef GPSTK_MATRIX_FUNCTORS_HPP
00009 #define GPSTK_MATRIX_FUNCTORS_HPP
00010 
00011 //============================================================================
00012 //
00013 //  This file is part of GPSTk, the GPS Toolkit.
00014 //
00015 //  The GPSTk is free software; you can redistribute it and/or modify
00016 //  it under the terms of the GNU Lesser General Public License as published
00017 //  by the Free Software Foundation; either version 2.1 of the License, or
00018 //  any later version.
00019 //
00020 //  The GPSTk is distributed in the hope that it will be useful,
00021 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
00022 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00023 //  GNU Lesser General Public License for more details.
00024 //
00025 //  You should have received a copy of the GNU Lesser General Public
00026 //  License along with GPSTk; if not, write to the Free Software Foundation,
00027 //  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
00028 //  
00029 //  Copyright 2004, The University of Texas at Austin
00030 //
00031 //============================================================================
00032 
00033 #include <cmath>
00034 
00035 namespace gpstk
00036 {
00037 
00040 
00076    template <class T>
00077    class SVD
00078    {
00079    public:
00080       SVD() : iterationMax(30) {}
00081 
00085       template <class BaseClass>
00086       bool operator() (const ConstMatrixBase<T, BaseClass>& mat)
00087          throw (MatrixException)
00088          {
00089             bool flip=false;
00090             U = mat;
00091             if(mat.rows() > mat.cols()) {
00092                flip = true;
00093                U = transpose(mat);
00094             }
00095 
00096             size_t n(U.cols()), m(U.rows());
00097             size_t i, j, k, l, nm, jj;
00098             T anorm(0), scale(0), g(0), s, f, h, c, x, y, z;
00099 
00100             V = Matrix<T>(n, n, T(0));
00101             S = Vector<T>(n, T(1));
00102             Vector<T> B(n, T(1));
00103 
00104             for (i = 0; i < n; i++) {
00105                l = i + 1;
00106                B[i] = scale * g;
00107                g = s = scale = T(0);
00108                if (i < m) {
00109                   for(k = i; k < m; k++) scale += ABS(U(k, i));
00110                   if (scale) {
00111                      for(k = i; k < m; k++) {
00112                         U(k, i) /= scale;
00113                         s += U(k, i) * U(k, i);
00114                      }
00115                      f = U(i, i);
00116                      g = -SIGN(SQRT(s),f);
00117                      h = f * g - s;
00118                      U(i,i) = f - g;
00119                      for(j = l; j < n; j++) {
00120                         for(s = T(0), k = i; k < m; k++) s += U(k, i) * U(k, j);
00121                         f = s / h;
00122                         for(k = i; k < m; k++) U(k, j) += f * U(k, i);
00123                      }
00124                      for(k = i; k < m; k++) U(k, i) *= scale;
00125                   } // if (scale)
00126                }  // if (i < m)
00127                S[i] = scale * g;
00128                g = s = scale = T(0);
00129                if ( (i < m) && (i != n-1) ) {
00130                   for(k = l; k < n; k++) scale += ABS(U(i, k));
00131                   if (scale) {
00132                      for(k = l; k < n; k++) {
00133                         U(i, k) /= scale;
00134                         s += U(i, k) * U(i, k);
00135                      }
00136                      f = U(i, l);
00137                      g = -SIGN(SQRT(s),f);
00138                      h = f * g - s;
00139                      U(i, l) = f - g;
00140                      for(k = l; k < n; k++) B[k] = U(i, k) / h;
00141                      for(j = l; j < m; j++) {
00142                         for(s = T(0), k = l; k < n; k++) s += U(j, k) * U(i, k);
00143                         for(k = l; k < n; k++) U(j, k) += s * B[k];
00144                      }
00145                      for(k = l; k < n; k++) U(i, k) *= scale;
00146                   }
00147                }
00148                if(ABS(S[i])+ABS(B[i]) > anorm) anorm=ABS(S[i])+ABS(B[i]);
00149             }
00150             for(i = n - 1; ; i--) {
00151                if (i < n - 1) {
00152                   if (g) {
00153                      for(j = l; j < n; j++) V(j, i) = (U(i, j) / U(i, l)) / g;
00154                      for(j = l; j < n; j++) {
00155                         for(s = T(0), k = l; k < n; k++) s += U(i, k) * V(k, j);
00156                         for(k = l; k < n; k++) V(k, j) += s * V(k, i);
00157                      }
00158                   }
00159                   for(j = l; j < n; j++) V(j, i) = V(i, j) = T(0);
00160                }
00161                V(i,i) =T(1);
00162                g = B[i];
00163                l = i;
00164                if(i==0) break;
00165             }
00166             for(i = ( (m-1 < n-1) ? m-1 : n-1); ; i--) {
00167                l = i+1;
00168                g = S[i];
00169                for(j=l; j<n; j++) U(i, j) = T(0);
00170                if (g) {
00171                   g = T(1) / g;
00172                   for(j = l; j < n; j++) {
00173                      for(s = T(0), k = l; k < m; k++) s += U(k,i) * U(k,j);
00174                      f = (s / U(i,i)) * g;
00175                      for(k=i; k<m; k++) U(k,j) += f * U(k,i);
00176                   }
00177                   for(j = i; j < m; j++) U(j,i) *= g;
00178                } 
00179                else {
00180                   for(j=i; j<m; j++) U(j,i) = T(0);
00181                }
00182                ++U(i,i);
00183                if(i==0) break;
00184             }
00185             for(k = n - 1; ; k--) {
00186                size_t its;
00187                for(its = 1; its <= iterationMax; its++) {
00188                   bool flag = true;
00189                   for(l = k; ; l--) {
00190                      nm = l - 1;
00191                      if ((ABS(B[l])+anorm) == anorm) {
00192                         flag = false;
00193                         break;
00194                      }
00195                      if (l == 0) { // should never happen
00196                         MatrixException e("SVD algorithm has nm==-1");
00197                         GPSTK_THROW(e);
00198                      }
00199                      if ((ABS(S[nm])+anorm) == anorm) break;
00200                      if(l == 0) break; // since l is unsigned...
00201                   }
00202                   if (flag) {
00203                      c = T(0);
00204                      s = T(1);
00205                      for(i = l; i <= k; i++) {
00206                         f = s * B[i];
00207                         B[i] = c * B[i];
00208                         if ((ABS(f) + anorm) == anorm) break;
00209                         g = S[i];
00210                         h = RSS(f,g);
00211                         S[i] = h;
00212                         h = T(1) / h;
00213                         c = g * h;
00214                         s = -f * h;
00215                         for(j = 0; j < m; j++) {
00216                            y = U(j, nm);
00217                            z = U(j,i);
00218                            U(j, nm) = y * c + z * s;
00219                            U(j,i) = z * c - y * s;
00220                         }
00221                      }
00222                   }
00223                   z = S[k];
00224                   if (l == k) {
00225                      if (z < T(0)) {
00226                         S[k] = -z;
00227                         for(j = 0; j < n; j++) V(j,k) = -V(j,k);
00228                      }
00229                      break;
00230                   }
00231             
00232                   if (its == iterationMax) {
00233                      MatrixException e("SVD algorithm did not converge");
00234                      GPSTK_THROW(e);
00235                   }
00236                   x = S[l];
00237                   if(k == 0) { // should never happen
00238                      MatrixException e("SVD algorithm has k==0");
00239                      GPSTK_THROW(e);
00240                   }
00241                   nm = k - 1;
00242                   y = S[nm];
00243                   g = B[nm];
00244                   h = B[k];
00245                   f = ( (y-z) * (y+z) + (g-h) * (g+h)) / (T(2) * h * y);
00246                   g = RSS(f,T(1));
00247                   f = ( (x-z) * (x+z) + h * ((y/(f + SIGN(g,f))) - h)) / x;
00248                   c = s = 1.0;
00249                   for(j = l; j <= nm; j++) {
00250                      i = j + 1;
00251                      g = B[i];
00252                      y = S[i];
00253                      h = s * g;
00254                      g = c * g;
00255                      z = RSS(f, h);
00256                      B[j] = z;
00257                      c = f / z;
00258                      s = h / z;
00259                      f = x * c + g * s;
00260                      g = g * c - x * s;
00261                      h = y * s;
00262                      y *= c;
00263                      for(jj = 0; jj < n; jj++) {
00264                         x = V(jj, j);
00265                         z = V(jj, i);
00266                         V(jj, j) = x * c + z * s;
00267                         V(jj, i) = z * c - x * s;
00268                      }
00269                      z = RSS(f, h);
00270                      S[j] = z;
00271                      if (z) {
00272                         z = T(1) / z;
00273                         c = f * z;
00274                         s = h * z;
00275                      }
00276                      f = c * g + s * y;
00277                      x = c * y - s * g;
00278                      for(jj = 0; jj < m; jj++) {
00279                         y = U(jj, j);
00280                         z = U(jj, i);
00281                         U(jj, j) = y * c + z * s;
00282                         U(jj, i) = z * c - y * s;
00283                      }
00284                   }
00285                   B[l] = T(0);
00286                   B[k] = f;
00287                   S[k] = x;
00288                }
00289                if(k==0) break;   // since k is unsigned...
00290             }
00291                // if U is not square - last n-m columns of U are zero - remove
00292             if(U.cols() > U.rows()) {
00293                for(i=1; i<S.size(); i++) {   // sort in descending order
00294                   T sv=S[i],svj;
00295                   int kk = i-1;  // not size_t ~ unsigned
00296                   while(kk >= 0) {
00297                      svj = S[kk];
00298                      if(sv < svj) break;
00299                      S[kk+1] = svj;
00300                      // exchange columns kk and kk+1 in U and V
00301                      U.swapCols(kk,kk+1);
00302                      V.swapCols(kk,kk+1);
00303                      kk = kk - 1;
00304                   }
00305                   S[kk+1] = sv;
00306                }
00307                Matrix<T> Temp(U);
00308                U = Matrix<T>(Temp,0,0,Temp.rows(),Temp.rows());
00309                S.resize(Temp.rows());
00310             }
00311 
00312             if(flip) {
00313                Matrix<T> Temp(U);
00314                U = V;
00315                V = Temp;
00316             }
00317 
00318             return true;
00319 
00320          }  // end SVD::operator() - the SVD algorithm
00321    
00328       template <class BaseClass>
00329       void backSub(RefVectorBase<T, BaseClass>& b) const 
00330          throw(MatrixException)
00331       {
00332          if(b.size() != U.rows())
00333          {
00334              MatrixException e("SVD::BackSub called with unequal dimensions");
00335              GPSTK_THROW(e);
00336          }
00337    
00338          size_t i, n=V.cols(), m=U.rows();
00339          Matrix<T> W(n,m,T(0));     // build the 'inverse singular values' matrix
00340          for(i=0; i<S.size(); i++) W(i,i)=(S(i)==T(0)?T(0):T(1)/S(i));
00341          Vector<T> Y;
00342          Y = V*W*transpose(U)*b;
00343          //b = Y;
00344          // this fails because operator= is not defined for the base class
00345          // (op= not inherited); use assignFrom instead
00346          b.assignFrom(Y);
00347 
00348       }  // end SVD::backSub
00349 
00351       void sort(bool descending)
00352          throw(MatrixException)
00353       {
00354          size_t i;
00355          int j;         // j must be allowed to go negative
00356          for(i=1; i<S.size(); i++) {
00357             T sv=S(i),svj;
00358             j = i - 1;
00359             while(j >= 0) {
00360                svj = S(j);
00361                if(descending && sv < svj) break;
00362                if(!descending && sv > svj) break;
00363                S(j+1) = svj;
00364                // exchange columns j and j+1 in U and V
00365                U.swapCols(j,j+1);
00366                V.swapCols(j,j+1);
00367                j = j - 1;
00368             }
00369             S(j+1) = sv;
00370          }
00371       }  // end SVD::sort
00372 
00374       inline T det(void)
00375          throw(MatrixException)
00376       {
00377          T d(1);
00378          for(size_t i=0; i<S.size(); i++) d *= S(i);
00379          return d;
00380       }  // end SVD::det
00381 
00383       Matrix<T> U;
00385       Vector<T> S;
00387       Matrix<T> V;
00388 
00389    private:
00390       const size_t iterationMax;
00391    
00392       T SIGN(T a, T b)
00393          { 
00394             if (b >= T(0))
00395                return ABS(a);
00396             else
00397                return -ABS(a);
00398          }
00399 
00400    }; // end class SVD
00401 
00407    template <class T>
00408    class LUDecomp
00409    {
00410    public:
00411       LUDecomp() {}        // why is there no constructor from ConstMatrixBase?
00412 
00414       template <class BaseClass>
00415       void operator() (const ConstMatrixBase<T, BaseClass>& m)
00416          throw (MatrixException)
00417          {
00418             if(!m.isSquare() || m.rows()<=1) {
00419                MatrixException e("LUDecomp requires a square, non-trivial matrix");
00420                GPSTK_THROW(e);
00421             }
00422 
00423             size_t N=m.rows(),i,j,k,n,imax;
00424             T big,t,d;
00425             Vector<T> V(N,T(0));
00426 
00427             LU = m;
00428             Pivot = Vector<int>(N);
00429             parity = 1;
00430 
00431             for(i=0; i<N; i++) {    // get scale of each row
00432                big = T(0);
00433                for(j=0; j<N; j++) {
00434                   t = ABS(LU(i,j));
00435                   if(t > big) big=t;
00436                }
00437                if(big <= T(0)) {    // m is singular
00438                   //LU *= T(0);
00439                   SingularMatrixException e("singular matrix!");
00440                   GPSTK_THROW(e);
00441                }
00442                V(i) = T(1)/big;
00443             }
00444 
00445             for(j=0; j<N; j++) {    // loop over columns
00446                for(i=0; i<j; i++) {
00447                   t = LU(i,j);
00448                   for(k=0; k<i; k++) t -= LU(i,k)*LU(k,j);
00449                   LU(i,j) = t;
00450                }
00451                big = T(0);          // find largest pivot
00452                for(i=j; i<N; i++) {
00453                   t = LU(i,j);
00454                   for(k=0; k<j; k++) t -= LU(i,k)*LU(k,j);
00455                   LU(i,j) = t;
00456                   d = V(i)*ABS(t);
00457                   if(d >= big) {
00458                      big = d;
00459                      imax = i;
00460                   }
00461                }
00462                if(j != imax) {
00463                   LU.swapRows(imax,j);
00464                   V(imax) = V(j);
00465                   parity = -parity;
00466                }
00467                Pivot(j) = imax;
00468 
00469                t = LU(j,j);
00470                if(t == 0.0) {       // m is singular
00471                   //LU *= T(0);
00472                   SingularMatrixException e("singular matrix!");
00473                   GPSTK_THROW(e);
00474                }
00475                if(j != N-1) {
00476                   d = T(1)/t;
00477                   for(i=j+1; i<N; i++) LU(i,j) *= d;
00478                }
00479             }
00480          }  // end LUDecomp()
00481 
00485       template <class BaseClass2>
00486       void backSub(RefVectorBase<T, BaseClass2>& v) const
00487          throw (MatrixException)
00488       {
00489          if(LU.rows() != v.size()) {
00490             MatrixException e("Vector size does not match dimension of LUDecomp");
00491             GPSTK_THROW(e);
00492          }
00493 
00494          bool first=true;
00495          size_t N=LU.rows(),i,j,ii(0);
00496          T sum;
00497 
00498          // un-pivot
00499          for(i=0; i<N; i++) {
00500             sum = v(Pivot(i));
00501             v(Pivot(i)) = v(i);
00502             if(first && sum != T(0)) {
00503                ii = i;
00504                first = false;
00505             }
00506             else for(j=ii; j<i; j++) sum -= LU(i,j)*v(j);
00507             v(i) = sum;
00508          }
00509          // back substitution
00510          for(i=N-1; ; i--) {
00511             sum = v(i);
00512             for(j=i+1; j<N; j++) sum -= LU(i,j)*v(j);
00513             v(i) = sum / LU(i,i);
00514             if(i == 0) break;       // b/c i is unsigned
00515          }
00516       }  // end LUD::backSub
00517 
00519       inline T det(void)
00520          throw(MatrixException)
00521       {
00522          T d(parity);
00523          for(size_t i=0; i<LU.rows(); i++) d *= LU(i,i);
00524          return d;
00525       }
00526 
00530          Matrix<T> LU;
00532          Vector<int> Pivot;
00534          int parity;
00535 
00536    }; // end class LUDecomp
00537 
00538 
00545    template <class T>
00546    class Cholesky
00547    {
00548    public:
00549       Cholesky() {}
00550 
00552       template <class BaseClass>
00553       void operator() (const ConstMatrixBase<T, BaseClass>& m)
00554          throw (MatrixException)
00555       {
00556          if(!m.isSquare()) {
00557             MatrixException e("Cholesky requires a square matrix");
00558             GPSTK_THROW(e);
00559          }
00560 
00561          size_t N=m.rows(),i,j,k;
00562          double d;
00563          Matrix<T> P(m);
00564          U = Matrix<T>(m.rows(),m.cols(),T(0));
00565 
00566          for(j=N-1; ; j--) {
00567             if(P(j,j) <= T(0)) {
00568                MatrixException e("Cholesky fails - eigenvalue <= 0");
00569                GPSTK_THROW(e);
00570             }
00571             U(j,j) = SQRT(P(j,j));
00572             d = T(1)/U(j,j);
00573             if(j > 0) {
00574                for(k=0; k<j; k++) U(k,j)=d*P(k,j);
00575                for(k=0; k<j; k++)
00576                   for(i=0; i<=k; i++)
00577                      P(i,k) -= U(k,j)*U(i,j);
00578             }
00579             if(j==0) break;      // since j is unsigned
00580          }
00581 
00582          // L does not = transpose(U);
00583          P = m;
00584          L = Matrix<T>(m.rows(),m.cols(),T(0));
00585          for(j=0; j<=N-1; j++) {
00586             if(P(j,j) <= T(0)) {
00587                MatrixException e("Cholesky fails - eigenvalue <= 0");
00588                GPSTK_THROW(e);
00589             }
00590             L(j,j) = SQRT(P(j,j));
00591             d = T(1)/L(j,j);
00592             if(j < N-1) {
00593                for(k=j+1; k<N; k++) L(k,j)=d*P(k,j);
00594                for(k=j+1; k<N; k++) {
00595                   for(i=k; i<N; i++) {
00596                      P(i,k) -= L(i,j)*L(k,j);
00597                   }
00598                }
00599             }
00600          }
00601 
00602       }  // end Cholesky::operator()
00603 
00604          /* Use backsubstition to solve the equation A*x=b where *this Cholesky
00605           * has been applied to A, i.e. A = L*transpose(L). The algorithm is in
00606           * two steps: since A*x=L*LT*x=b, first solve L*y=b for y, then solve
00607           * LT*x=y for x. x is returned as b.
00608           */
00609       template <class BaseClass2>
00610       void backSub(RefVectorBase<T, BaseClass2>& b) const
00611          throw (MatrixException)
00612       {
00613          if (L.rows() != b.size())
00614          {
00615             MatrixException e("Vector size does not match dimension of Cholesky");
00616             GPSTK_THROW(e);
00617          }
00618          size_t i,j,N=L.rows();
00619       
00620          Vector<T> y(b.size());
00621          y(0) = b(0)/L(0,0);
00622          for(i=1; i<N; i++) {
00623             y(i) = b(i);
00624             for(j=0; j<i; j++) y(i)-=L(i,j)*y(j);
00625             y(i) /= L(i,i);
00626          }
00627          // b is now x
00628          b(N-1) = y(N-1)/L(N-1,N-1);
00629          for(i=N-1; ; i--) {
00630             b(i) = y(i);
00631             for(j=i+1; j<N; j++) b(i)-=L(j,i)*b(j);
00632             b(i) /= L(i,i);
00633             if(i==0) break;
00634          }
00635 
00636       }  // end Cholesky::backSub
00637 
00639       Matrix<T> L, U;
00640 
00641    }; // end class Cholesky
00642 
00650    template <class T> 
00651    class CholeskyCrout : public Cholesky<T>
00652    {
00653    public:
00654        template <class BaseClass>
00655        void operator() (const ConstMatrixBase<T, BaseClass>& m) throw(MatrixException)
00656        {
00657            if(!m.isSquare()) {
00658                MatrixException e("CholeskyCrout requires a square matrix");
00659                GPSTK_THROW(e);
00660            }
00661 
00662            int N = m.rows(), i, j, k;
00663            double sum;
00664            (*this).L = Matrix<T>(N,N, 0.0);
00665 
00666            for(j=0; j<N; j++) {
00667                sum = m(j,j);
00668                for(k=0; k<j; k++ ) sum -= (*this).L(j,k)*(*this).L(j,k);
00669                if(sum > 0.0) {
00670                    (*this).L(j,j) = SQRT(sum);
00671                } else {
00672                    MatrixException e("CholeskyCrout fails - eigenvalue <= 0");
00673                    GPSTK_THROW(e);          
00674                }
00675 
00676                for(i=j+1; i<N; i++){
00677                    sum = m(i,j);
00678                    for(k=0; k<j; k++) sum -= (*this).L(i,k)*(*this).L(j,k);
00679                    (*this).L(i,j) = sum/(*this).L(j,j);
00680                }
00681            }
00682 
00683            (*this).U = transpose((*this).L);
00684        }
00685 
00686    }; // end class CholeskyCrout
00687 
00693    template <class T>
00694    class Householder
00695    {
00696    public:
00697       Householder() {}
00698 
00709       template <class BaseClass>
00710       inline void operator() (const ConstMatrixBase<T, BaseClass>& m)
00711          throw (MatrixException)
00712          {
00713             A = m;
00714             size_t i,j,k;
00715             Vector<T> v(A.rows());
00716             T sum,alpha;
00717 
00718             // loop over cols
00719             const T EPS(1.e-200);
00720             for(j=0; (j<A.cols() && j<A.rows()-1); j++) {
00721                sum = T(0);
00722                // loop over rows at diagonal and below
00723                for(i=j; i<A.rows(); i++) {
00724                   v(i) = A(i,j);
00725                   A(i,j) = T(0);
00726                   sum += v(i)*v(i);
00727                }
00728 
00729                if(sum < EPS) continue;             // already zero below diagonal
00730 
00731                sum = SQRT(sum);
00732                if(v(j) > T(0)) sum = -sum;
00733                A(j,j) = sum;
00734                v(j) = v(j) - sum;
00735                sum = 1/(sum*v(j));
00736 
00737                // loop over columns beyond j
00738                for(k=j+1; k<A.cols(); k++) {
00739                   alpha = T(0);
00740                   // loop over rows at and below j
00741                   for(i=j; i<A.rows(); i++) {
00742                      alpha += A(i,k)*v(i);
00743                   }
00744                   alpha *= sum;
00745                   // modify column k at and below j
00746                   for(i=j; i<A.rows(); i++) {
00747                      A(i,k) += alpha*v(i);
00748                   }
00749                }  // end loop over cols > j
00750 
00751             }  // end loop over cols
00752 
00753          }  // end Householder::operator()
00754       
00756       Matrix<T> A;
00757 
00758    }; // end class Householder
00759 
00761 
00762 }  // namespace
00763 
00764 #endif