MatrixFunctors.hpp

Go to the documentation of this file.

//==============================================================================
//
//  This file is part of GNSSTk, the ARL:UT GNSS Toolkit.
//
//  The GNSSTk 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 3.0 of the License, or
//  any later version.
//
//  The GNSSTk 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 GNSSTk; if not, write to the Free Software Foundation,
//  Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110, USA
//
//  This software was developed by Applied Research Laboratories at the
//  University of Texas at Austin.
//  Copyright 2004-2022, The Board of Regents of The University of Texas System
//
//==============================================================================
 
//==============================================================================
//
//  This software was developed by Applied Research Laboratories at the
//  University of Texas at Austin, under contract to an agency or agencies
//  within the U.S. Department of Defense. The U.S. Government retains all
//  rights to use, duplicate, distribute, disclose, or release this software.
//
//  Pursuant to DoD Directive 523024
//
//  DISTRIBUTION STATEMENT A: This software has been approved for public
//                            release, distribution is unlimited.
//
//==============================================================================
 
 
#ifndef GNSSTK_MATRIX_FUNCTORS_HPP
#define GNSSTK_MATRIX_FUNCTORS_HPP
 
#include <cmath>
#include <limits>
#include <algorithm>
 
namespace gnsstk
{
 
 
 
   template <class T>
   class SVD
   {
   public:
      SVD() : iterationMax(30) {}
 
