matrix.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright 2011. All rights reserved.
00003 Institute of Measurement and Control Systems
00004 Karlsruhe Institute of Technology, Germany
00005 
00006 This file is part of libviso2.
00007 Authors: Andreas Geiger
00008 
00009 libviso2 is free software; you can redistribute it and/or modify it under the
00010 terms of the GNU General Public License as published by the Free Software
00011 Foundation; either version 2 of the License, or any later version.
00012 
00013 libviso2 is distributed in the hope that it will be useful, but WITHOUT ANY
00014 WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
00015 PARTICULAR PURPOSE. See the GNU General Public License for more details.
00016 
00017 You should have received a copy of the GNU General Public License along with
00018 libviso2; if not, write to the Free Software Foundation, Inc., 51 Franklin
00019 Street, Fifth Floor, Boston, MA 02110-1301, USA 
00020 */
00021 
00022 #include "matrix.h"
00023 #include <math.h>
00024 
00025 #define SWAP(a,b) {temp=a;a=b;b=temp;}
00026 #define SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
00027 static FLOAT sqrarg;
00028 #define SQR(a) ((sqrarg=(a)) == 0.0 ? 0.0 : sqrarg*sqrarg)
00029 static FLOAT maxarg1,maxarg2;
00030 #define FMAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))
00031 static int32_t iminarg1,iminarg2;
00032 #define IMIN(a,b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ? (iminarg1) : (iminarg2))
00033 
00034 
00035 using namespace std;
00036 
00037 Matrix::Matrix () {
00038   m   = 0;
00039   n   = 0;
00040   val = 0;
00041 }
00042 
00043 Matrix::Matrix (const int32_t m_,const int32_t n_) {
00044   allocateMemory(m_,n_);
00045 }
00046 
00047 Matrix::Matrix (const int32_t m_,const int32_t n_,const FLOAT* val_) {
00048   allocateMemory(m_,n_);
00049   int32_t k=0;
00050   for (int32_t i=0; i<m_; i++)
00051     for (int32_t j=0; j<n_; j++)
00052       val[i][j] = val_[k++];
00053 }
00054 
00055 Matrix::Matrix (const Matrix &M) {
00056   allocateMemory(M.m,M.n);
00057   for (int32_t i=0; i<M.m; i++)
00058     memcpy(val[i],M.val[i],M.n*sizeof(FLOAT));
00059 }
00060 
00061 Matrix::~Matrix () {
00062   releaseMemory();
00063 }
00064 
00065 Matrix& Matrix::operator= (const Matrix &M) {
00066   if (this!=&M) {
00067     if (M.m!=m || M.n!=n) {
00068       releaseMemory();
00069       allocateMemory(M.m,M.n);
00070     }
00071     if (M.n>0)
00072       for (int32_t i=0; i<M.m; i++)
00073         memcpy(val[i],M.val[i],M.n*sizeof(FLOAT));
00074   }
00075   return *this;
00076 }
00077 
00078 void Matrix::getData(FLOAT* val_,int32_t i1,int32_t j1,int32_t i2,int32_t j2) {
00079   if (i2==-1) i2 = m-1;
00080   if (j2==-1) j2 = n-1;
00081   int32_t k=0;
00082   for (int32_t i=i1; i<=i2; i++)
00083     for (int32_t j=j1; j<=j2; j++)
00084       val_[k++] = val[i][j];
00085 }
00086 
00087 Matrix Matrix::getMat(int32_t i1,int32_t j1,int32_t i2,int32_t j2) {
00088   if (i2==-1) i2 = m-1;
00089   if (j2==-1) j2 = n-1;
00090   if (i1<0 || i2>=m || j1<0 || j2>=n || i2<i1 || j2<j1) {
00091     cerr << "ERROR: Cannot get submatrix [" << i1 << ".." << i2 <<
00092         "] x [" << j1 << ".." << j2 << "]" <<
00093         " of a (" << m << "x" << n << ") matrix." << endl;
00094     exit(0);
00095   }
00096   Matrix M(i2-i1+1,j2-j1+1);
00097   for (int32_t i=0; i<M.m; i++)
00098     for (int32_t j=0; j<M.n; j++)
00099       M.val[i][j] = val[i1+i][j1+j];
00100   return M;
00101 }
00102 
00103 void Matrix::setMat(const Matrix &M,const int32_t i1,const int32_t j1) {
00104   if (i1<0 || j1<0 || i1+M.m>m || j1+M.n>n) {
00105     cerr << "ERROR: Cannot set submatrix [" << i1 << ".." << i1+M.m-1 <<
00106         "] x [" << j1 << ".." << j1+M.n-1 << "]" <<
00107         " of a (" << m << "x" << n << ") matrix." << endl;
00108     exit(0);
00109   }
00110   for (int32_t i=0; i<M.m; i++)
00111     for (int32_t j=0; j<M.n; j++)
00112       val[i1+i][j1+j] = M.val[i][j];
00113 }
00114 
00115 void Matrix::setVal(FLOAT s,int32_t i1,int32_t j1,int32_t i2,int32_t j2) {
00116   if (i2==-1) i2 = m-1;
00117   if (j2==-1) j2 = n-1;
00118   if (i2<i1 || j2<j1) {
00119     cerr << "ERROR in setVal: Indices must be ordered (i1<=i2, j1<=j2)." << endl;
00120     exit(0);
00121   }
00122   for (int32_t i=i1; i<=i2; i++)
00123     for (int32_t j=j1; j<=j2; j++)
00124       val[i][j] = s;
00125 }
00126 
00127 void Matrix::setDiag(FLOAT s,int32_t i1,int32_t i2) {
00128   if (i2==-1) i2 = min(m-1,n-1);
00129   for (int32_t i=i1; i<=i2; i++)
00130     val[i][i] = s;
00131 }
00132 
00133 void Matrix::zero() {
00134   setVal(0);
00135 }
00136 
00137 Matrix Matrix::extractCols (vector<int> idx) {
00138   Matrix M(m,idx.size());
00139   for (int32_t j=0; j<M.n; j++)
00140     if (idx[j]<n)
00141       for (int32_t i=0; i<m; i++)
00142         M.val[i][j] = val[i][idx[j]];
00143   return M;
00144 }
00145 
00146 Matrix Matrix::eye (const int32_t m) {
00147   Matrix M(m,m);
00148   for (int32_t i=0; i<m; i++)
00149     M.val[i][i] = 1;
00150   return M;
00151 }
00152 
00153 void Matrix::eye () {
00154   for (int32_t i=0; i<m; i++)
00155     for (int32_t j=0; j<n; j++)
00156       val[i][j] = 0;
00157   for (int32_t i=0; i<min(m,n); i++)
00158     val[i][i] = 1;
00159 }
00160 
00161 Matrix Matrix::diag (const Matrix &M) {
00162   if (M.m>1 && M.n==1) {
00163     Matrix D(M.m,M.m);
00164     for (int32_t i=0; i<M.m; i++)
00165       D.val[i][i] = M.val[i][0];
00166     return D;
00167   } else if (M.m==1 && M.n>1) {
00168     Matrix D(M.n,M.n);
00169     for (int32_t i=0; i<M.n; i++)
00170       D.val[i][i] = M.val[0][i];
00171     return D;
00172   }
00173   cout << "ERROR: Trying to create diagonal matrix from vector of size (" << M.m << "x" << M.n << ")" << endl;
00174   exit(0);
00175 }
00176 
00177 Matrix Matrix::reshape(const Matrix &M,int32_t m_,int32_t n_) {
00178   if (M.m*M.n != m_*n_) {
00179     cerr << "ERROR: Trying to reshape a matrix of size (" << M.m << "x" << M.n <<
00180             ") to size (" << m_ << "x" << n_ << ")" << endl;
00181     exit(0);
00182   }
00183   Matrix M2(m_,n_);
00184   for (int32_t k=0; k<m_*n_; k++) {
00185     int32_t i1 = k/M.n;
00186     int32_t j1 = k%M.n;
00187     int32_t i2 = k/n_;
00188     int32_t j2 = k%n_;
00189     M2.val[i2][j2] = M.val[i1][j1];
00190   }
00191   return M2;
00192 }
00193 
00194 Matrix Matrix::rotMatX (const FLOAT &angle) {
00195   FLOAT s = sin(angle);
00196   FLOAT c = cos(angle);
00197   Matrix R(3,3);
00198   R.val[0][0] = +1;
00199   R.val[1][1] = +c;
00200   R.val[1][2] = -s;
00201   R.val[2][1] = +s;
00202   R.val[2][2] = +c;
00203   return R;
00204 }
00205 
00206 Matrix Matrix::rotMatY (const FLOAT &angle) {
00207   FLOAT s = sin(angle);
00208   FLOAT c = cos(angle);
00209   Matrix R(3,3);
00210   R.val[0][0] = +c;
00211   R.val[0][2] = +s;
00212   R.val[1][1] = +1;
00213   R.val[2][0] = -s;
00214   R.val[2][2] = +c;
00215   return R;
00216 }
00217 
00218 Matrix Matrix::rotMatZ (const FLOAT &angle) {
00219   FLOAT s = sin(angle);
00220   FLOAT c = cos(angle);
00221   Matrix R(3,3);
00222   R.val[0][0] = +c;
00223   R.val[0][1] = -s;
00224   R.val[1][0] = +s;
00225   R.val[1][1] = +c;
00226   R.val[2][2] = +1;
00227   return R;
00228 }
00229 
00230 Matrix Matrix::operator+ (const Matrix &M) {
00231   const Matrix &A = *this;
00232   const Matrix &B = M;
00233   if (A.m!=B.m || A.n!=B.n) {
00234     cerr << "ERROR: Trying to add matrices of size (" << A.m << "x" << A.n <<
00235         ") and (" << B.m << "x" << B.n << ")" << endl;
00236     exit(0);
00237   }
00238   Matrix C(A.m,A.n);
00239   for (int32_t i=0; i<m; i++)
00240     for (int32_t j=0; j<n; j++)
00241       C.val[i][j] = A.val[i][j]+B.val[i][j];
00242   return C;
00243 }
00244 
00245 Matrix Matrix::operator- (const Matrix &M) {
00246   const Matrix &A = *this;
00247   const Matrix &B = M;
00248   if (A.m!=B.m || A.n!=B.n) {
00249     cerr << "ERROR: Trying to subtract matrices of size (" << A.m << "x" << A.n <<
00250         ") and (" << B.m << "x" << B.n << ")" << endl;
00251     exit(0);
00252   }
00253   Matrix C(A.m,A.n);
00254   for (int32_t i=0; i<m; i++)
00255     for (int32_t j=0; j<n; j++)
00256       C.val[i][j] = A.val[i][j]-B.val[i][j];
00257   return C;
00258 }
00259 
00260 Matrix Matrix::operator* (const Matrix &M) {
00261   const Matrix &A = *this;
00262   const Matrix &B = M;
00263   if (A.n!=B.m) {
00264     cerr << "ERROR: Trying to multiply matrices of size (" << A.m << "x" << A.n <<
00265         ") and (" << B.m << "x" << B.n << ")" << endl;
00266     exit(0);
00267   }
00268   Matrix C(A.m,B.n);
00269   for (int32_t i=0; i<A.m; i++)
00270     for (int32_t j=0; j<B.n; j++)
00271       for (int32_t k=0; k<A.n; k++)
00272         C.val[i][j] += A.val[i][k]*B.val[k][j];
00273   return C;
00274 }
00275 
00276 Matrix Matrix::operator* (const FLOAT &s) {
00277   Matrix C(m,n);
00278   for (int32_t i=0; i<m; i++)
00279     for (int32_t j=0; j<n; j++)
00280       C.val[i][j] = val[i][j]*s;
00281   return C;
00282 }
00283 
00284 Matrix Matrix::operator/ (const Matrix &M) {
00285   const Matrix &A = *this;
00286   const Matrix &B = M;
00287   
00288   if (A.m==B.m && A.n==B.n) {
00289     Matrix C(A.m,A.n);
00290     for (int32_t i=0; i<A.m; i++)
00291       for (int32_t j=0; j<A.n; j++)
00292         if (B.val[i][j]!=0)
00293           C.val[i][j] = A.val[i][j]/B.val[i][j];
00294     return C;
00295     
00296   } else if (A.m==B.m && B.n==1) {
00297     Matrix C(A.m,A.n);
00298     for (int32_t i=0; i<A.m; i++)
00299       for (int32_t j=0; j<A.n; j++)
00300         if (B.val[i][0]!=0)
00301           C.val[i][j] = A.val[i][j]/B.val[i][0];
00302     return C;
00303     
00304   } else if (A.n==B.n && B.m==1) {
00305     Matrix C(A.m,A.n);
00306     for (int32_t i=0; i<A.m; i++)
00307       for (int32_t j=0; j<A.n; j++)
00308         if (B.val[0][j]!=0)
00309           C.val[i][j] = A.val[i][j]/B.val[0][j];
00310     return C;
00311     
00312   } else {
00313     cerr << "ERROR: Trying to divide matrices of size (" << A.m << "x" << A.n <<
00314         ") and (" << B.m << "x" << B.n << ")" << endl;
00315     exit(0);
00316   } 
00317 }
00318 
00319 Matrix Matrix::operator/ (const FLOAT &s) {
00320   if (fabs(s)<1e-20) {
00321     cerr << "ERROR: Trying to divide by zero!" << endl;
00322     exit(0);
00323   }
00324   Matrix C(m,n);
00325   for (int32_t i=0; i<m; i++)
00326     for (int32_t j=0; j<n; j++)
00327       C.val[i][j] = val[i][j]/s;
00328   return C;
00329 }
00330 
00331 Matrix Matrix::operator- () {
00332   Matrix C(m,n);
00333   for (int32_t i=0; i<m; i++)
00334     for (int32_t j=0; j<n; j++)
00335       C.val[i][j] = -val[i][j];
00336   return C;
00337 }
00338 
00339 Matrix Matrix::operator~ () {
00340   Matrix C(n,m);
00341   for (int32_t i=0; i<m; i++)
00342     for (int32_t j=0; j<n; j++)
00343       C.val[j][i] = val[i][j];
00344   return C;
00345 }
00346 
00347 FLOAT Matrix::l2norm () {
00348   FLOAT norm = 0;
00349   for (int32_t i=0; i<m; i++)
00350     for (int32_t j=0; j<n; j++)
00351       norm += val[i][j]*val[i][j];
00352   return sqrt(norm);
00353 }
00354 
00355 FLOAT Matrix::mean () {
00356   FLOAT mean = 0;
00357   for (int32_t i=0; i<m; i++)
00358     for (int32_t j=0; j<n; j++)
00359       mean += val[i][j];
00360   return mean/(FLOAT)(m*n);
00361 }
00362 
00363 Matrix Matrix::cross (const Matrix &a, const Matrix &b) {
00364   if (a.m!=3 || a.n!=1 || b.m!=3 || b.n!=1) {
00365     cerr << "ERROR: Cross product vectors must be of size (3x1)" << endl;
00366     exit(0);
00367   }
00368   Matrix c(3,1);
00369   c.val[0][0] = a.val[1][0]*b.val[2][0]-a.val[2][0]*b.val[1][0];
00370   c.val[1][0] = a.val[2][0]*b.val[0][0]-a.val[0][0]*b.val[2][0];
00371   c.val[2][0] = a.val[0][0]*b.val[1][0]-a.val[1][0]*b.val[0][0];
00372   return c;
00373 }
00374 
00375 Matrix Matrix::inv (const Matrix &M) {
00376   if (M.m!=M.n) {
00377     cerr << "ERROR: Trying to invert matrix of size (" << M.m << "x" << M.n << ")" << endl;
00378     exit(0);
00379   }
00380   Matrix A(M);
00381   Matrix B = eye(M.m);
00382   B.solve(A);
00383   return B;
00384 }
00385 
00386 bool Matrix::inv () {
00387   if (m!=n) {
00388     cerr << "ERROR: Trying to invert matrix of size (" << m << "x" << n << ")" << endl;
00389     exit(0);
00390   }
00391   Matrix A(*this);
00392   eye();
00393   solve(A);
00394   return true;
00395 }
00396 
00397 FLOAT Matrix::det () {
00398   
00399   if (m != n) {
00400     cerr << "ERROR: Trying to compute determinant of a matrix of size (" << m << "x" << n << ")" << endl;
00401     exit(0);
00402   }
00403     
00404   Matrix A(*this);
00405   int32_t *idx = (int32_t*)malloc(m*sizeof(int32_t));
00406   FLOAT d = 1;
00407   A.lu(idx,d);
00408   for( int32_t i=0; i<m; i++)
00409     d *= A.val[i][i];
00410   free(idx);
00411   return d;
00412 }
00413 
00414 bool Matrix::solve (const Matrix &M, FLOAT eps) {
00415   
00416   // substitutes
00417   const Matrix &A = M;
00418   Matrix &B       = *this;
00419   
00420   if (A.m != A.n || A.m != B.m || A.m<1 || B.n<1) {
00421     cerr << "ERROR: Trying to eliminate matrices of size (" << A.m << "x" << A.n <<
00422             ") and (" << B.m << "x" << B.n << ")" << endl;
00423     exit(0);
00424   }
00425   
00426   // index vectors for bookkeeping on the pivoting
00427   int32_t* indxc = new int32_t[m];
00428   int32_t* indxr = new int32_t[m];
00429   int32_t* ipiv  = new int32_t[m];
00430   
00431   // loop variables
00432   int32_t i, icol, irow, j, k, l, ll;
00433   FLOAT big, dum, pivinv, temp;
00434   
00435   // initialize pivots to zero
00436   for (j=0;j<m;j++) ipiv[j]=0;
00437   
00438   // main loop over the columns to be reduced
00439   for (i=0;i<m;i++) {
00440     
00441     big=0.0;
00442     
00443     // search for a pivot element
00444     for (j=0;j<m;j++)
00445       if (ipiv[j]!=1)
00446         for (k=0;k<m;k++)
00447           if (ipiv[k]==0)
00448             if (fabs(A.val[j][k])>=big) {
00449       big=fabs(A.val[j][k]);
00450       irow=j;
00451       icol=k;
00452             }
00453     ++(ipiv[icol]);
00454     
00455     // We now have the pivot element, so we interchange rows, if needed, to put the pivot
00456     // element on the diagonal. The columns are not physically interchanged, only relabeled.
00457     if (irow != icol) {
00458       for (l=0;l<m;l++) SWAP(A.val[irow][l], A.val[icol][l])
00459       for (l=0;l<n;l++) SWAP(B.val[irow][l], B.val[icol][l])
00460     }
00461     
00462     indxr[i]=irow; // We are now ready to divide the pivot row by the
00463     indxc[i]=icol; // pivot element, located at irow and icol.
00464     
00465     // check for singularity
00466     if (fabs(A.val[icol][icol]) < eps) {
00467       delete[] indxc;
00468       delete[] indxr;
00469       delete[] ipiv;
00470       return false;
00471     }
00472     
00473     pivinv=1.0/A.val[icol][icol];
00474     A.val[icol][icol]=1.0;
00475     for (l=0;l<m;l++) A.val[icol][l] *= pivinv;
00476     for (l=0;l<n;l++) B.val[icol][l] *= pivinv;
00477     
00478     // Next, we reduce the rows except for the pivot one
00479     for (ll=0;ll<m;ll++)
00480       if (ll!=icol) {
00481       dum = A.val[ll][icol];
00482       A.val[ll][icol] = 0.0;
00483       for (l=0;l<m;l++) A.val[ll][l] -= A.val[icol][l]*dum;
00484       for (l=0;l<n;l++) B.val[ll][l] -= B.val[icol][l]*dum;
00485       }
00486   }
00487   
00488   // This is the end of the main loop over columns of the reduction. It only remains to unscramble
00489   // the solution in view of the column interchanges. We do this by interchanging pairs of
00490   // columns in the reverse order that the permutation was built up.
00491   for (l=m-1;l>=0;l--) {
00492     if (indxr[l]!=indxc[l])
00493       for (k=0;k<m;k++)
00494         SWAP(A.val[k][indxr[l]], A.val[k][indxc[l]])
00495   }
00496   
00497   // success
00498   delete[] indxc;
00499   delete[] indxr;
00500   delete[] ipiv;
00501   return true;
00502 }
00503 
00504 // Given a matrix a[1..n][1..n], this routine replaces it by the LU decomposition of a rowwise
00505 // permutation of itself. a and n are input. a is output, arranged as in equation (2.3.14) above;
00506 // indx[1..n] is an output vector that records the row permutation effected by the partial
00507 // pivoting; d is output as ±1 depending on whether the number of row interchanges was even
00508 // or odd, respectively. This routine is used in combination with lubksb to solve linear equations
00509 // or invert a matrix.
00510 
00511 bool Matrix::lu(int32_t *idx, FLOAT &d, FLOAT eps) {
00512   
00513   if (m != n) {
00514     cerr << "ERROR: Trying to LU decompose a matrix of size (" << m << "x" << n << ")" << endl;
00515     exit(0);
00516   }
00517   
00518   int32_t i,imax,j,k;
00519   FLOAT   big,dum,sum,temp;
00520   FLOAT* vv = (FLOAT*)malloc(n*sizeof(FLOAT)); // vv stores the implicit scaling of each row.
00521   d = 1.0;
00522   for (i=0; i<n; i++) { // Loop over rows to get the implicit scaling information.
00523     big = 0.0;
00524     for (j=0; j<n; j++)
00525       if ((temp=fabs(val[i][j]))>big)
00526         big = temp;
00527     if (big == 0.0) { // No nonzero largest element.
00528       free(vv);
00529       return false;
00530     }
00531     vv[i] = 1.0/big; // Save the scaling.
00532   }
00533   for (j=0; j<n; j++) { // This is the loop over columns of Crout’s method.
00534     for (i=0; i<j; i++) { // This is equation (2.3.12) except for i = j.
00535       sum = val[i][j];
00536       for (k=0; k<i; k++)
00537         sum -= val[i][k]*val[k][j];
00538       val[i][j] = sum;
00539     }
00540     big = 0.0; // Initialize the search for largest pivot element.
00541     for (i=j; i<n; i++) {
00542       sum = val[i][j];
00543       for (k=0; k<j; k++)
00544         sum -= val[i][k]*val[k][j];
00545       val[i][j] = sum;
00546       if ( (dum=vv[i]*fabs(sum))>=big) {
00547         big  = dum;
00548         imax = i;
00549       }
00550     }
00551     if (j!=imax) { // Do we need to interchange rows?
00552       for (k=0; k<n; k++) { // Yes, do so...
00553         dum          = val[imax][k];
00554         val[imax][k] = val[j][k];
00555         val[j][k]    = dum;
00556       }
00557       d = -d;     // ...and change the parity of d.
00558       vv[imax]=vv[j]; // Also interchange the scale factor.
00559     }
00560     idx[j] = imax;
00561     if (j!=n-1) { // Now, finally, divide by the pivot element.
00562       dum = 1.0/val[j][j];
00563       for (i=j+1; i<n; i++)
00564         val[i][j] *= dum;
00565     }
00566   } // Go back for the next column in the reduction.
00567   
00568   // success
00569   free(vv);
00570   return true;
00571 }
00572 
00573 // Given a matrix M/A[1..m][1..n], this routine computes its singular value decomposition, M/A =
00574 // U·W·V T. Thematrix U replaces a on output. The diagonal matrix of singular values W is output
00575 // as a vector w[1..n]. Thematrix V (not the transpose V T ) is output as v[1..n][1..n].
00576 void Matrix::svd(Matrix &U2,Matrix &W,Matrix &V) {
00577 
00578   Matrix U = Matrix(*this);
00579   U2 = Matrix(m,m);
00580   V  = Matrix(n,n);
00581 
00582   FLOAT* w   = (FLOAT*)malloc(n*sizeof(FLOAT));
00583   FLOAT* rv1 = (FLOAT*)malloc(n*sizeof(FLOAT));
00584 
00585   int32_t flag,i,its,j,jj,k,l,nm;
00586   FLOAT   anorm,c,f,g,h,s,scale,x,y,z;
00587 
00588   g = scale = anorm = 0.0; // Householder reduction to bidiagonal form.
00589   for (i=0;i<n;i++) {
00590     l = i+1;
00591     rv1[i] = scale*g;
00592     g = s = scale = 0.0;
00593     if (i < m) {
00594       for (k=i;k<m;k++) scale += fabs(U.val[k][i]);
00595       if (scale) {
00596         for (k=i;k<m;k++) {
00597           U.val[k][i] /= scale;
00598           s += U.val[k][i]*U.val[k][i];
00599         }
00600         f = U.val[i][i];
00601         g = -SIGN(sqrt(s),f);
00602         h = f*g-s;
00603         U.val[i][i] = f-g;
00604         for (j=l;j<n;j++) {
00605           for (s=0.0,k=i;k<m;k++) s += U.val[k][i]*U.val[k][j];
00606           f = s/h;
00607           for (k=i;k<m;k++) U.val[k][j] += f*U.val[k][i];
00608         }
00609         for (k=i;k<m;k++) U.val[k][i] *= scale;
00610       }
00611     }
00612     w[i] = scale*g;
00613     g = s = scale = 0.0;
00614     if (i<m && i!=n-1) {
00615       for (k=l;k<n;k++) scale += fabs(U.val[i][k]);
00616       if (scale) {
00617         for (k=l;k<n;k++) {
00618           U.val[i][k] /= scale;
00619           s += U.val[i][k]*U.val[i][k];
00620         }
00621         f = U.val[i][l];
00622         g = -SIGN(sqrt(s),f);
00623         h = f*g-s;
00624         U.val[i][l] = f-g;
00625         for (k=l;k<n;k++) rv1[k] = U.val[i][k]/h;
00626         for (j=l;j<m;j++) {
00627           for (s=0.0,k=l;k<n;k++) s += U.val[j][k]*U.val[i][k];
00628           for (k=l;k<n;k++) U.val[j][k] += s*rv1[k];
00629         }
00630         for (k=l;k<n;k++) U.val[i][k] *= scale;
00631       }
00632     }
00633     anorm = FMAX(anorm,(fabs(w[i])+fabs(rv1[i])));
00634   }
00635   for (i=n-1;i>=0;i--) { // Accumulation of right-hand transformations.
00636     if (i<n-1) {
00637       if (g) {
00638         for (j=l;j<n;j++) // Double division to avoid possible underflow.
00639           V.val[j][i]=(U.val[i][j]/U.val[i][l])/g;
00640         for (j=l;j<n;j++) {
00641           for (s=0.0,k=l;k<n;k++) s += U.val[i][k]*V.val[k][j];
00642           for (k=l;k<n;k++) V.val[k][j] += s*V.val[k][i];
00643         }
00644       }
00645       for (j=l;j<n;j++) V.val[i][j] = V.val[j][i] = 0.0;
00646     }
00647     V.val[i][i] = 1.0;
00648     g = rv1[i];
00649     l = i;
00650   }
00651   for (i=IMIN(m,n)-1;i>=0;i--) { // Accumulation of left-hand transformations.
00652     l = i+1;
00653     g = w[i];
00654     for (j=l;j<n;j++) U.val[i][j] = 0.0;
00655     if (g) {
00656       g = 1.0/g;
00657       for (j=l;j<n;j++) {
00658         for (s=0.0,k=l;k<m;k++) s += U.val[k][i]*U.val[k][j];
00659         f = (s/U.val[i][i])*g;
00660         for (k=i;k<m;k++) U.val[k][j] += f*U.val[k][i];
00661       }
00662       for (j=i;j<m;j++) U.val[j][i] *= g;
00663     } else for (j=i;j<m;j++) U.val[j][i]=0.0;
00664     ++U.val[i][i];
00665   }
00666   for (k=n-1;k>=0;k--) { // Diagonalization of the bidiagonal form: Loop over singular values,
00667     for (its=0;its<30;its++) { // and over allowed iterations.
00668       flag = 1;
00669       for (l=k;l>=0;l--) { // Test for splitting.
00670         nm = l-1;
00671         if ((FLOAT)(fabs(rv1[l])+anorm) == anorm) { flag = 0; break; }
00672         if ((FLOAT)(fabs( w[nm])+anorm) == anorm) { break; }
00673       }
00674       if (flag) {
00675         c = 0.0; // Cancellation of rv1[l], if l > 1.
00676         s = 1.0;
00677         for (i=l;i<=k;i++) {
00678           f = s*rv1[i];
00679           rv1[i] = c*rv1[i];
00680           if ((FLOAT)(fabs(f)+anorm) == anorm) break;
00681           g = w[i];
00682           h = pythag(f,g);
00683           w[i] = h;
00684           h = 1.0/h;
00685           c = g*h;
00686           s = -f*h;
00687           for (j=0;j<m;j++) {
00688             y = U.val[j][nm];
00689             z = U.val[j][i];
00690             U.val[j][nm] = y*c+z*s;
00691             U.val[j][i]  = z*c-y*s;
00692           }
00693         }
00694       }
00695       z = w[k];
00696       if (l==k) { // Convergence.
00697         if (z<0.0) { // Singular value is made nonnegative.
00698           w[k] = -z;
00699           for (j=0;j<n;j++) V.val[j][k] = -V.val[j][k];
00700         }
00701         break;
00702       }
00703       if (its == 29)
00704         cerr << "ERROR in SVD: No convergence in 30 iterations" << endl;
00705       x = w[l]; // Shift from bottom 2-by-2 minor.
00706       nm = k-1;
00707       y = w[nm];
00708       g = rv1[nm];
00709       h = rv1[k];
00710       f = ((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
00711       g = pythag(f,1.0);
00712       f = ((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
00713       c = s = 1.0; // Next QR transformation:
00714       for (j=l;j<=nm;j++) {
00715         i = j+1;
00716         g = rv1[i];
00717         y = w[i];
00718         h = s*g;
00719         g = c*g;
00720         z = pythag(f,h);
00721         rv1[j] = z;
00722         c = f/z;
00723         s = h/z;
00724         f = x*c+g*s;
00725         g = g*c-x*s;
00726         h = y*s;
00727         y *= c;
00728         for (jj=0;jj<n;jj++) {
00729           x = V.val[jj][j];
00730           z = V.val[jj][i];
00731           V.val[jj][j] = x*c+z*s;
00732           V.val[jj][i] = z*c-x*s;
00733         }
00734         z = pythag(f,h);
00735         w[j] = z; // Rotation can be arbitrary if z = 0.
00736         if (z) {
00737           z = 1.0/z;
00738           c = f*z;
00739           s = h*z;
00740         }
00741         f = c*g+s*y;
00742         x = c*y-s*g;
00743         for (jj=0;jj<m;jj++) {
00744           y = U.val[jj][j];
00745           z = U.val[jj][i];
00746           U.val[jj][j] = y*c+z*s;
00747           U.val[jj][i] = z*c-y*s;
00748         }
00749       }
00750       rv1[l] = 0.0;
00751       rv1[k] = f;
00752       w[k] = x;
00753     }
00754   }
00755   
00756   // sort singular values and corresponding columns of u and v
00757   // by decreasing magnitude. Also, signs of corresponding columns are
00758   // flipped so as to maximize the number of positive elements.
00759   int32_t s2,inc=1;
00760   FLOAT   sw;
00761   FLOAT* su = (FLOAT*)malloc(m*sizeof(FLOAT));
00762   FLOAT* sv = (FLOAT*)malloc(n*sizeof(FLOAT));
00763   do { inc *= 3; inc++; } while (inc <= n);
00764   do {
00765     inc /= 3;
00766     for (i=inc;i<n;i++) {
00767       sw = w[i];
00768       for (k=0;k<m;k++) su[k] = U.val[k][i];
00769       for (k=0;k<n;k++) sv[k] = V.val[k][i];
00770       j = i;
00771       while (w[j-inc] < sw) {
00772         w[j] = w[j-inc];
00773         for (k=0;k<m;k++) U.val[k][j] = U.val[k][j-inc];
00774         for (k=0;k<n;k++) V.val[k][j] = V.val[k][j-inc];
00775         j -= inc;
00776         if (j < inc) break;
00777       }
00778       w[j] = sw;
00779       for (k=0;k<m;k++) U.val[k][j] = su[k];
00780       for (k=0;k<n;k++) V.val[k][j] = sv[k];
00781     }
00782   } while (inc > 1);
00783   for (k=0;k<n;k++) { // flip signs
00784     s2=0;
00785     for (i=0;i<m;i++) if (U.val[i][k] < 0.0) s2++;
00786     for (j=0;j<n;j++) if (V.val[j][k] < 0.0) s2++;
00787     if (s2 > (m+n)/2) {
00788       for (i=0;i<m;i++) U.val[i][k] = -U.val[i][k];
00789       for (j=0;j<n;j++) V.val[j][k] = -V.val[j][k];
00790     }
00791   }
00792 
00793   // create vector and copy singular values
00794   W = Matrix(min(m,n),1,w);
00795   
00796   // extract mxm submatrix U
00797   U2.setMat(U.getMat(0,0,m-1,min(m-1,n-1)),0,0);
00798 
00799   // release temporary memory
00800   free(w);
00801   free(rv1);
00802   free(su);
00803   free(sv);
00804 }
00805 
00806 ostream& operator<< (ostream& out,const Matrix& M) {
00807   if (M.m==0 || M.n==0) {
00808     out << "[empty matrix]";
00809   } else {
00810     char buffer[1024];
00811     for (int32_t i=0; i<M.m; i++) {
00812       for (int32_t j=0; j<M.n; j++) {
00813         sprintf(buffer,"%12.7f ",M.val[i][j]);
00814         out << buffer;
00815       }
00816       if (i<M.m-1)
00817         out << endl;
00818     }
00819   }
00820   return out;
00821 }
00822 
00823 void Matrix::allocateMemory (const int32_t m_,const int32_t n_) {
00824   m = abs(m_); n = abs(n_);
00825   if (m==0 || n==0) {
00826     val = 0;
00827     return;
00828   }
00829   val    = (FLOAT**)malloc(m*sizeof(FLOAT*));
00830   val[0] = (FLOAT*)calloc(m*n,sizeof(FLOAT));
00831   for(int32_t i=1; i<m; i++)
00832     val[i] = val[i-1]+n;
00833 }
00834 
00835 void Matrix::releaseMemory () {
00836   if (val!=0) {
00837     free(val[0]);
00838     free(val);
00839   }
00840 }
00841 
00842 FLOAT Matrix::pythag(FLOAT a,FLOAT b) {
00843   FLOAT absa,absb;
00844   absa = fabs(a);
00845   absb = fabs(b);
00846   if (absa > absb)
00847     return absa*sqrt(1.0+SQR(absb/absa));
00848   else
00849     return (absb == 0.0 ? 0.0 : absb*sqrt(1.0+SQR(absa/absb)));
00850 }
00851 


libviso2
Author(s): Andreas Geiger, maintained by: Stephan Wirth/stephan.wirth@uib.es
autogenerated on Tue Jan 7 2014 11:42:14