TooN 2.1
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