matrixfunctors.hpp

gps源代码

#pragma ident "$Id: MatrixFunctors.hpp 142 2006-09-22 16:51:26Z architest $"



/**
 * @file MatrixFunctors.hpp
 * Matrix function operators (SVD, LUD, etc)
 */
 
#ifndef GPSTK_MATRIX_FUNCTORS_HPP
#define GPSTK_MATRIX_FUNCTORS_HPP

//============================================================================
//
//  This file is part of GPSTk, the GPS Toolkit.
//
//  The GPSTk is free software; you can redistribute it and/or modify
//  it under the terms of the GNU Lesser General Public License as published
//  by the Free Software Foundation; either version 2.1 of the License, or
//  any later version.
//
//  The GPSTk is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//  GNU Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with GPSTk; if not, write to the Free Software Foundation,
//  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//  
//  Copyright 2004, The University of Texas at Austin
//
//============================================================================

#include <cmath>

namespace gpstk
{

 /** @addtogroup VectorGroup */
   //@{

/**
 * Class SVD: A function object for the singular value decomposition of a matrix.
 * Given a matrix A [m,n], the SVD of A = U*S*transpose(V), where U is [m,m],
 * V is [n,n], and S is [m,n] (like A). Both U and V are unitary [meaning
 * transpose(U)*U = unity = transpose(V)*V] and the columns of U[resp,V]
 * are orthonormal vectors spanning the space A*transpose(A) [transpose(A)*A].
 * Note that U*transpose(U)=1 and V*transpose(V)=1 are not true in general,
 * but may be. S[m,n] is 'diagonal' in the sense that only diagonal elements
 * are non-zero (even when m != n); the min(m,n) diagonal elements are called
 * the singular values of A, often referred to as S[i]. The singular values
 * may be sorted, as the SVD is invariant under a consistent re-ordering of
 * {singular values / columns of U / columns of V}.
 * The condition number of A is the ratio
 *    cn = fabs(largest S[i])/fabs(smallest S[i]).
 * Note that inverse(A) = V*inverse(S)*UT where inverse(S) is diagonal with
 * elements equal to the inverse of elements of S, and with dimension [n,m].
 * The matrix A is non-singular matrix if and only if all of its singular
 * values are non-zero. If some of the singular values are zero, the
 * 'generalized inverse' of A may be formed by editing the singular values
 * in this way: if the ratio of S[i] to S[0] (where S[0] is the largest
 * singular value) is bigger than some tolerance (1.e-7 is good), then 1/S[i]
 * is set to zero in the inverse. In this way the 'generalized inverse' of
 * ANY matrix is guaranteed to exist.
 * The SVD algorithm never fails.
 *
 * Ref: Bulirsch and Stoer, "Introduction to Numerical Analysis,"
 * NY, Springer-Verlag, 1980.
 *
 * @code
 * Matrix<double> m(and is assigned some value);
 * SVD<double> d;
 * d(m);
 * cout << d.U << endl << d.V << endl << d.S << endl;
 * @endcode
 */
   template <class T>
   class SVD
   {
   public:
      SVD() : iterationMax(30) {}

