aruco_pose: rpp_svd.cpp Source File

Go to the documentation of this file.
00001 /* ========================================================================
00002  * PROJECT: ARToolKitPlus
00003  * ========================================================================
00004  *
00005  * Copyright of the derived and new portions of this work
00006  *     (C) 2006 Graz University of Technology
00007  *
00008  * This framework is free software; you can redistribute it and/or modify
00009  * it under the terms of the GNU General Public License as published by
00010  * the Free Software Foundation; either version 2 of the License, or
00011  * (at your option) any later version.
00012  *
00013  * This framework 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
00016  * GNU General Public License for more details.
00017  *
00018  * You should have received a copy of the GNU General Public License
00019  * along with this framework; if not, write to the Free Software
00020  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00021  *
00022  * For further information please contact 
00023  *   Dieter Schmalstieg
00024  *   <schmalstieg@icg.tu-graz.ac.at>
00025  *   Graz University of Technology, 
00026  *   Institut for Computer Graphics and Vision,
00027  *   Inffeldgasse 16a, 8010 Graz, Austria.
00028  * ========================================================================
00029  ** @author   Thomas Pintaric
00030  *
00031  * $Id$
00032  * @file
00033  * ======================================================================== */
00034 
00035 
00036 #ifndef _NO_LIBRPP_
00037 
00038 
00039 #include <math.h>
00040 #include <assert.h>
00041 
00042 #include "rpp/rpp_types.h"
00043 
00044 
00045 namespace rpp {
00046 
00047 typedef double SVD_FLOAT;
00048 
00049 
00050 static SVD_FLOAT at,bt,ct;
00051 static SVD_FLOAT maxarg1,maxarg2;
00052 
00053 #define PYTHAG(a,b) ((at=fabs(a)) > (bt=fabs(b)) ? \
00054     (ct=bt/at,at*sqrt(SVD_FLOAT(1.0f)+ct*ct)) : (bt ? (ct=at/bt,bt*sqrt(SVD_FLOAT(1.0f)+ct*ct)): SVD_FLOAT(0.0)))
00055 
00056 #define MAX(a,b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))
00057 
00058 #define SIGN(a,b) ((b) >= SVD_FLOAT(0.0f) ? fabs(a) : -fabs(a))
00059 
00060 
00061 int svdcmp( double **a, int m,int n, double *w,double **v)
00062 {
00063     int flag,i,its,j,jj,k,ii=0,nm=0;
00064     SVD_FLOAT c,f,h,s,x,y,z;
00065     SVD_FLOAT anorm=0.0,g=0.0,scale=0.0;
00066 
00067     if (m < n) return -1;       // must augment A with extra zero rows
00068 
00069         assert(n==3);
00070         SVD_FLOAT rv1[3];               // was: rv1=G_alloc_vector(n);
00071 
00072     n--;
00073     m--;
00074 
00075     for (i=0;i<=n;i++) {
00076         ii=i+1;
00077         rv1[i]=scale*g;
00078         g=s=scale=0.0;
00079         if (i <= m) {
00080             for (k=i;k<=m;k++) scale += (SVD_FLOAT)fabs(a[k][i]);
00081             if (scale) {
00082                 for (k=i;k<=m;k++) {
00083                     a[k][i] /= (SVD_FLOAT)(scale);
00084                     s += a[k][i]*a[k][i];
00085                 }
00086                 f=a[i][i];
00087                 g = -SIGN(sqrt(s),f);
00088                 h=f*g-s;
00089                 a[i][i]=(SVD_FLOAT)(f-g);
00090                 if (i != n) {
00091                     for (j=ii;j<=n;j++) {
00092                         for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
00093                         f=s/h;
00094                         for (k=i;k<=m;k++) a[k][j] += (SVD_FLOAT)(f)*a[k][i];
00095                     }
00096                 }
00097                 for (k=i;k<=m;k++) a[k][i] *= (SVD_FLOAT)(scale);
00098             }
00099         }
00100         w[i]=scale*g;
00101         g=s=scale=0.0;
00102         if (i <= m && i != n) {
00103             for (k=ii;k<=n;k++) scale += fabs(a[i][k]);
00104             if (scale) {
00105                 for (k=ii;k<=n;k++) {
00106                     a[i][k] /= scale;
00107                     s += a[i][k]*a[i][k];
00108                 }
00109                 f=a[i][ii];
00110                 g = -SIGN(sqrt(s),f);
00111                 h=f*g-s;
00112                 a[i][ii]=f-g;
00113                 for (k=ii;k<=n;k++) rv1[k]=a[i][k]/h;
00114                 if (i != m) {
00115                     for (j=ii;j<=m;j++) {
00116                         for (s=0.0,k=ii;k<=n;k++) s += a[j][k]*a[i][k];
00117                         for (k=ii;k<=n;k++) a[j][k] += s*rv1[k];
00118                     }
00119                 }
00120                 for (k=ii;k<=n;k++) a[i][k] *= scale;
00121             }
00122         }
00123         anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
00124     }
00125     for (i=n;i>=0;i--) {
00126         if (i < n) {
00127             if (g) {
00128                 for (j=ii;j<=n;j++)
00129                     v[j][i]=(a[i][j]/a[i][ii])/g;
00130                 for (j=ii;j<=n;j++) {
00131                     for (s=0.0,k=ii;k<=n;k++) s += a[i][k]*v[k][j];
00132                     for (k=ii;k<=n;k++) v[k][j] += s*v[k][i];
00133                 }
00134             }
00135             for (j=ii;j<=n;j++) v[i][j]=v[j][i]=0.0;
00136         }
00137         v[i][i]=1.0;
00138         g=rv1[i];
00139         ii=i;
00140     }
00141     for (i=n;i>=0;i--) {
00142         ii=i+1;
00143         g=w[i];
00144         if (i < n)
00145             for (j=ii;j<=n;j++) a[i][j]=0.0;
00146         if (g) {
00147             g=SVD_FLOAT(1.0)/g;
00148             if (i != n) {
00149                 for (j=ii;j<=n;j++) {
00150                     for (s=0.0,k=ii;k<=m;k++) s += a[k][i]*a[k][j];
00151                     f=(s/a[i][i])*g;
00152                     for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
00153                 }
00154             }
00155             for (j=i;j<=m;j++) a[j][i] *= g;
00156         } else {
00157             for (j=i;j<=m;j++) a[j][i]=0.0;
00158         }
00159         ++a[i][i];
00160     }
00161     for (k=n;k>=0;k--) {
00162         for (its=1;its<=30;its++) {
00163             flag=1;
00164             for (ii=k;ii>=0;ii--) {
00165                 nm=ii-1;
00166                 if (fabs(rv1[ii])+anorm == anorm) {
00167                     flag=0;
00168                     break;
00169                 }
00170                 if (fabs(w[nm])+anorm == anorm) break;
00171             }
00172             if (flag) {
00173                 c=0.0;
00174                 s=1.0;
00175                 for (i=ii;i<=k;i++) {
00176                     f=s*rv1[i];
00177                     if (fabs(f)+anorm != anorm) {
00178                         g=w[i];
00179                         h=PYTHAG(f,g);
00180                         w[i]=h;
00181                         h=SVD_FLOAT(1.0)/h;
00182                         c=g*h;
00183                         s=(-f*h);
00184                         for (j=0;j<=m;j++) {
00185                             y=a[j][nm];
00186                             z=a[j][i];
00187                             a[j][nm]=y*c+z*s;
00188                             a[j][i]=z*c-y*s;
00189                         }
00190                     }
00191                 }
00192             }
00193             z=w[k];
00194             if (ii == k) {
00195                 if (z < 0.0) {
00196                     w[k] = -z;
00197                     for (j=0;j<=n;j++) v[j][k]=(-v[j][k]);
00198                 }
00199                 break;
00200             }
00201             if (its == 30) return -2; /*No convergence in 30 SVDCMP iterations*/
00202             x=w[ii];
00203             nm=k-1;
00204             y=w[nm];
00205             g=rv1[nm];
00206             h=rv1[k];
00207             f=((y-z)*(y+z)+(g-h)*(g+h))/(SVD_FLOAT(2.0)*h*y);
00208             g=PYTHAG(f,SVD_FLOAT(1.0));
00209             f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
00210             c=s=SVD_FLOAT(1.0);
00211             for (j=ii;j<=nm;j++) {
00212                 i=j+1;
00213                 g=rv1[i];
00214                 y=w[i];
00215                 h=s*g;
00216                 g=c*g;
00217                 z=PYTHAG(f,h);
00218                 rv1[j]=z;
00219                 c=f/z;
00220                 s=h/z;
00221                 f=x*c+g*s;
00222                 g=g*c-x*s;
00223                 h=y*s;
00224                 y=y*c;
00225                 for (jj=0;jj<=n;jj++) {
00226                     x=v[jj][j];
00227                     z=v[jj][i];
00228                     v[jj][j]=x*c+z*s;
00229                     v[jj][i]=z*c-x*s;
00230                 }
00231                 z=PYTHAG(f,h);
00232                 w[j]=z;
00233                 if (z) {
00234                     z=SVD_FLOAT(1.0)/z;
00235                     c=f*z;
00236                     s=h*z;
00237                 }
00238                 f=(c*g)+(s*y);
00239                 x=(c*y)-(s*g);
00240                 for (jj=0;jj<=m;jj++) {
00241                     y=a[jj][j];
00242                     z=a[jj][i];
00243                     a[jj][j]=y*c+z*s;
00244                     a[jj][i]=z*c-y*s;
00245                 }
00246             }
00247             rv1[ii]=SVD_FLOAT(0.0);
00248             rv1[k]=f;
00249             w[k]=x;
00250         }
00251     }
00252 
00253     //G_free_vector(rv1);               // no longer used, rv1 is now on stack
00254 
00255     return 0;
00256 }
00257 
00258 #undef SIGN
00259 #undef MAX
00260 #undef PYTHAG
00261 
00262 /*
00263 
00264 int G_svbksb(
00265     SVD_FLOAT **u,SVD_FLOAT w[],SVD_FLOAT **v,
00266     int m,int n,SVD_FLOAT b[],SVD_FLOAT x[])
00267 {
00268     int j,i;
00269     SVD_FLOAT s,*tmp; //,*G_alloc_vector();
00270 
00271     tmp=G_alloc_vector(n);
00272     for (j=0;j<n;j++) {
00273         s=0.0;
00274         if (w[j]) {
00275             for (i=0;i<m;i++) s += u[i][j]*b[i];
00276             s /= w[j];
00277         }
00278         tmp[j]=s;
00279     }
00280     for (j=0;j<n;j++) {
00281         s=0.0;
00282         for (i=0;i<n;i++) s += v[j][i]*tmp[i];
00283         x[j]=s;
00284     }
00285     G_free_vector(tmp);
00286 
00287     return 0;
00288 }
00289 
00290 #define TOL 1e-8
00291 
00292 int G_svelim( SVD_FLOAT *w, int n)
00293 {
00294     int i;
00295     SVD_FLOAT thresh;
00296 
00297     thresh=0.0; // remove singularity
00298     for(i=0;i<n;i++)
00299         if (w[i] > thresh)
00300             thresh=w[i];
00301     thresh *= TOL;
00302     for(i=0;i<n;i++)
00303         if (w[i] < thresh)
00304             w[i]=0.0;
00305 
00306     return 0;
00307 }
00308 
00309 #undef TOL
00310 
00311 */
00312 
00313 }  // namespace rpp
00314 
00315 
00316 #endif //_NO_LIBRPP_