TooN: GR_SVD.h Source File

TooN 2.0.0-beta8

- Main Page
- Related Pages
- Modules
- Namespaces
- Classes
- Files

- File List

GR_SVD.h

    00001 // -*- c++ -*-
    00002
    00003 // Copyright (C) 2009 Georg Klein (gk@robots.ox.ac.uk)
    00004 //
    00005 // This file is part of the TooN Library.  This library is free
    00006 // software; you can redistribute it and/or modify it under the
    00007 // terms of the GNU General Public License as published by the
    00008 // Free Software Foundation; either version 2, or (at your option)
    00009 // any later version.
    00010
    00011 // This library is distributed in the hope that it will be useful,
    00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
    00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    00014 // GNU General Public License for more details.
    00015
    00016 // You should have received a copy of the GNU General Public License along
    00017 // with this library; see the file COPYING.  If not, write to the Free
    00018 // Software Foundation, 59 Temple Place - Suite 330, Boston, MA 02111-1307,
    00019 // USA.
    00020
    00021 // As a special exception, you may use this file as part of a free software
    00022 // library without restriction.  Specifically, if other files instantiate
    00023 // templates or use macros or inline functions from this file, or you compile
    00024 // this file and link it with other files to produce an executable, this
    00025 // file does not by itself cause the resulting executable to be covered by
    00026 // the GNU General Public License.  This exception does not however
    00027 // invalidate any other reasons why the executable file might be covered by
    00028 // the GNU General Public License.
    00029
    00030 #ifndef __GR_SVD_H
    00031 #define __GR_SVD_H
    00032
    00033 #include <TooN/TooN.h>
    00034 #include <cmath>
    00035 #include <vector>
    00036 #include <algorithm>
    00037
    00038 namespace TooN
    00039 {
    00040
    00041   /**
    00042      @class GR_SVD TooN/GR_SVD.h
    00043      Performs SVD and back substitute to solve equations.
    00044      This code is a c++ translation of the FORTRAN routine give in
    00045      George E. Forsythe et al, Computer Methods for Mathematical
    00046      Computations, Prentice-Hall 1977. That code itself is a
    00047      translation of the ALGOL routine by Golub and Reinsch,
    00048      Num. Math. 14, 403-420, 1970.
    00049
    00050      N.b. the singular values returned by this routine are not sorted.
    00051      N.b. this also means that even for MxN matrices with M<N, N
    00052      singular values are computed and used.
    00053
    00054      The template parameters WANT_U and WANT_V may be set to false to
    00055      indicate that U and/or V are not needed for a minor speed-up.
    00056
    00057      @ingroup gDecomps
    00058   **/
    00059   template<int M, int N = M, class Precision = DefaultPrecision, bool WANT_U = 1, bool WANT_V = 1>
    00060   class GR_SVD
    00061   {
    00062   public:
    00063
    00064     template<class Precision2, class Base> GR_SVD(const Matrix<M, N, Precision2, Base> &A);
    00065
    00066     static const int BigDim = M>N?M:N;
    00067     static const int SmallDim = M<N?M:N;
    00068
    00069     const Matrix<M,N,Precision>& get_U() { if(!WANT_U) throw(0); return mU;}
    00070     const Matrix<N,N,Precision>& get_V() { if(!WANT_V) throw(0); return mV;}
    00071     const Vector<N, Precision>& get_diagonal() {return vDiagonal;}
    00072
    00073     Precision get_largest_singular_value();
    00074     Precision get_smallest_singular_value();
    00075     int get_smallest_singular_value_index();
    00076
    00077     ///Return the pesudo-inverse diagonal. The reciprocal of the diagonal elements
    00078     ///is returned if the elements are well scaled with respect to the largest element,
    00079     ///otherwise 0 is returned.
    00080     ///@param inv_diag Vector in which to return the inverse diagonal.
    00081     ///@param condition Elements must be larger than this factor times the largest diagonal element to be considered well scaled.
    00082     void get_inv_diag(Vector<N>& inv_diag, const Precision condition)
    00083     {
    00084       Precision dMax = get_largest_singular_value();
    00085       for(int i=0; i<N; ++i)
    00086     inv_diag[i] = (vDiagonal[i] * condition > dMax) ?
    00087       static_cast<Precision>(1)/vDiagonal[i] : 0;
    00088     }
    00089
    00090     /// Calculate result of multiplying the (pseudo-)inverse of M by another matrix.
    00091     /// For a matrix \f$A\f$, this calculates \f$M^{\dagger}A\f$ by back substitution
    00092     /// (i.e. without explictly calculating the (pseudo-)inverse).
    00093     /// See the detailed description for a description of condition variables.
    00094     template <int Rows2, int Cols2, typename P2, typename B2>
    00095     Matrix<N,Cols2, typename Internal::MultiplyType<Precision,P2>::type >
    00096     backsub(const Matrix<Rows2,Cols2,P2,B2>& rhs, const Precision condition=1e9)
    00097     {
    00098       Vector<N,Precision> inv_diag;
    00099       get_inv_diag(inv_diag,condition);
    00100       return (get_V() * diagmult(inv_diag, (get_U().T() * rhs)));
    00101     }
    00102
    00103     /// Calculate result of multiplying the (pseudo-)inverse of M by a vector.
    00104     /// For a vector \f$b\f$, this calculates \f$M^{\dagger}b\f$ by back substitution
    00105     /// (i.e. without explictly calculating the (pseudo-)inverse).
    00106     /// See the detailed description for a description of condition variables.
    00107     template <int Size, typename P2, typename B2>
    00108     Vector<N, typename Internal::MultiplyType<Precision,P2>::type >
    00109     backsub(const Vector<Size,P2,B2>& rhs, const Precision condition=1e9)
    00110     {
    00111       Vector<N,Precision> inv_diag;
    00112       get_inv_diag(inv_diag,condition);
    00113       return (get_V() * diagmult(inv_diag, (get_U().T() * rhs)));
    00114     }
    00115
    00116     /// Get the pseudo-inverse \f$M^{\dagger}\f$
    00117     Matrix<N,M,Precision> get_pinv(const Precision condition=1e9)
    00118     {
    00119       Vector<N,Precision> inv_diag(N);
    00120       get_inv_diag(inv_diag,condition);
    00121       return diagmult(get_V(),inv_diag) * get_U().T();
    00122     }
    00123
    00124     /// Reorder the components so the singular values are in descending order
    00125     void reorder();
    00126
    00127   protected:
    00128     void Bidiagonalize();
    00129     void Accumulate_RHS();
    00130     void Accumulate_LHS();
    00131     void Diagonalize();
    00132     bool Diagonalize_SubLoop(int k, Precision &z);
    00133
    00134     Vector<N,Precision> vDiagonal;
    00135     Vector<BigDim, Precision> vOffDiagonal;
    00136     Matrix<M, N, Precision> mU;
    00137     Matrix<N, N, Precision> mV;
    00138
    00139     int nError;
    00140     int nIterations;
    00141     Precision anorm;
    00142   };
    00143
    00144
    00145
    00146   template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
    00147   template<class Precision2, class Base>
    00148   GR_SVD<M, N, Precision, WANT_U, WANT_V>::GR_SVD(const Matrix<M, N, Precision2, Base> &mA)
    00149   {
    00150     nError = 0;
    00151     mU = mA;
    00152     Bidiagonalize();
    00153     Accumulate_RHS();
    00154     Accumulate_LHS();
    00155     Diagonalize();
    00156   };
    00157
    00158   template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
    00159   void GR_SVD<M,N,Precision, WANT_U, WANT_V>::Bidiagonalize()
    00160   {
    00161     using std::abs;
    00162     using std::max;
    00163     using std::sqrt;
    00164     // ------------  Householder reduction to bidiagonal form
    00165     Precision g = 0.0;
    00166     Precision scale = 0.0;
    00167     anorm = 0.0;
    00168     for(int i=0; i<N; ++i) // 300
    00169       {
    00170     const int l = i+1;
    00171     vOffDiagonal[i] = scale * g;
    00172     g = 0.0;
    00173     Precision s = 0.0;
    00174     scale = 0.0;
    00175     if( i < M )
    00176       {
    00177         for(int k=i; k<M; ++k)
    00178           scale += abs(mU[k][i]);
    00179         if(scale != 0.0)
    00180           {
    00181         for(int k=i; k<M; ++k)
    00182           {
    00183             mU[k][i] /= scale;
    00184             s += mU[k][i] * mU[k][i];
    00185           }
    00186         Precision f = mU[i][i];
    00187         g = -(f>=0?sqrt(s):-sqrt(s));
    00188         Precision h = f * g - s;
    00189         mU[i][i] = f - g;
    00190         if(i!=(N-1))
    00191           {
    00192             for(int j=l; j<N; ++j)
    00193               {
    00194             s = 0.0;
    00195             for(int k=i; k<M; ++k)
    00196               s += mU[k][i] * mU[k][j];
    00197             f = s / h;
    00198             for(int k=i; k<M; ++k)
    00199               mU[k][j] += f * mU[k][i];
    00200               } // 150
    00201           }// 190
    00202         for(int k=i; k<M; ++k)
    00203           mU[k][i] *= scale;
    00204           } // 210
    00205       } // 210
    00206     vDiagonal[i] = scale * g;
    00207     g = 0.0;
    00208     s = 0.0;
    00209     scale = 0.0;
    00210     if(!(i >= M || i == (N-1)))
    00211       {
    00212         for(int k=l; k<N; ++k)
    00213           scale += abs(mU[i][k]);
    00214         if(scale != 0.0)
    00215           {
    00216         for(int k=l; k<N; k++)
    00217           {
    00218             mU[i][k] /= scale;
    00219             s += mU[i][k] * mU[i][k];
    00220           }
    00221         Precision f = mU[i][l];
    00222         g = -(f>=0?sqrt(s):-sqrt(s));
    00223         Precision h = f * g - s;
    00224         mU[i][l] = f - g;
    00225         for(int k=l; k<N; ++k)
    00226           vOffDiagonal[k] = mU[i][k] / h;
    00227         if(i != (M-1)) // 270
    00228           {
    00229             for(int j=l; j<M; ++j)
    00230               {
    00231             s = 0.0;
    00232             for(int k=l; k<N; ++k)
    00233               s += mU[j][k] * mU[i][k];
    00234             for(int k=l; k<N; ++k)
    00235               mU[j][k] += s * vOffDiagonal[k];
    00236               } // 260
    00237           } // 270
    00238         for(int k=l; k<N; ++k)
    00239           mU[i][k] *= scale;
    00240           } // 290
    00241       } // 290
    00242     anorm = max(anorm, abs(vDiagonal[i]) + abs(vOffDiagonal[i]));
    00243       } // 300
    00244
    00245     // Accumulation of right-hand transformations
    00246   }
    00247
    00248   template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
    00249   void GR_SVD<M,N,Precision,WANT_U,WANT_V>::Accumulate_RHS()
    00250   {
    00251     // Get rid of fakey loop over ii, do a loop over i directly
    00252     // This here would happen on the first run of the loop with
    00253     // i = N-1
    00254     mV[N-1][N-1] = static_cast<Precision>(1);
    00255     Precision g = vOffDiagonal[N-1];
    00256
    00257     // The loop
    00258     for(int i=N-2; i>=0; --i) // 400
    00259       {
    00260     const int l = i + 1;
    00261     if( g!=0) // 360
    00262       {
    00263         for(int j=l; j<N; ++j)
    00264           mV[j][i] = (mU[i][j] / mU[i][l]) / g; // double division avoids possible underflow
    00265         for(int j=l; j<N; ++j)
    00266           { // 350
    00267         Precision s = 0;
    00268         for(int k=l; k<N; ++k)
    00269           s += mU[i][k] * mV[k][j];
    00270         for(int k=l; k<N; ++k)
    00271           mV[k][j] += s * mV[k][i];
    00272           } // 350
    00273       } // 360
    00274     for(int j=l; j<N; ++j)
    00275       mV[i][j] = mV[j][i] = 0;
    00276     mV[i][i] = static_cast<Precision>(1);
    00277     g = vOffDiagonal[i];
    00278       } // 400
    00279   }
    00280
    00281   template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
    00282   void GR_SVD<M,N,Precision,WANT_U,WANT_V>::Accumulate_LHS()
    00283   {
    00284     // Same thing; remove loop over dummy ii and do straight over i
    00285     // Some implementations start from N here
    00286     for(int i=SmallDim-1; i>=0; --i)
    00287       { // 500
    00288     const int l = i+1;
    00289     Precision g = vDiagonal[i];
    00290     // SqSVD here uses i<N ?
    00291     if(i != (N-1))
    00292       for(int j=l; j<N; ++j)
    00293         mU[i][j] = 0.0;
    00294     if(g == 0.0)
    00295       for(int j=i; j<M; ++j)
    00296         mU[j][i] = 0.0;
    00297     else
    00298       { // 475
    00299         // Can pre-divide g here
    00300         Precision inv_g = static_cast<Precision>(1) / g;
    00301         if(i != (SmallDim-1))
    00302           { // 460
    00303         for(int j=l; j<N; ++j)
    00304           { // 450
    00305             Precision s = 0;
    00306             for(int k=l; k<M; ++k)
    00307               s += mU[k][i] * mU[k][j];
    00308             Precision f = (s / mU[i][i]) * inv_g;  // double division
    00309             for(int k=i; k<M; ++k)
    00310               mU[k][j] += f * mU[k][i];
    00311           } // 450
    00312           } // 460
    00313         for(int j=i; j<M; ++j)
    00314           mU[j][i] *= inv_g;
    00315       } // 475
    00316     mU[i][i] += static_cast<Precision>(1);
    00317       } // 500
    00318   }
    00319
    00320   template<int M, int N, class Precision,bool WANT_U, bool WANT_V>
    00321   void GR_SVD<M,N,Precision,WANT_U,WANT_V>::Diagonalize()
    00322   {
    00323     // Loop directly over descending k
    00324     for(int k=N-1; k>=0; --k)
    00325       { // 700
    00326     nIterations = 0;
    00327     Precision z;
    00328     bool bConverged_Or_Error = false;
    00329     do
    00330       bConverged_Or_Error = Diagonalize_SubLoop(k, z);
    00331     while(!bConverged_Or_Error);
    00332
    00333     if(nError)
    00334       return;
    00335
    00336     if(z < 0)
    00337       {
    00338         vDiagonal[k] = -z;
    00339         if(WANT_V)
    00340           for(int j=0; j<N; ++j)
    00341         mV[j][k] = -mV[j][k];
    00342       }
    00343       } // 700
    00344   };
    00345
    00346
    00347   template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
    00348   bool GR_SVD<M,N,Precision,WANT_U, WANT_V>::Diagonalize_SubLoop(int k, Precision &z)
    00349   {
    00350     using std::abs;
    00351     using std::sqrt;
    00352     const int k1 = k-1;
    00353     // 520 is here!
    00354     for(int l=k; l>=0; --l)
    00355       { // 530
    00356     const int l1 = l-1;
    00357     if((abs(vOffDiagonal[l]) + anorm) == anorm)
    00358         goto line_565;
    00359     if((abs(vDiagonal[l1]) + anorm) == anorm)
    00360         goto line_540;
    00361     continue;
    00362
    00363     line_540:
    00364       {
    00365         Precision c = 0;
    00366         Precision s = 1.0;
    00367         for(int i=l; i<=k; ++i)
    00368           { // 560
    00369         Precision f = s * vOffDiagonal[i];
    00370         vOffDiagonal[i] *= c;
    00371         if((abs(f) + anorm) == anorm)
    00372           break; // goto 565, effectively
    00373         Precision g = vDiagonal[i];
    00374         Precision h = sqrt(f * f + g * g); // Other implementations do this bit better
    00375         vDiagonal[i] = h;
    00376         c = g / h;
    00377         s = -f / h;
    00378         if(WANT_U)
    00379           for(int j=0; j<M; ++j)
    00380             {
    00381               Precision y = mU[j][l1];
    00382               Precision z = mU[j][i];
    00383               mU[j][l1] = y*c + z*s;
    00384               mU[j][i] = -y*s + z*c;
    00385             }
    00386           } // 560
    00387       }
    00388
    00389     line_565:
    00390       {
    00391         // Check for convergence..
    00392         z = vDiagonal[k];
    00393         if(l == k)
    00394           return true; // convergence.
    00395         if(nIterations == 30)
    00396           {
    00397         nError = k;
    00398         return true;
    00399           }
    00400         ++nIterations;
    00401         Precision x = vDiagonal[l];
    00402         Precision y = vDiagonal[k1];
    00403         Precision g = vOffDiagonal[k1];
    00404         Precision h = vOffDiagonal[k];
    00405         Precision f = ((y-z)*(y+z) + (g-h)*(g+h)) / (2.0*h*y);
    00406         g = sqrt(f*f + 1.0);
    00407         Precision signed_g =  (f>=0)?g:-g;
    00408         f = ((x-z)*(x+z) + h*(y/(f + signed_g) - h)) / x;
    00409
    00410         // Next QR transformation
    00411         Precision c = 1.0;
    00412         Precision s = 1.0;
    00413         for(int i1 = l; i1<=k1; ++i1)
    00414           { // 600
    00415         const int i=i1+1;
    00416         g = vOffDiagonal[i];
    00417         y = vDiagonal[i];
    00418         h = s*g;
    00419         g = c*g;
    00420         z = sqrt(f*f + h*h);
    00421         vOffDiagonal[i1] = z;
    00422         c = f/z;
    00423         s = h/z;
    00424         f = x*c + g*s;
    00425         g = -x*s + g*c;
    00426         h = y*s;
    00427         y *= c;
    00428         if(WANT_V)
    00429           for(int j=0; j<N; ++j)
    00430             {
    00431               Precision xx = mV[j][i1];
    00432               Precision zz = mV[j][i];
    00433               mV[j][i1] = xx*c + zz*s;
    00434               mV[j][i] = -xx*s + zz*c;
    00435             }
    00436         z = sqrt(f*f + h*h);
    00437         vDiagonal[i1] = z;
    00438         if(z!=0)
    00439           {
    00440             c = f/z;
    00441             s = h/z;
    00442           }
    00443         f = c*g + s*y;
    00444         x = -s*g + c*y;
    00445         if(WANT_U)
    00446           for(int j=0; j<M; ++j)
    00447             {
    00448               Precision yy = mU[j][i1];
    00449               Precision zz = mU[j][i];
    00450               mU[j][i1] = yy*c + zz*s;
    00451               mU[j][i] = -yy*s + zz*c;
    00452             }
    00453           } // 600
    00454         vOffDiagonal[l] = 0;
    00455         vOffDiagonal[k] = f;
    00456         vDiagonal[k] = x;
    00457         return false;
    00458         // EO IF NOT CONVERGED CHUNK
    00459       } // EO IF TESTS HOLD
    00460       } // 530
    00461     // Code should never get here!
    00462     throw(0);
    00463     //return false;
    00464   }
    00465
    00466
    00467   template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
    00468   Precision GR_SVD<M,N,Precision,WANT_U,WANT_V>::get_largest_singular_value()
    00469   {
    00470     using std::max;
    00471     Precision d = vDiagonal[0];
    00472     for(int i=1; i<N; ++i) d = max(d, vDiagonal[i]);
    00473     return d;
    00474   }
    00475
    00476   template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
    00477   Precision GR_SVD<M,N,Precision,WANT_U,WANT_V>::get_smallest_singular_value()
    00478   {
    00479     using std::min;
    00480     Precision d = vDiagonal[0];
    00481     for(int i=1; i<N; ++i) d = min(d, vDiagonal[i]);
    00482     return d;
    00483   }
    00484
    00485   template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
    00486   int GR_SVD<M,N,Precision,WANT_U,WANT_V>::get_smallest_singular_value_index()
    00487   {
    00488     using std::min;
    00489     int nMin=0;
    00490     Precision d = vDiagonal[0];
    00491     for(int i=1; i<N; ++i)
    00492       if(vDiagonal[i] < d)
    00493     {
    00494       d = vDiagonal[i];
    00495       nMin = i;
    00496     }
    00497     return nMin;
    00498   }
    00499
    00500   template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
    00501   void GR_SVD<M,N,Precision,WANT_U,WANT_V>::reorder()
    00502   {
    00503     std::vector<std::pair<Precision, unsigned int> > vSort;
    00504     vSort.reserve(N);
    00505     for(unsigned int i=0; i<N; ++i)
    00506       vSort.push_back(std::make_pair(-vDiagonal[i], i));
    00507     std::sort(vSort.begin(), vSort.end());
    00508     for(unsigned int i=0; i<N; ++i)
    00509       vDiagonal[i] = -vSort[i].first;
    00510     if(WANT_U)
    00511       {
    00512     Matrix<M, N, Precision> mU_copy = mU;
    00513     for(unsigned int i=0; i<N; ++i)
    00514       mU.T()[i] = mU_copy.T()[vSort[i].second];
    00515       }
    00516     if(WANT_V)
    00517       {
    00518     Matrix<N, N, Precision> mV_copy = mV;
    00519     for(unsigned int i=0; i<N; ++i)
    00520       mV.T()[i] = mV_copy.T()[vSort[i].second];
    00521       }
    00522   }
    00523
    00524 }
    00525 #endif
    00526
    00527
    00528
    00529
    00530
    00531