orocos_kdl: svd_eigen_HH.cpp Source File

Go to the documentation of this file.
00001 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 
00003 // Version: 1.0
00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // URL: http://www.orocos.org/kdl
00007 
00008 // This library is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 2.1 of the License, or (at your option) any later version.
00012 
00013 // This library is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 // Lesser General Public License for more details.
00017 
00018 // You should have received a copy of the GNU Lesser General Public
00019 // License along with this library; if not, write to the Free Software
00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
00021 
00022 #include "svd_eigen_HH.hpp"
00023 
00024 namespace KDL{
00025     
00026     int svd_eigen_HH(const MatrixXd& A,MatrixXd& U,VectorXd& S,MatrixXd& V,VectorXd& tmp,int maxiter,double epsilon)
00027     {
00028         //get the rows/columns of the matrix
00029         const int rows = A.rows();
00030         const int cols = A.cols();
00031     
00032         U.setZero();
00033         U.topLeftCorner(rows,cols)=A;
00034 
00035         int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
00036         int ppi(0);
00037         bool flag,maxarg1,maxarg2;
00038         double anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);
00039 
00040         /* Householder reduction to bidiagonal form. */
00041         for (i=0;i<cols;i++) {
00042             ppi=i+1;
00043             tmp(i)=scale*g;
00044             g=s=scale=0.0; 
00045             if (i<rows) {
00046                 // compute the sum of the i-th column, starting from the i-th row
00047                 for (k=i;k<rows;k++) scale += fabs(U(k,i));
00048                 if (fabs(scale)>epsilon) {
00049                     // multiply the i-th column by 1.0/scale, start from the i-th element
00050                     // sum of squares of column i, start from the i-th element
00051                     for (k=i;k<rows;k++) {
00052                         U(k,i) /= scale;
00053                         s += U(k,i)*U(k,i);
00054                     }
00055                     f=U(i,i);  // f is the diag elem
00056                                         if (!(s>=0)) return -3;
00057                     g = -SIGN(sqrt(s),f);
00058                     h=f*g-s;
00059                     U(i,i)=f-g;
00060                     for (j=ppi;j<cols;j++) {
00061                         // dot product of columns i and j, starting from the i-th row
00062                         for (s=0.0,k=i;k<rows;k++) s += U(k,i)*U(k,j);
00063                                                 if (!(h!=0)) return -4;
00064                         f=s/h;
00065                         // copy the scaled i-th column into the j-th column
00066                         for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
00067                     }
00068                     for (k=i;k<rows;k++) U(k,i) *= scale;
00069                 }
00070             }
00071             // save singular value
00072             S(i)=scale*g;
00073             g=s=scale=0.0;
00074             if ((i <rows) && (i+1 != cols)) {
00075                 // sum of row i, start from columns i+1
00076                 for (k=ppi;k<cols;k++) scale += fabs(U(i,k));
00077                 if (fabs(scale)>epsilon) {
00078                     for (k=ppi;k<cols;k++) {
00079                         U(i,k) /= scale;
00080                         s += U(i,k)*U(i,k);
00081                     }
00082                     f=U(i,ppi);
00083                                         if (!(s>=0)) return -5;
00084                     g = -SIGN(sqrt(s),f);
00085                     h=f*g-s;
00086                     U(i,ppi)=f-g;
00087                                         if (!(h!=0)) return -6;
00088                     for (k=ppi;k<cols;k++) tmp(k)=U(i,k)/h;
00089                     for (j=ppi;j<rows;j++) {
00090                         for (s=0.0,k=ppi;k<cols;k++) s += U(j,k)*U(i,k);
00091                         for (k=ppi;k<cols;k++) U(j,k) += s*tmp(k);
00092                     }
00093                     for (k=ppi;k<cols;k++) U(i,k) *= scale;
00094                 }
00095             }
00096             maxarg1=anorm;
00097             maxarg2=(fabs(S(i))+fabs(tmp(i)));
00098             anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;              
00099         }
00100         /* Accumulation of right-hand transformations. */
00101         for (i=cols-1;i>=0;i--) {
00102             if (i<cols-1) {
00103                 if (fabs(g)>epsilon) {
00104                                         if (!(U(i,ppi)!=0)) return -7;
00105                     for (j=ppi;j<cols;j++) V(j,i)=(U(i,j)/U(i,ppi))/g;
00106                     for (j=ppi;j<cols;j++) {
00107                         for (s=0.0,k=ppi;k<cols;k++) s += U(i,k)*V(k,j);
00108                         for (k=ppi;k<cols;k++) V(k,j) += s*V(k,i);
00109                     }
00110                 }
00111                 for (j=ppi;j<cols;j++) V(i,j)=V(j,i)=0.0;
00112             }
00113             V(i,i)=1.0;
00114             g=tmp(i);
00115             ppi=i;
00116         }
00117         /* Accumulation of left-hand transformations. */
00118         for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
00119             ppi=i+1;
00120             g=S(i);
00121             for (j=ppi;j<cols;j++) U(i,j)=0.0;
00122             if (fabs(g)>epsilon) {
00123                 g=1.0/g;
00124                 for (j=ppi;j<cols;j++) {
00125                     for (s=0.0,k=ppi;k<rows;k++) s += U(k,i)*U(k,j);
00126                                         if (!(U(i,i)!=0)) return -8;
00127                     f=(s/U(i,i))*g;
00128                     for (k=i;k<rows;k++) U(k,j) += f*U(k,i);
00129                 }
00130                 for (j=i;j<rows;j++) U(j,i) *= g;
00131             } else {
00132                 for (j=i;j<rows;j++) U(j,i)=0.0;
00133             }
00134             ++U(i,i);
00135         }
00136         
00137         /* Diagonalization of the bidiagonal form. */
00138         for (k=cols-1;k>=0;k--) { /* Loop over singular values. */
00139             for (its=1;its<=maxiter;its++) {  /* Loop over allowed iterations. */
00140                 flag=true;
00141                 for (ppi=k;ppi>=0;ppi--) {  /* Test for splitting. */
00142                     nm=ppi-1;             /* Note that tmp[1] is always zero. */
00143                     if ((fabs(tmp(ppi))+anorm) == anorm) {
00144                         flag=false;
00145                         break;
00146                     }
00147                     if ((fabs(S(nm)+anorm) == anorm)) break;
00148                 }
00149                 if (flag) {
00150                     c=0.0;           /* Cancellation of tmp[l], if l>1: */
00151                     s=1.0;
00152                     for (i=ppi;i<=k;i++) {
00153                         f=s*tmp(i);
00154                         tmp(i)=c*tmp(i);
00155                         if ((fabs(f)+anorm) == anorm) break;
00156                         g=S(i);
00157                         h=PYTHAG(f,g);
00158                         S(i)=h;
00159                                                 if (!(h!=0)) return -9;
00160                         h=1.0/h;
00161                         c=g*h;
00162                         s=(-f*h);
00163                         for (j=0;j<rows;j++) {
00164                             y=U(j,nm);
00165                             z=U(j,i);
00166                             U(j,nm)=y*c+z*s;
00167                             U(j,i)=z*c-y*s;
00168                         }
00169                     }
00170                 }
00171                 z=S(k);
00172                 
00173                 if (ppi == k) {       /* Convergence. */
00174                     if (z < 0.0) {   /* Singular value is made nonnegative. */
00175                         S(k) = -z;
00176                         for (j=0;j<cols;j++) V(j,k)=-V(j,k);
00177                     }
00178                     break;
00179                 }
00180                 
00181                 x=S(ppi);            /* Shift from bottom 2-by-2 minor: */
00182                 nm=k-1;
00183                 y=S(nm);
00184                 g=tmp(nm);
00185                 h=tmp(k);
00186                                 if (!(h!=0&&y!=0)) return -10;
00187                 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
00188                 
00189                 g=PYTHAG(f,1.0);
00190                                 if (!(x!=0)) return -11;
00191                                 if (!((f+SIGN(g,f))!=0)) return -12;
00192                 f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
00193                 
00194                 /* Next QR transformation: */
00195                 c=s=1.0;
00196                 for (j=ppi;j<=nm;j++) {
00197                     i=j+1;
00198                     g=tmp(i);
00199                     y=S(i);
00200                     h=s*g;
00201                     g=c*g;
00202                     z=PYTHAG(f,h);
00203                     tmp(j)=z;
00204                                         if (!(z!=0)) return -13;
00205                     c=f/z;
00206                     s=h/z;
00207                     f=x*c+g*s;
00208                     g=g*c-x*s;
00209                     h=y*s;
00210                     y=y*c;
00211                     for (jj=0;jj<cols;jj++) {
00212                         x=V(jj,j);
00213                         z=V(jj,i);
00214                         V(jj,j)=x*c+z*s;
00215                         V(jj,i)=z*c-x*s;
00216                     }
00217                     z=PYTHAG(f,h);
00218                     S(j)=z;
00219                     if (fabs(z)>epsilon) {
00220                         z=1.0/z;
00221                         c=f*z;
00222                         s=h*z;
00223                     }
00224                     f=(c*g)+(s*y);
00225                     x=(c*y)-(s*g);
00226                     for (jj=0;jj<rows;jj++) {
00227                         y=U(jj,j);
00228                         z=U(jj,i);
00229                         U(jj,j)=y*c+z*s;
00230                         U(jj,i)=z*c-y*s;
00231                     }
00232                 }
00233                 tmp(ppi)=0.0;
00234                 tmp(k)=f;
00235                 S(k)=x;
00236             }
00237         }
00238 
00239         //Sort eigen values:
00240         for (i=0; i<cols; i++){
00241             
00242             double S_max = S(i);
00243             int i_max = i;
00244             for (j=i+1; j<cols; j++){
00245                 double Sj = S(j);
00246                 if (Sj > S_max){
00247                     S_max = Sj;
00248                     i_max = j;
00249                 }
00250             }
00251             if (i_max != i){
00252                 /* swap eigenvalues */
00253                 double tmp = S(i);
00254                 S(i)=S(i_max);
00255                 S(i_max)=tmp;
00256                 
00257                 /* swap eigenvectors */
00258                 U.col(i).swap(U.col(i_max));
00259                 V.col(i).swap(V.col(i_max));
00260             }
00261         }
00262         
00263         if (its == maxiter) 
00264             return (-2);
00265         else 
00266             return (0);
00267     }
00268 }