00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
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
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058 template<int M, int N, class Precision = DefaultPrecision, bool WANT_U = 1, bool WANT_V = 1>
00059 class GR_SVD
00060 {
00061 public:
00062
00063 template<class Precision2, class Base> GR_SVD(const Matrix<M, N, Precision2, Base> &A);
00064
00065 static const int BigDim = M>N?M:N;
00066 static const int SmallDim = M<N?M:N;
00067
00068 const Matrix<M,N,Precision>& get_U() { if(!WANT_U) throw(0); return mU;}
00069 const Matrix<N,N,Precision>& get_V() { if(!WANT_V) throw(0); return mV;}
00070 const Vector<N, Precision>& get_diagonal() {return vDiagonal;}
00071
00072 Precision get_largest_singular_value();
00073 Precision get_smallest_singular_value();
00074 int get_smallest_singular_value_index();
00075
00076
00077
00078
00079
00080
00081 void get_inv_diag(Vector<N>& inv_diag, const Precision condition)
00082 {
00083 Precision dMax = get_largest_singular_value();
00084 for(int i=0; i<N; ++i)
00085 inv_diag[i] = (vDiagonal[i] * condition > dMax) ?
00086 static_cast<Precision>(1)/vDiagonal[i] : 0;
00087 }
00088
00089
00090
00091
00092
00093 template <int Rows2, int Cols2, typename P2, typename B2>
00094 Matrix<N,Cols2, typename Internal::MultiplyType<Precision,P2>::type >
00095 backsub(const Matrix<Rows2,Cols2,P2,B2>& rhs, const Precision condition=1e9)
00096 {
00097 Vector<N,Precision> inv_diag;
00098 get_inv_diag(inv_diag,condition);
00099 return (get_V() * diagmult(inv_diag, (get_U().T() * rhs)));
00100 }
00101
00102
00103
00104
00105
00106 template <int Size, typename P2, typename B2>
00107 Vector<N, typename Internal::MultiplyType<Precision,P2>::type >
00108 backsub(const Vector<Size,P2,B2>& rhs, const Precision condition=1e9)
00109 {
00110 Vector<N,Precision> inv_diag;
00111 get_inv_diag(inv_diag,condition);
00112 return (get_V() * diagmult(inv_diag, (get_U().T() * rhs)));
00113 }
00114
00115
00116 Matrix<N,M,Precision> get_pinv(const Precision condition=1e9)
00117 {
00118 Vector<N,Precision> inv_diag(N);
00119 get_inv_diag(inv_diag,condition);
00120 return diagmult(get_V(),inv_diag) * get_U().T();
00121 }
00122
00123
00124 void reorder();
00125
00126 protected:
00127 void Bidiagonalize();
00128 void Accumulate_RHS();
00129 void Accumulate_LHS();
00130 void Diagonalize();
00131 bool Diagonalize_SubLoop(int k, Precision &z);
00132
00133 Vector<N,Precision> vDiagonal;
00134 Vector<BigDim, Precision> vOffDiagonal;
00135 Matrix<M, N, Precision> mU;
00136 Matrix<N, N, Precision> mV;
00137
00138 int nError;
00139 int nIterations;
00140 Precision anorm;
00141 };
00142
00143
00144
00145 template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
00146 template<class Precision2, class Base>
00147 GR_SVD<M, N, Precision, WANT_U, WANT_V>::GR_SVD(const Matrix<M, N, Precision2, Base> &mA)
00148 {
00149 nError = 0;
00150 mU = mA;
00151 Bidiagonalize();
00152 Accumulate_RHS();
00153 Accumulate_LHS();
00154 Diagonalize();
00155 };
00156
00157 template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
00158 void GR_SVD<M,N,Precision, WANT_U, WANT_V>::Bidiagonalize()
00159 {
00160 using std::abs;
00161 using std::max;
00162
00163 Precision g = 0.0;
00164 Precision scale = 0.0;
00165 anorm = 0.0;
00166 for(int i=0; i<N; ++i)
00167 {
00168 const int l = i+1;
00169 vOffDiagonal[i] = scale * g;
00170 g = 0.0;
00171 Precision s = 0.0;
00172 scale = 0.0;
00173 if( i < M )
00174 {
00175 for(int k=i; k<M; ++k)
00176 scale += abs(mU[k][i]);
00177 if(scale != 0.0)
00178 {
00179 for(int k=i; k<M; ++k)
00180 {
00181 mU[k][i] /= scale;
00182 s += mU[k][i] * mU[k][i];
00183 }
00184 Precision f = mU[i][i];
00185 g = -(f>=0?sqrt(s):-sqrt(s));
00186 Precision h = f * g - s;
00187 mU[i][i] = f - g;
00188 if(i!=(N-1))
00189 {
00190 for(int j=l; j<N; ++j)
00191 {
00192 s = 0.0;
00193 for(int k=i; k<M; ++k)
00194 s += mU[k][i] * mU[k][j];
00195 f = s / h;
00196 for(int k=i; k<M; ++k)
00197 mU[k][j] += f * mU[k][i];
00198 }
00199 }
00200 for(int k=i; k<M; ++k)
00201 mU[k][i] *= scale;
00202 }
00203 }
00204 vDiagonal[i] = scale * g;
00205 g = 0.0;
00206 s = 0.0;
00207 scale = 0.0;
00208 if(!(i >= M || i == (N-1)))
00209 {
00210 for(int k=l; k<N; ++k)
00211 scale += abs(mU[i][k]);
00212 if(scale != 0.0)
00213 {
00214 for(int k=l; k<N; k++)
00215 {
00216 mU[i][k] /= scale;
00217 s += mU[i][k] * mU[i][k];
00218 }
00219 Precision f = mU[i][l];
00220 g = -(f>=0?sqrt(s):-sqrt(s));
00221 Precision h = f * g - s;
00222 mU[i][l] = f - g;
00223 for(int k=l; k<N; ++k)
00224 vOffDiagonal[k] = mU[i][k] / h;
00225 if(i != (M-1))
00226 {
00227 for(int j=l; j<M; ++j)
00228 {
00229 s = 0.0;
00230 for(int k=l; k<N; ++k)
00231 s += mU[j][k] * mU[i][k];
00232 for(int k=l; k<N; ++k)
00233 mU[j][k] += s * vOffDiagonal[k];
00234 }
00235 }
00236 for(int k=l; k<N; ++k)
00237 mU[i][k] *= scale;
00238 }
00239 }
00240 anorm = max(anorm, abs(vDiagonal[i]) + abs(vOffDiagonal[i]));
00241 }
00242
00243
00244 }
00245
00246 template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
00247 void GR_SVD<M,N,Precision,WANT_U,WANT_V>::Accumulate_RHS()
00248 {
00249
00250
00251
00252 mV[N-1][N-1] = static_cast<Precision>(1);
00253 Precision g = vOffDiagonal[N-1];
00254
00255
00256 for(int i=N-2; i>=0; --i)
00257 {
00258 const int l = i + 1;
00259 if( g!=0)
00260 {
00261 for(int j=l; j<N; ++j)
00262 mV[j][i] = (mU[i][j] / mU[i][l]) / g;
00263 for(int j=l; j<N; ++j)
00264 {
00265 Precision s = 0;
00266 for(int k=l; k<N; ++k)
00267 s += mU[i][k] * mV[k][j];
00268 for(int k=l; k<N; ++k)
00269 mV[k][j] += s * mV[k][i];
00270 }
00271 }
00272 for(int j=l; j<N; ++j)
00273 mV[i][j] = mV[j][i] = 0;
00274 mV[i][i] = static_cast<Precision>(1);
00275 g = vOffDiagonal[i];
00276 }
00277 }
00278
00279 template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
00280 void GR_SVD<M,N,Precision,WANT_U,WANT_V>::Accumulate_LHS()
00281 {
00282
00283
00284 for(int i=SmallDim-1; i>=0; --i)
00285 {
00286 const int l = i+1;
00287 Precision g = vDiagonal[i];
00288
00289 if(i != (N-1))
00290 for(int j=l; j<N; ++j)
00291 mU[i][j] = 0.0;
00292 if(g == 0.0)
00293 for(int j=i; j<M; ++j)
00294 mU[j][i] = 0.0;
00295 else
00296 {
00297
00298 Precision inv_g = static_cast<Precision>(1) / g;
00299 if(i != (SmallDim-1))
00300 {
00301 for(int j=l; j<N; ++j)
00302 {
00303 Precision s = 0;
00304 for(int k=l; k<M; ++k)
00305 s += mU[k][i] * mU[k][j];
00306 Precision f = (s / mU[i][i]) * inv_g;
00307 for(int k=i; k<M; ++k)
00308 mU[k][j] += f * mU[k][i];
00309 }
00310 }
00311 for(int j=i; j<M; ++j)
00312 mU[j][i] *= inv_g;
00313 }
00314 mU[i][i] += static_cast<Precision>(1);
00315 }
00316 }
00317
00318 template<int M, int N, class Precision,bool WANT_U, bool WANT_V>
00319 void GR_SVD<M,N,Precision,WANT_U,WANT_V>::Diagonalize()
00320 {
00321
00322 for(int k=N-1; k>=0; --k)
00323 {
00324 nIterations = 0;
00325 Precision z;
00326 bool bConverged_Or_Error = false;
00327 do
00328 bConverged_Or_Error = Diagonalize_SubLoop(k, z);
00329 while(!bConverged_Or_Error);
00330
00331 if(nError)
00332 return;
00333
00334 if(z < 0)
00335 {
00336 vDiagonal[k] = -z;
00337 if(WANT_V)
00338 for(int j=0; j<N; ++j)
00339 mV[j][k] = -mV[j][k];
00340 }
00341 }
00342 };
00343
00344
00345 template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
00346 bool GR_SVD<M,N,Precision,WANT_U, WANT_V>::Diagonalize_SubLoop(int k, Precision &z)
00347 {
00348 using std::abs;
00349 const int k1 = k-1;
00350
00351 for(int l=k; l>=0; --l)
00352 {
00353 const int l1 = l-1;
00354 const bool rv1_test = ((abs(vOffDiagonal[l]) + anorm) == anorm);
00355 const bool w_test = ((abs(vDiagonal[l1]) + anorm) == anorm);
00356 if(!rv1_test && w_test)
00357 {
00358 Precision c = 0;
00359 Precision s = 1.0;
00360 for(int i=l; i<=k; ++i)
00361 {
00362 Precision f = s * vOffDiagonal[i];
00363 vOffDiagonal[i] *= c;
00364 if((abs(f) + anorm) == anorm)
00365 break;
00366 Precision g = vDiagonal[i];
00367 Precision h = sqrt(f * f + g * g);
00368 vDiagonal[i] = h;
00369 c = g / h;
00370 s = -f / h;
00371 if(WANT_U)
00372 for(int j=0; j<M; ++j)
00373 {
00374 Precision y = mU[j][l1];
00375 Precision z = mU[j][i];
00376 mU[j][l1] = y*c + z*s;
00377 mU[j][i] = -y*s + z*c;
00378 }
00379 }
00380 }
00381 if(rv1_test || w_test)
00382 {
00383
00384 z = vDiagonal[k];
00385 if(l == k)
00386 return true;
00387 if(nIterations == 30)
00388 {
00389 nError = k;
00390 return true;
00391 }
00392 ++nIterations;
00393 Precision x = vDiagonal[l];
00394 Precision y = vDiagonal[k1];
00395 Precision g = vOffDiagonal[k1];
00396 Precision h = vOffDiagonal[k];
00397 Precision f = ((y-z)*(y+z) + (g-h)*(g+h)) / (2.0*h*y);
00398 g = sqrt(f*f + 1.0);
00399 Precision signed_g = (f>=0)?g:-g;
00400 f = ((x-z)*(x+z) + h*(y/(f + signed_g) - h)) / x;
00401
00402
00403 Precision c = 1.0;
00404 Precision s = 1.0;
00405 for(int i1 = l; i1<=k1; ++i1)
00406 {
00407 const int i=i1+1;
00408 g = vOffDiagonal[i];
00409 y = vDiagonal[i];
00410 h = s*g;
00411 g = c*g;
00412 z = sqrt(f*f + h*h);
00413 vOffDiagonal[i1] = z;
00414 c = f/z;
00415 s = h/z;
00416 f = x*c + g*s;
00417 g = -x*s + g*c;
00418 h = y*s;
00419 y *= c;
00420 if(WANT_V)
00421 for(int j=0; j<N; ++j)
00422 {
00423 Precision xx = mV[j][i1];
00424 Precision zz = mV[j][i];
00425 mV[j][i1] = xx*c + zz*s;
00426 mV[j][i] = -xx*s + zz*c;
00427 }
00428 z = sqrt(f*f + h*h);
00429 vDiagonal[i1] = z;
00430 if(z!=0)
00431 {
00432 c = f/z;
00433 s = h/z;
00434 }
00435 f = c*g + s*y;
00436 x = -s*g + c*y;
00437 if(WANT_U)
00438 for(int j=0; j<M; ++j)
00439 {
00440 Precision yy = mU[j][i1];
00441 Precision zz = mU[j][i];
00442 mU[j][i1] = yy*c + zz*s;
00443 mU[j][i] = -yy*s + zz*c;
00444 }
00445 }
00446 vOffDiagonal[l] = 0;
00447 vOffDiagonal[k] = f;
00448 vDiagonal[k] = x;
00449 return false;
00450
00451 }
00452 }
00453
00454 throw(0);
00455 return false;
00456 }
00457
00458
00459 template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
00460 Precision GR_SVD<M,N,Precision,WANT_U,WANT_V>::get_largest_singular_value()
00461 {
00462 using std::max;
00463 Precision d = vDiagonal[0];
00464 for(int i=1; i<N; ++i) d = max(d, vDiagonal[i]);
00465 return d;
00466 }
00467
00468 template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
00469 Precision GR_SVD<M,N,Precision,WANT_U,WANT_V>::get_smallest_singular_value()
00470 {
00471 using std::min;
00472 Precision d = vDiagonal[0];
00473 for(int i=1; i<N; ++i) d = min(d, vDiagonal[i]);
00474 return d;
00475 }
00476
00477 template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
00478 int GR_SVD<M,N,Precision,WANT_U,WANT_V>::get_smallest_singular_value_index()
00479 {
00480 using std::min;
00481 int nMin=0;
00482 Precision d = vDiagonal[0];
00483 for(int i=1; i<N; ++i)
00484 if(vDiagonal[i] < d)
00485 {
00486 d = vDiagonal[i];
00487 nMin = i;
00488 }
00489 return nMin;
00490 }
00491
00492 template<int M, int N, class Precision, bool WANT_U, bool WANT_V>
00493 void GR_SVD<M,N,Precision,WANT_U,WANT_V>::reorder()
00494 {
00495 std::vector<std::pair<Precision, unsigned int> > vSort;
00496 vSort.reserve(N);
00497 for(unsigned int i=0; i<N; ++i)
00498 vSort.push_back(std::make_pair(-vDiagonal[i], i));
00499 std::sort(vSort.begin(), vSort.end());
00500 for(unsigned int i=0; i<N; ++i)
00501 vDiagonal[i] = -vSort[i].first;
00502 if(WANT_U)
00503 {
00504 Matrix<M, N, Precision> mU_copy = mU;
00505 for(unsigned int i=0; i<N; ++i)
00506 mU.T()[i] = mU_copy.T()[vSort[i].second];
00507 }
00508 if(WANT_V)
00509 {
00510 Matrix<N, N, Precision> mV_copy = mV;
00511 for(unsigned int i=0; i<N; ++i)
00512 mV.T()[i] = mV_copy.T()[vSort[i].second];
00513 }
00514 }
00515
00516 }
00517 #endif
00518
00519
00520
00521
00522
00523