      template <class BaseClass>
      bool operator() (const ConstMatrixBase<T, BaseClass>& mat)
      {
         const T eps=T(8)*std::numeric_limits<T>::epsilon();
         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;
         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])/MAX(ABS(B[l]),anorm) < eps) {
                     flag = false;
                     break;
                  }
                  if (l == 0) { // should never happen
                     MatrixException e("SVD algorithm has nm==-1");
                     GNSSTK_THROW(e);
                  }
                  if (ABS(S[nm])/MAX(ABS(S[nm]),anorm) < eps) 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)/MAX(ABS(f),anorm) < eps) 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");
                  GNSSTK_THROW(e);
               }
               x = S[l];
               if(k == 0) { // should never happen
                  MatrixException e("SVD algorithm has k==0");
                  GNSSTK_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;
               int kk = i-1;  // not size_t ~ unsigned
               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
 
 
      template <class BaseClass>
      void backSub(RefVectorBase<T, BaseClass>& b) const
      {
         if(b.size() != U.rows())
         {
            MatrixException e("SVD::BackSub called with unequal dimensions");
            GNSSTK_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
 
      void sort(bool descending=true)
      {
         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
 
      inline T det(void)
      {
         T d(1);
         for(size_t i=0; i<S.size(); i++) d *= S(i);
         return d;
      }  // end SVD::det
 
      Matrix<T> U;
      Vector<T> S;
      Matrix<T> V;
 
   private:
      const size_t iterationMax;
 
      T SIGN(T a, T b)
      {
         if (b >= T(0))
            return ABS(a);
         else
            return -ABS(a);
      }
 
   }; // end class SVD
 
   template <class T>
   class LUDecomp
   {
   public:
      LUDecomp() {}        // why is there no constructor from ConstMatrixBase?
 
      template <class BaseClass>
      void operator() (const ConstMatrixBase<T, BaseClass>& m)
      {
         if(!m.isSquare() || m.rows()<1) {
            MatrixException e("LUDecomp requires a square, non-trivial matrix");
            GNSSTK_THROW(e);
         }
 
         size_t N=m.rows(),i,j,k,imax;
         T big,t,d;
         Vector<T> V(N,T(0));
 
         LU = m;
         Pivot = Vector<int>(N);
         parity = 1;
 
         for(i=0; i<N; i++) {    // get scale of each row
            big = T(0);
            for(j=0; j<N; j++) {
               t = ABS(LU(i,j));
               if(t > big) big=t;
            }
            if(big <= T(0)) {    // m is singular
                  //LU *= T(0);
               SingularMatrixException e("singular matrix!");
               GNSSTK_THROW(e);
            }
            V(i) = T(1)/big;
         }
 
         for(j=0; j<N; j++) {    // loop over columns
            for(i=0; i<j; i++) {
               t = LU(i,j);
               for(k=0; k<i; k++) t -= LU(i,k)*LU(k,j);
               LU(i,j) = t;
            }
            big = T(0);          // find largest pivot
            for(i=j; i<N; i++) {
               t = LU(i,j);
               for(k=0; k<j; k++) t -= LU(i,k)*LU(k,j);
               LU(i,j) = t;
               d = V(i)*ABS(t);
               if(d >= big) {
                  big = d;
                  imax = i;
               }
            }
            if(j != imax) {
               LU.swapRows(imax,j);
               V(imax) = V(j);
               parity = -parity;
            }
            Pivot(j) = imax;
 
            t = LU(j,j);
            if(t == 0.0) {       // m is singular
                  //LU *= T(0);
               SingularMatrixException e("singular matrix!");
               GNSSTK_THROW(e);
            }
            if(j != N-1) {
               d = T(1)/t;
               for(i=j+1; i<N; i++) LU(i,j) *= d;
            }
         }
      }  // end LUDecomp()
 
      template <class BaseClass2>
      void backSub(RefVectorBase<T, BaseClass2>& v) const
      {
         if(LU.rows() != v.size()) {
            MatrixException e("Vector size does not match dimension of LUDecomp");
            GNSSTK_THROW(e);
         }
 
         bool first=true;
         size_t N=LU.rows(),i,j,ii(0);
         T sum;
 
            // un-pivot
         for(i=0; i<N; i++) {
            sum = v(Pivot(i));
            v(Pivot(i)) = v(i);
            if(first && sum != T(0)) {
               ii = i;
               first = false;
            }
            else for(j=ii; j<i; j++) sum -= LU(i,j)*v(j);
            v(i) = sum;
         }
            // back substitution
         for(i=N-1; ; i--) {
            sum = v(i);
            for(j=i+1; j<N; j++) sum -= LU(i,j)*v(j);
            v(i) = sum / LU(i,i);
            if(i == 0) break;       // b/c i is unsigned
         }
      }  // end LUD::backSub
 
      inline T det(void)
      {
         T d(static_cast<T>(parity));
         for(size_t i=0; i<LU.rows(); i++) d *= LU(i,i);
         return d;
      }
 
      Matrix<T> LU;
      Vector<int> Pivot;
      int parity;
 
   }; // end class LUDecomp
 
 
      // Compute Cholesky decomposition (triangular square root) of the given matrix,
      // which must be square and positive definite. That is, if M is square and positive
      // definite, then M = C * transpose(C) where C is the Cholesky decomposition. This
      // routine computes both the upper (U) and lower (L) triangular C's;
      // thus M = L * transpose(L) = U * transpose(U).
      //
      // NB Note that M = U*UT NOT UT*U; if M = U'T*U' is desired, use U'=transpose(L);
      // then note the similarity to LU decomposition: M = L*U' = L*LT = U'T*U'.
      //
      // Positive definite (PD) implies positive eigenvalues, and the reverse.
      //
      // Note that the result is not unique, as any orthogonal transformation R of UT,
      // including a change in sign, will preserve the result m = U*UT = U*RT*R*UT.
      //
      // @code
      // Matrix<double> M(i,i);     // square and PD
      // Ch(M);
      // cout << Ch.U << endl << Ch.L << endl;
      // // note that M = Ch.L * transpose(Ch.L);
      // // note that M = Ch.U * transpose(Ch.U);
      // // and that if Matrix<double> Up(transpose(Ch.L)); then
      // // M = transpose(Up) * Up;
      // @endcode
   template <class T>
   class Cholesky
   {
   public:
      Cholesky() {}
 
      template <class BaseClass>
      void operator() (const ConstMatrixBase<T, BaseClass>& m)
      {
         if(!m.isSquare()) {
            MatrixException e("Cholesky requires a square matrix");
            GNSSTK_THROW(e);
         }
 
         size_t N=m.rows(),i,j,k;
         double d;
         Matrix<T> P(m);
         U = Matrix<T>(m.rows(),m.cols(),T(0));
 
         for(j=N-1; ; j--) {
            if(P(j,j) <= T(0)) {
               MatrixException e("Cholesky fails - eigenvalue <= 0");
               GNSSTK_THROW(e);
            }
            U(j,j) = SQRT(P(j,j));
            d = T(1)/U(j,j);
            if(j > 0) {
               for(k=0; k<j; k++) U(k,j)=d*P(k,j);
               for(k=0; k<j; k++)
                  for(i=0; i<=k; i++)
                     P(i,k) -= U(k,j)*U(i,j);
            }
            if(j==0) break;      // since j is unsigned
         }
 
         P = m;
         L = Matrix<T>(m.rows(),m.cols(),T(0));
         for(j=0; j<=N-1; j++) {
            if(P(j,j) <= T(0)) {
               MatrixException e("Cholesky fails - eigenvalue <= 0");
               GNSSTK_THROW(e);
            }
            L(j,j) = SQRT(P(j,j));
            d = T(1)/L(j,j);
            if(j < N-1) {
               for(k=j+1; k<N; k++) L(k,j)=d*P(k,j);
               for(k=j+1; k<N; k++) {
                  for(i=k; i<N; i++) {
                     P(i,k) -= L(i,j)*L(k,j);
                  }
               }
            }
         }
 
      }  // end Cholesky::operator()
 
      template <class BaseClass2>
      void backSub(RefVectorBase<T, BaseClass2>& b) const
      {
         if (L.rows() != b.size())
         {
            MatrixException e("Vector size does not match dimension of Cholesky");
            GNSSTK_THROW(e);
         }
         size_t i,j,N=L.rows();
 
         Vector<T> y(b.size());
         y(0) = b(0)/L(0,0);
         for(i=1; i<N; i++) {
            y(i) = b(i);
            for(j=0; j<i; j++) y(i)-=L(i,j)*y(j);
            y(i) /= L(i,i);
         }
            // b is now x
         b(N-1) = y(N-1)/L(N-1,N-1);
         for(i=N-1; ; i--) {
            b(i) = y(i);
            for(j=i+1; j<N; j++) b(i)-=L(j,i)*b(j);
            b(i) /= L(i,i);
            if(i==0) break;
         }
 
      }  // end Cholesky::backSub
 
      Matrix<T> L, U;
 
   }; // end class Cholesky
 
   template <class T>
   class CholeskyCrout : public Cholesky<T>
   {
   public:
      template <class BaseClass>
      void operator() (const ConstMatrixBase<T, BaseClass>& m)
      {
         if(!m.isSquare()) {
            MatrixException e("CholeskyCrout requires a square matrix");
            GNSSTK_THROW(e);
         }
 
         int N = m.rows(), i, j, k;
         double sum;
         (*this).L = Matrix<T>(N,N, 0.0);
 
         for(j=0; j<N; j++) {
            sum = m(j,j);
            for(k=0; k<j; k++ ) sum -= (*this).L(j,k)*(*this).L(j,k);
            if(sum > 0.0) {
               (*this).L(j,j) = SQRT(sum);
            } else {
               MatrixException e("CholeskyCrout fails - eigenvalue <= 0");
               GNSSTK_THROW(e);
            }
 
            for(i=j+1; i<N; i++){
               sum = m(i,j);
               for(k=0; k<j; k++) sum -= (*this).L(i,k)*(*this).L(j,k);
               (*this).L(i,j) = sum/(*this).L(j,j);
            }
         }
 
         (*this).U = transpose((*this).L);
      }
 
   }; // end class CholeskyCrout
 
 
      // The Householder transformation is simply an orthogonal transformation
      // designed to make the elements below the diagonal zero. It applies to any
      // matrix.
   template <class T>
   class Householder
   {
   public:
      Householder() {}
 
      template <class BaseClass>
      inline void operator() (const ConstMatrixBase<T, BaseClass>& m)
      {
         A = m;
         size_t i,j,k;
         Vector<T> v(A.rows());
         T sum,alpha;
 
            // loop over cols
         const T EPS(1.e-200);
         for(j=0; (j<A.cols()-1 && j<A.rows()-1); j++) {
            sum = T(0);
               // loop over rows at diagonal and below
            for(i=j; i<A.rows(); i++) {
               v(i) = A(i,j);
               A(i,j) = T(0);       // this is optional - below the diag is trash
               sum += v(i)*v(i);
            }
 
            if(sum < EPS) continue;             // already zero below diagonal
 
            sum = SQRT(sum);
            if(v(j) > T(0)) sum = -sum;
            A(j,j) = sum;
            v(j) = v(j) - sum;
            sum = T(1)/(sum*v(j));
 
               // loop over columns beyond j
            for(k=j+1; k<A.cols(); k++) {
               alpha = T(0);
                  // loop over rows at and below j
               for(i=j; i<A.rows(); i++) {
                  alpha += A(i,k)*v(i);
               }
               alpha *= sum;
               if(alpha*alpha < EPS) continue;
                  // modify column k at and below j
               for(i=j; i<A.rows(); i++) {
                  A(i,k) += alpha*v(i);
               }
            }  // end loop over cols > j
 
         }  // end loop over cols
 
      }  // end Householder::operator()
 
      Matrix<T> A;
 
   }; // end class Householder
 
 
}  // namespace
 
#endif