svd_HH.cpp
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 
00023 //Based on the svd of the KDL-0.2 library by Erwin Aertbelien
00024 
00025 #include "svd_HH.hpp"
00026 
00027 namespace KDL
00028 {
00029     inline double PYTHAG(double a,double b) {
00030         double at,bt,ct;
00031         at = fabs(a);
00032         bt = fabs(b);
00033         if (at > bt ) {
00034             ct=bt/at;
00035             return at*sqrt(1.0+ct*ct);
00036         } else {
00037             if (bt==0)
00038                 return 0.0;
00039             else {
00040                 ct=at/bt;
00041                 return bt*sqrt(1.0+ct*ct);
00042             }
00043         }
00044     }
00045 
00046 
00047     inline double SIGN(double a,double b) {
00048         return ((b) >= 0.0 ? fabs(a) : -fabs(a));
00049     }
00050 
00051     SVD_HH::SVD_HH(const Jacobian& jac):
00052         tmp(jac.columns())
00053     {
00054     }
00055 
00056     SVD_HH::~SVD_HH()
00057     {
00058     }
00059 
00060     int SVD_HH::calculate(const Jacobian& jac,std::vector<JntArray>& U,JntArray& w,std::vector<JntArray>& v,int maxiter)
00061     {
00062 
00063         //get the rows/columns of the jacobian
00064         const int rows = jac.rows();
00065         const int cols = jac.columns();
00066 
00067         int i(-1),its(-1),j(-1),jj(-1),k(-1),nm=0;
00068         int ppi(0);
00069         bool flag,maxarg1,maxarg2;
00070         double anorm(0),c(0),f(0),h(0),s(0),scale(0),x(0),y(0),z(0),g(0);
00071 
00072         for(i=0;i<rows;i++)
00073             for(j=0;j<cols;j++)
00074                 U[i](j)=jac(i,j);
00075         if(rows>cols)
00076             for(i=rows;i<cols;i++)
00077                 for(j=0;j<cols;j++)
00078                     U[i](j)=0;
00079 
00080         /* Householder reduction to bidiagonal form. */
00081         for (i=0;i<cols;i++) {
00082             ppi=i+1;
00083             tmp(i)=scale*g;
00084             g=s=scale=0.0;
00085             if (i<rows) {
00086                 for (k=i;k<rows;k++) scale += fabs(U[k](i));
00087                 if (scale) {
00088                     // multiply the i-th column by 1.0/scale, start from the i-th element
00089                     // sum of squares of column i, start from the i-th element
00090                     for (k=i;k<rows;k++) {
00091                         U[k](i) /= scale;
00092                         s += U[k](i)*U[k](i);
00093                     }
00094                     f=U[i](i);  // f is the diag elem
00095                     g = -SIGN(sqrt(s),f);
00096                     h=f*g-s;
00097                     U[i](i)=f-g;
00098                     for (j=ppi;j<cols;j++) {
00099                         // dot product of columns i and j, starting from the i-th row
00100                         for (s=0.0,k=i;k<rows;k++) s += U[k](i)*U[k](j);
00101                         f=s/h;
00102                         // copy the scaled i-th column into the j-th column
00103                         for (k=i;k<rows;k++) U[k](j) += f*U[k](i);
00104                     }
00105                     for (k=i;k<rows;k++) U[k](i) *= scale;
00106                 }
00107             }
00108             // save singular value
00109             w(i)=scale*g;
00110             g=s=scale=0.0;
00111             if ((i <rows) && (i+1 != cols)) {
00112                 // sum of row i, start from columns i+1
00113                 for (k=ppi;k<cols;k++) scale += fabs(U[i](k));
00114                 if (scale) {
00115                     for (k=ppi;k<cols;k++) {
00116                         U[i](k) /= scale;
00117                         s += U[i](k)*U[i](k);
00118                     }
00119                     f=U[i](ppi);
00120                     g = -SIGN(sqrt(s),f);
00121                     h=f*g-s;
00122                     U[i](ppi)=f-g;
00123                     for (k=ppi;k<cols;k++) tmp(k)=U[i](k)/h;
00124                     for (j=ppi;j<rows;j++) {
00125                         for (s=0.0,k=ppi;k<cols;k++) s += U[j](k)*U[i](k);
00126                         for (k=ppi;k<cols;k++) U[j](k) += s*tmp(k);
00127                     }
00128                     for (k=ppi;k<cols;k++) U[i](k) *= scale;
00129                 }
00130             }
00131             maxarg1=anorm;
00132             maxarg2=(fabs(w(i))+fabs(tmp(i)));
00133             anorm = maxarg1 > maxarg2 ? maxarg1 : maxarg2;
00134         }
00135         /* Accumulation of right-hand transformations. */
00136         for (i=cols-1;i>=0;i--) {
00137             if (i<cols-1) {
00138                 if (g) {
00139                     for (j=ppi;j<cols;j++) v[j](i)=(U[i](j)/U[i](ppi))/g;
00140                     for (j=ppi;j<cols;j++) {
00141                         for (s=0.0,k=ppi;k<cols;k++) s += U[i](k)*v[k](j);
00142                         for (k=ppi;k<cols;k++) v[k](j) += s*v[k](i);
00143                     }
00144                 }
00145                 for (j=ppi;j<cols;j++) v[i](j)=v[j](i)=0.0;
00146             }
00147             v[i](i)=1.0;
00148             g=tmp(i);
00149             ppi=i;
00150         }
00151         /* Accumulation of left-hand transformations. */
00152         for (i=cols-1<rows-1 ? cols-1:rows-1;i>=0;i--) {
00153             ppi=i+1;
00154             g=w(i);
00155             for (j=ppi;j<cols;j++) U[i](j)=0.0;
00156             if (g) {
00157                 g=1.0/g;
00158                 for (j=ppi;j<cols;j++) {
00159                     for (s=0.0,k=ppi;k<rows;k++) s += U[k](i)*U[k](j);
00160                     f=(s/U[i](i))*g;
00161                     for (k=i;k<rows;k++) U[k](j) += f*U[k](i);
00162                 }
00163                 for (j=i;j<rows;j++) U[j](i) *= g;
00164             } else {
00165                 for (j=i;j<rows;j++) U[j](i)=0.0;
00166             }
00167             ++U[i](i);
00168         }
00169 
00170         /* Diagonalization of the bidiagonal form. */
00171         for (k=cols-1;k>=0;k--) { /* Loop over singular values. */
00172             for (its=1;its<=maxiter;its++) {  /* Loop over allowed iterations. */
00173                 flag=true;
00174                 for (ppi=k;ppi>=0;ppi--) {  /* Test for splitting. */
00175                     nm=ppi-1;             /* Note that tmp[1] is always zero. */
00176                     if ((fabs(tmp(ppi))+anorm) == anorm) {
00177                         flag=false;
00178                         break;
00179                     }
00180                     if ((fabs(w(nm)+anorm) == anorm)) break;
00181                 }
00182                 if (flag) {
00183                     c=0.0;           /* Cancellation of tmp[l], if l>1: */
00184                     s=1.0;
00185                     for (i=ppi;i<=k;i++) {
00186                         f=s*tmp(i);
00187                         tmp(i)=c*tmp(i);
00188                         if ((fabs(f)+anorm) == anorm) break;
00189                         g=w(i);
00190                         h=PYTHAG(f,g);
00191                         w(i)=h;
00192                         h=1.0/h;
00193                         c=g*h;
00194                         s=(-f*h);
00195                         for (j=0;j<rows;j++) {
00196                             y=U[j](nm);
00197                             z=U[j](i);
00198                             U[j](nm)=y*c+z*s;
00199                             U[j](i)=z*c-y*s;
00200                         }
00201                     }
00202                 }
00203                 z=w(k);
00204 
00205                 if (ppi == k) {       /* Convergence. */
00206                     if (z < 0.0) {   /* Singular value is made nonnegative. */
00207                         w(k) = -z;
00208                         for (j=0;j<cols;j++) v[j](k)=-v[j](k);
00209                     }
00210                     break;
00211                 }
00212                 x=w(ppi);            /* Shift from bottom 2-by-2 minor: */
00213                 nm=k-1;
00214                 y=w(nm);
00215                 g=tmp(nm);
00216                 h=tmp(k);
00217                 f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
00218 
00219                 g=PYTHAG(f,1.0);
00220                 f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
00221 
00222                 /* Next QR transformation: */
00223                 c=s=1.0;
00224                 for (j=ppi;j<=nm;j++) {
00225                     i=j+1;
00226                     g=tmp(i);
00227                     y=w(i);
00228                     h=s*g;
00229                     g=c*g;
00230                     z=PYTHAG(f,h);
00231                     tmp(j)=z;
00232                     c=f/z;
00233                     s=h/z;
00234                     f=x*c+g*s;
00235                     g=g*c-x*s;
00236                     h=y*s;
00237                     y=y*c;
00238                     for (jj=0;jj<cols;jj++) {
00239                         x=v[jj](j);
00240                         z=v[jj](i);
00241                         v[jj](j)=x*c+z*s;
00242                         v[jj](i)=z*c-x*s;
00243                     }
00244                     z=PYTHAG(f,h);
00245                     w(j)=z;
00246                     if (z) {
00247                         z=1.0/z;
00248                         c=f*z;
00249                         s=h*z;
00250                     }
00251                     f=(c*g)+(s*y);
00252                     x=(c*y)-(s*g);
00253                     for (jj=0;jj<rows;jj++) {
00254                         y=U[jj](j);
00255                         z=U[jj](i);
00256                         U[jj](j)=y*c+z*s;
00257                         U[jj](i)=z*c-y*s;
00258                     }
00259                 }
00260                 tmp(ppi)=0.0;
00261                 tmp(k)=f;
00262                 w(k)=x;
00263             }
00264         }
00265         if (its == maxiter)
00266             return (-2);
00267         else
00268             return (0);
00269     }
00270 
00271 }
00272 
00273 
00274 
00275 


orocos_kdl
Author(s): Ruben Smits, Erwin Aertbelien, Orocos Developers
autogenerated on Sat Dec 28 2013 17:17:25