         /**
          * Singular Value Decomposition
          */
      template <class BaseClass>
      bool operator() (const ConstMatrixBase<T, BaseClass>& mat)
         throw (MatrixException)
         {
            bool flip=false;
            U = mat;
            if(mat.rows() > mat.cols()) {
               flip = true;
               U = transpose(mat);
            }

            size_t n(U.cols()), m(U.rows());
            size_t i, j, k, l, nm, jj, kk;
            T anorm(0), scale(0), g(0), s, f, h, c, x, y, z;

            V = Matrix<T>(n, n, T(0));
            S = Vector<T>(n, T(1));
            Vector<T> B(n, T(1));

            for (i = 0; i < n; i++) {
               l = i + 1;
               B[i] = scale * g;
               g = s = scale = T(0);
               if (i < m) {
                  for(k = i; k < m; k++) scale += ABS(U(k, i));
                  if (scale) {
                     for(k = i; k < m; k++) {
                        U(k, i) /= scale;
                        s += U(k, i) * U(k, i);
                     }
                     f = U(i, i);
                     g = -SIGN(SQRT(s),f);
                     h = f * g - s;
                     U(i,i) = f - g;
                     for(j = l; j < n; j++) {
                        for(s = T(0), k = i; k < m; k++) s += U(k, i) * U(k, j);
                        f = s / h;
                        for(k = i; k < m; k++) U(k, j) += f * U(k, i);
                     }
                     for(k = i; k < m; k++) U(k, i) *= scale;
                  } // if (scale)
               }  // if (i < m)
               S[i] = scale * g;
               g = s = scale = T(0);
               if ( (i < m) && (i != n-1) ) {
                  for(k = l; k < n; k++) scale += ABS(U(i, k));
                  if (scale) {
                     for(k = l; k < n; k++) {
                        U(i, k) /= scale;
                        s += U(i, k) * U(i, k);
                     }
                     f = U(i, l);
                     g = -SIGN(SQRT(s),f);
                     h = f * g - s;
                     U(i, l) = f - g;
                     for(k = l; k < n; k++) B[k] = U(i, k) / h;
                     for(j = l; j < m; j++) {
                        for(s = T(0), k = l; k < n; k++) s += U(j, k) * U(i, k);
                        for(k = l; k < n; k++) U(j, k) += s * B[k];
                     }
                     for(k = l; k < n; k++) U(i, k) *= scale;
                  }
               }
               if(ABS(S[i])+ABS(B[i]) > anorm) anorm=ABS(S[i])+ABS(B[i]);
            }
            for(i = n - 1; ; i--) {
               if (i < n - 1) {
                  if (g) {
                     for(j = l; j < n; j++) V(j, i) = (U(i, j) / U(i, l)) / g;
                     for(j = l; j < n; j++) {
                        for(s = T(0), k = l; k < n; k++) s += U(i, k) * V(k, j);
                        for(k = l; k < n; k++) V(k, j) += s * V(k, i);
                     }
                  }
                  for(j = l; j < n; j++) V(j, i) = V(i, j) = T(0);
               }
               V(i,i) =T(1);
               g = B[i];
               l = i;
               if(i==0) break;
            }
            for(i = ( (m-1 < n-1) ? m-1 : n-1); ; i--) {
               l = i+1;
               g = S[i];
               for(j=l; j<n; j++) U(i, j) = T(0);
               if (g) {
                  g = T(1) / g;
                  for(j = l; j < n; j++) {
                     for(s = T(0), k = l; k < m; k++) s += U(k,i) * U(k,j);
                     f = (s / U(i,i)) * g;
                     for(k=i; k<m; k++) U(k,j) += f * U(k,i);
                  }
                  for(j = i; j < m; j++) U(j,i) *= g;
               } 
               else {
                  for(j=i; j<m; j++) U(j,i) = T(0);
               }
               ++U(i,i);
               if(i==0) break;
            }
            for(k = n - 1; ; k--) {
               size_t its;
               for(its = 1; its <= iterationMax; its++) {
                  bool flag = true;
                  for(l = k; ; l--) {
                     nm = l - 1;
                     if ((ABS(B[l])+anorm) == anorm) {
                        flag = false;
                        break;
                     }
                     if (l == 0) { // should never happen
                        MatrixException e("SVD algorithm has nm==-1");
                        GPSTK_THROW(e);
                     }
                     if ((ABS(S[nm])+anorm) == anorm) break;
                     if(l == 0) break; // since l is unsigned...
                  }
                  if (flag) {
                     c = T(0);
                     s = T(1);
                     for(i = l; i <= k; i++) {
                        f = s * B[i];
                        B[i] = c * B[i];
                        if ((ABS(f) + anorm) == anorm) break;
                        g = S[i];
                        h = RSS(f,g);
                        S[i] = h;
                        h = T(1) / h;
                        c = g * h;
                        s = -f * h;
                        for(j = 0; j < m; j++) {
                           y = U(j, nm);
                           z = U(j,i);
                           U(j, nm) = y * c + z * s;
                           U(j,i) = z * c - y * s;
                        }
                     }
                  }
                  z = S[k];
                  if (l == k) {
                     if (z < T(0)) {
                        S[k] = -z;
                        for(j = 0; j < n; j++) V(j,k) = -V(j,k);
                     }
                     break;
                  }
            
                  if (its == iterationMax) {
                     MatrixException e("SVD algorithm did not converge");
                     GPSTK_THROW(e);
                  }
                  x = S[l];
                  if(k == 0) { // should never happen
                     MatrixException e("SVD algorithm has k==0");
                     GPSTK_THROW(e);
                  }
                  nm = k - 1;
                  y = S[nm];
                  g = B[nm];
                  h = B[k];
                  f = ( (y-z) * (y+z) + (g-h) * (g+h)) / (T(2) * h * y);
                  g = RSS(f,T(1));
                  f = ( (x-z) * (x+z) + h * ((y/(f + SIGN(g,f))) - h)) / x;
                  c = s = 1.0;
                  for(j = l; j <= nm; j++) {
                     i = j + 1;
                     g = B[i];
                     y = S[i];
                     h = s * g;
                     g = c * g;
                     z = RSS(f, h);
                     B[j] = z;
                     c = f / z;
                     s = h / z;
                     f = x * c + g * s;
                     g = g * c - x * s;
                     h = y * s;
                     y *= c;
                     for(jj = 0; jj < n; jj++) {
                        x = V(jj, j);
                        z = V(jj, i);
                        V(jj, j) = x * c + z * s;
                        V(jj, i) = z * c - x * s;
                     }
                     z = RSS(f, h);
                     S[j] = z;
                     if (z) {
                        z = T(1) / z;
                        c = f * z;
                        s = h * z;
                     }
                     f = c * g + s * y;
                     x = c * y - s * g;
                     for(jj = 0; jj < m; jj++) {
                        y = U(jj, j);
                        z = U(jj, i);
                        U(jj, j) = y * c + z * s;
                        U(jj, i) = z * c - y * s;
                     }
                  }
                  B[l] = T(0);
                  B[k] = f;
                  S[k] = x;
               }
               if(k==0) break;   // since k is unsigned...
            }
               // if U is not square - last n-m columns of U are zero - remove
            if(U.cols() > U.rows()) {
               for(i=1; i<S.size(); i++) {   // sort in descending order
                  T sv=S[i],svj;
                  kk = i-1;
                  while(kk >= 0) {
                     svj = S[kk];
                     if(sv < svj) break;
                     S[kk+1] = svj;
                     // exchange columns kk and kk+1 in U and V
                     U.swapCols(kk,kk+1);
                     V.swapCols(kk,kk+1);
                     kk = kk - 1;
                  }
                  S[kk+1] = sv;
               }
               Matrix<T> Temp(U);
               U = Matrix<T>(Temp,0,0,Temp.rows(),Temp.rows());
               S.resize(Temp.rows());
            }

            if(flip) {
               Matrix<T> Temp(U);
               U = V;
               V = Temp;
            }

            return true;

         }  // end SVD::operator() - the SVD algorithm
   
