matrix_wrapper.cpp
Go to the documentation of this file.
00001 #include "matrix_wrapper.h"
00002 #include <math.h>
00003 #include <vector>
00004 
00005 using namespace std;
00006 
00007 namespace MatrixWrapper
00008 {
00009 
00010     bool
00011     SymmetricMatrix_Wrapper::cholesky_semidefinite(MyMatrix& a) const
00012     {
00013         // apply cholesky to *this
00014         // put result in a
00015         // result is lower triangular matrix
00016         a = (*(MySymmetricMatrix*)this);
00017         int sz = a.rows();
00018           for (int k=1; k<sz+1; ++k) {
00019            // check if close to zero => put to zero
00020            if (a(k,k)< std::numeric_limits<double>::epsilon() && a(k,k)> -std::numeric_limits<double>::epsilon()){
00021                    a(k,k) = 0.0; //set to zero, matrix is semidefinite
00022            }
00023            if (a(k,k) < 0.0) {
00024                  std::cout<< "Warning: matrix of which cholesky decomposition is asked, is negative definite!: returning zero matrix" << std::endl;
00025                  std::cout<< "a(" << k << "," << k << ")=" << a(k,k)  << std::endl;
00026                  a = 0.0; return false;//matrix is negative definite
00027            }
00028            else  a(k,k)=sqrt(a(k,k));
00029            for (int i=k+1; i<sz+1; ++i) {
00030                if (a(k,k)< std::numeric_limits<double>::epsilon()){
00031                    a(i,k) = 0.0; //set to zero, matrix is semidefinite
00032                }
00033                else a(i,k)/=a(k,k);
00034            }
00035            for (int j=k+1; j<sz+1; ++j) {
00036              for (int i=j; i<sz+1; ++i)  a(i,j)-=a(i,k)*a(j,k);
00037            }
00038           }
00039           //delete upper  triangle
00040           for (int i=1; i<sz+1; i++) {
00041             for (int j=i+1; j<sz+1; j++) a(i,j)=0.0;
00042           }
00043         return true;
00044     }
00045 
00046     bool
00047     Matrix_Wrapper::SVD(MyColumnVector& w, MyMatrix& U, MyMatrix& V) const
00048     {
00049         //get the rows of the matrix
00050         const int rows=this->rows();
00051         //get the columns of the matrix
00052         const int cols=this->columns();
00053 
00054         U = *((MyMatrix*)(this));
00055 
00056         static const int maxIter=150;
00057 
00058         w.resize(cols);
00059         V.resize(cols,cols,false,true);
00060         int i(-1),its(-1),j(-1),jj(-1),k(-1),nm(0);
00061         int ppi(0);
00062         bool flag;
00063                 double maxarg1, maxarg2;
00064         double  anorm(0),c(0),f(0),g(0),h(0),s(0),scale(0),x(0),y(0),z(0);
00065 
00066         // Householder reduction to bidiagonal form
00067         std::vector<double> rv1(cols,0.0);
00068 
00069         g=scale=anorm=0.0;
00070 
00071         for (i=1; i <= cols; i++){
00072           ppi=i+1;
00073           rv1.at(i-1)=scale*g;
00074           g=s=scale=0.0;
00075           if (i <= rows ) {
00076             // compute the sum of the i-th column, starting from the i-th row
00077             for(k=i;k<=rows;k++) scale += fabs(U(k,i));
00078             if (scale) {
00079               // multiply the i-th column by 1.0/scale, start from the i-th element
00080               // sum of squares of column i, start from the i-th element
00081               for (k=i;k<=rows;k++){
00082                 U(k,i)/= scale;
00083                 s += U(k,i) * U(k,i);
00084               }
00085               f=U(i,i);  // f is the diag elem
00086               g=-SIGN(sqrt(s),f);
00087               h=f*g-s;
00088               U(i,i)=f-g;
00089               for (j=ppi; j <= cols; j++) {
00090                 // dot product of columns i and j, starting from the i-th row
00091                 for (s=0.0,k=i;k<=rows;k++) s += U(k,i) * U(k,j);
00092                 f=s/h;
00093                 // copy the scaled i-th column into the j-th column
00094                 for (k=i;k<=rows;k++) U(k,j) += f*U(k,i);
00095               }
00096                 for (k=i;k<=rows;k++) U(k,i) *= scale;
00097             }
00098           }
00099           // save singular value
00100           w(i) = scale*g;
00101           g=s=scale=0.0;
00102           if ((i <= rows) && (i != cols)) {
00103             // sum of row i, start from columns i+1
00104             for(k=ppi;k<=cols;k++) scale += fabs(U(i,k));
00105             if (scale) {
00106               for(k=ppi;k<=cols;k++){
00107                 U(i,k) /= scale;
00108                 s += U(i,k)*U(i,k);
00109               }
00110               f=U(i,ppi);
00111               g=-SIGN(sqrt(s),f); //<---- do something
00112               h=f*g-s;
00113               U(i,ppi)=f-g;
00114               for ( k=ppi; k <= cols; k++)   rv1.at(k-1)=U(i,k)/h;
00115               for ( j=ppi; j <= rows; j++) {
00116                 for( s=0.0,k=ppi;k<=cols;k++) s += U(j,k) * U(i,k);
00117                 for ( k=ppi; k <= cols; k++)  U(j,k) += s*rv1.at(k-1);
00118               }
00119                 for( k=ppi;k<=cols;k++) U(i,k) *= scale;
00120             }
00121           }
00122           maxarg1=anorm;
00123           maxarg2=(fabs(w(i))+fabs(rv1.at(i-1)));
00124           anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
00125 
00126         }
00127 
00128         // Accumulation of right-hand transformation
00129         for (i= cols ; i>=1; i--) {
00130           if ( i < cols ) {
00131             if (g) {
00132               for ( j=ppi; j <= cols; j++)  V(j,i)=(U(i,j)/U(i,ppi))/g;
00133               for ( j=ppi; j <= cols; j++) {
00134                 for (s=0.0,k=ppi;k <= cols;k++)  s += U(i,k)*V(k,j);
00135                 for ( k=ppi; k <= cols;k++) V(k,j) += s*V(k,i);
00136               }
00137             }
00138             for( j=ppi; j<=cols ; j++) V(i,j)=V(j,i)=0.0;
00139           }
00140           V(i,i)=1.0;
00141           g=rv1.at(i-1);
00142           ppi=i;
00143         }
00144 
00145         // Accumulation of left-hand transformation
00146         for (i= cols < rows ? cols: rows; i>=1; i--) {
00147           ppi=i+1;
00148           g=w(i);
00149           for( j=ppi; j<=cols ; j++) U(i,j)=0.0;
00150           if (g) {
00151             g=1.0/g;
00152             for ( j=ppi; j <= cols; j++) {
00153               for( s=0.0, k=ppi; k<=rows ; k++) s += U(k,i)*U(k,j);
00154               f=(s/U(i,i))*g;
00155               for ( k=i; k <= rows;k++)  U(k,j) += f*U(k,i);
00156             }
00157             for( j=i; j<=rows ; j++) U(j,i) *=g;
00158           } else {
00159             for( j=i; j<=rows ; j++) U(j,i) = 0.0;
00160           }
00161           ++U(i,i);
00162         }
00163 
00164         // Diagonalization of the bidiagonal form:
00165         // Loop over singular values,and over allowed iterations
00166         for ( k=cols; k >= 1; k--) {
00167           for (its=1; its <= maxIter; its++) {
00168             flag=true;
00169             //Test for splitting. Note that rv1[i] is always 0
00170             for (ppi=k; ppi >= 1; ppi--) {
00171               nm=ppi-1;
00172               if ((fabs(rv1.at(ppi-1))+anorm) == anorm) {
00173                 flag=false;
00174                 break;
00175               }
00176               if ((fabs(w(nm)+anorm) == anorm)) {
00177                 break;
00178               }
00179             }
00180             //Cancellation of rv1[ppi],if ppi>1.
00181             if (flag) {
00182               c=0.0;
00183               s=1.0;
00184               for ( i=ppi; i <= k ;i++) {
00185                 f=s*rv1.at(i-1);
00186                 rv1.at(i-1)=c*rv1.at(i-1);
00187                 if ((fabs(f)+anorm) == anorm) {
00188                   break;
00189                 }
00190                 g=w(i);
00191                 h=PYTHAG(f,g);
00192                 w(i)=h;
00193                 h=1.0/h;
00194                 c=g*h;
00195                 s=-f*h;
00196                 for ( j=1;j <= rows; j++) {
00197                   y=U(j,nm);
00198                   z=U(j,i);
00199                   U(j,nm)=y*c+z*s;
00200                   U(j,i)=z*c-y*s;
00201                 }
00202               }
00203             }
00204             z=w(k);
00205 
00206             // Convergence. Singular value is made nonnegative.
00207             if (ppi == k) {
00208               if (z < 0.0) {
00209                 w(k)=-z;
00210                 for (j=1; j <= cols; j++) V(j,k)=-V(j,k);
00211               }
00212               break;
00213             }
00214 
00215             if (its == maxIter) {
00216               //char x[80];
00217               std::cout << "SVD did not converge after " <<  maxIter << " iterations!" << std::endl;
00218               // make all singular values zero -> rank 0
00219               w = 0.0;
00220               return false;
00221             }
00222             // shift from bottom 2-by-2 minor
00223             x=w(ppi);
00224             nm=k-1;
00225             y=w(nm);
00226             g=rv1.at(nm-1);
00227             h=rv1.at(k-1);
00228             f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
00229 
00230             g = PYTHAG(f,1.0);
00231             f = ((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
00232 
00233             //Next QR transformation
00234             c=s=1.0;
00235             for ( j=ppi; j<=nm ;j++){
00236               i=j+1;
00237               g=rv1.at(i-1);
00238               y=w(i);
00239               h=s*g;
00240               g=c*g;
00241               z=PYTHAG(f,h);
00242               rv1.at(j-1)=z;
00243               c=f/z;
00244               s=h/z;
00245               f=x*c+g*s;
00246               g=g*c-x*s;
00247               h=y*s;
00248               y*=c;
00249               for (jj=1; jj<=cols ;jj++){
00250                 x=V(jj,j);
00251                 z=V(jj,i);
00252                 V(jj,j)=x*c+z*s;
00253                 V(jj,i)=z*c-x*s;
00254               }
00255               z=PYTHAG(f,h);
00256               w(j)=z;
00257 
00258               if (z) {
00259                 z=1.0/z;
00260                 c=f*z;
00261                 s=h*z;
00262               }
00263               f=c*g+s*y;
00264               x=c*y-s*g;
00265               for (jj=1; jj<=rows; jj++){
00266                 y=U(jj,j);
00267                 z=U(jj,i);
00268                 U(jj,j)=y*c+z*s;
00269                 U(jj,i)=z*c-y*s;
00270               }
00271             }
00272             rv1.at(ppi-1)=0.0;
00273             rv1.at(k-1)=f;
00274             w(k)=x;
00275 
00276           }
00277         }
00278         return true;
00279     }
00280 
00281 
00282   double
00283   Matrix_Wrapper::PYTHAG(double a,double b) const
00284  {
00285      double at,bt,ct;
00286      at = fabs(a);
00287      bt = fabs(b);
00288      if (at > bt ) {
00289          ct=bt/at;
00290          return at*sqrt(1.0+ct*ct);
00291      } else {
00292          if (bt==0)
00293              return 0.0;
00294          else {
00295              ct=at/bt;
00296              return bt*sqrt(1.0+ct*ct);
00297          }
00298      }
00299  }
00300 
00301 
00302  double
00303  Matrix_Wrapper::SIGN(double a,double b) const
00304  {
00305      return ((b) >= 0.0 ? fabs(a) : -fabs(a));
00306  }
00307 
00308  // See <http://dsp.ee.sun.ac.za/~schwardt/dsp813/lecture10/node7.html>
00309  MyMatrix
00310  Matrix_Wrapper::pseudoinverse(double epsilon) const
00311  {
00312    int rows;
00313    rows = this->rows();
00314    int cols = this->columns();
00315    // calculate SVD decomposition
00316    MyMatrix U,V;
00317    MyColumnVector D;
00318 
00319    bool res;
00320    res = SVD(D,U,V);  // U=mxn  D=n  V=nxn
00321    assert(res);
00322 
00323    Matrix Dinv(cols,cols);
00324    Dinv = 0;
00325    for (unsigned int i=0; i<D.rows(); i++)
00326      if ( D(i+1) < epsilon )
00327        Dinv(i+1,i+1) = 0;
00328      else
00329        Dinv(i+1,i+1) = 1/D(i+1);
00330 
00331    #ifdef __DEBUG__
00332      std::cout << "MATRIX::pseudoinverse() Dinv =\n" << Dinv << std::endl;
00333    #endif //__DEBUG__
00334 
00335    return V * Dinv * U.transpose();
00336  }
00337 
00338 } //namespace


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Mon Feb 11 2019 03:45:12