         /** Backsubstitution using SVD.
           * Solve A*x=b for vector x where A [mxn] has been SVD'ed and is given by
           * U,W,V (*this); that is A[mxn] = U[mxm]*W[mxn]*VT[nxn]. b has dimension m,
           * x dimension n. Singular values are NOT edited, except that if s.v. == 0,
           * 1/0 is replaced by 0. Result is returned as b.
         */
      template <class BaseClass>
      void backSub(RefVectorBase<T, BaseClass>& b) const 
         throw(MatrixException)
      {
         if(b.size() != U.rows())
         {
             MatrixException e("SVD::BackSub called with unequal dimensions");
             GPSTK_THROW(e);
         }
   
         size_t i, n=V.cols(), m=U.rows();
         Matrix<T> W(n,m,T(0));     // build the 'inverse singular values' matrix
         for(i=0; i<S.size(); i++) W(i,i)=(S(i)==T(0)?T(0):T(1)/S(i));
         Vector<T> Y;
         Y = V*W*transpose(U)*b;
         //b = Y;
         // this fails because operator= is not defined for the base class
         // (op= not inherited); use assignFrom instead
         b.assignFrom(Y);

      }  // end SVD::backSub

         /// sort singular values
      void sort(bool descending)
         throw(MatrixException)
      {
         size_t i;
         int j;         // j must be allowed to go negative
         for(i=1; i<S.size(); i++) {
            T sv=S(i),svj;
            j = i - 1;
            while(j >= 0) {
               svj = S(j);
               if(descending && sv < svj) break;
               if(!descending && sv > svj) break;
               S(j+1) = svj;
               // exchange columns j and j+1 in U and V
               U.swapCols(j,j+1);
               V.swapCols(j,j+1);
               j = j - 1;
            }
            S(j+1) = sv;
         }
      }  // end SVD::sort

         /// compute determinant from SVD
      inline T det(void)
         throw(MatrixException)
      {
         T d(1);
         for(size_t i=0; i<S.size(); i++) d *= S(i);
         return d;
      }  // end SVD::det