rtcEigenSystem.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2009
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: Autonomous Technologies
00011  * file .......: rtcEigenSystem.h
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 08/16/2006
00015  * modified ...: $Date: 2009-01-21 18:19:16 -0800 (Wed, 21 Jan 2009) $
00016  * changed by .: $Author: benjaminpitzer $
00017  * revision ...: $Revision: 14 $
00018  */
00019 #ifndef RTC_EIGENSYSTEM_H
00020 #define RTC_EIGENSYSTEM_H
00021 
00022 // puma includes
00023 #include <rtc/rtcException.h>
00024 #include "rtc/rtcMath.h"
00025 #include "rtc/rtcSMat.h"
00026 #include "rtc/rtcVec.h"
00027 
00028 #define PUMA_JACOBI_ROTATE(a,i,j,k,l) g=a.x[i*M+j];h=a.x[k*M+l];\
00029                                         a.x[i*M+j]=g-s*(h+g*tau);\
00030                                         a.x[k*M+l]=h+s*(g-h*tau);
00031 #define PUMA_RADIX 2.0
00032 #define PUMA_SIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
00033 
00034 namespace rtc {
00035 
00037   // DECLARATIONS
00039 
00040   // Forward declarations
00041   template <class T, int M> class Vec;          // M-d vector
00042   template <class T, int M> class SMat;         // MxM Square Matrix
00043   template <class T, int M> class EigenSystem;  // Eigensystem
00044 
00046 
00059   template <class T, int M>
00060   class EigenSystem {
00061   public:
00062     // Constructors/Destructor
00063     EigenSystem(SMat<T,M>& a_);
00064     ~EigenSystem();
00065 
00066     // Eigen System routines for real symmetric matrices
00067 
00068     /*
00069      * Produces a vector of eigenvalues (D) and a matrix of eigenvectors (V) of
00070      * matrix A.
00071      */
00072     bool jacobi(Vec<T,M>& eigenvalues, SMat<T,M>& eigenvectors, int* nrot);
00073     bool jacobi(Vec<T,M>& eigenvalues, SMat<T,M>& eigenvectors);
00074     void eigenSort(Vec<T,M>& d, SMat<T,M>& v);
00075 
00076     // non-tested routines
00077     void householderReduce(Vec<T,M>& d, Vec<T,M>& e, bool vecs = true);
00078     void householderSolve(Vec<T,M>& d, Vec<T,M>& e, bool vecs = true);
00079     void balance();
00080     void hessenbergReduce();
00081     void qr(Vec<T,M>& wreal, Vec<T,M>& wimag);
00082 
00083   protected:
00084     void rotate(double& g,double& h,SMat<T,M>& a,
00085                 const int i,const int j,const int k,const int l,
00086                 const double s,const double tau) const;
00087     void rotateT(double& g,double& h,SMat<T,M>& a,
00088                  const int i,const int j,const int k,const int l,
00089                  const double s,const double tau) const;
00090     // Data
00091     SMat<T,M>& a;
00092   }; // end class EigenSystem<T,M>
00093 
00095   // DEFINITIONS
00097 
00099 
00100   // Constructor/Destructor
00101 
00104   template <class T, int M>
00105   inline EigenSystem<T,M>::EigenSystem(SMat<T,M>& a_) : a(a_) {
00106   }
00107 
00110   template <class T, int M>
00111   inline EigenSystem<T,M>::~EigenSystem() {
00112   }
00113 
00114   // Eigen System routines
00115 
00121   template <class T, int M>
00122   inline bool EigenSystem<T,M>::jacobi(Vec<T,M>& d, SMat<T,M>& v) {
00123     int nrot;
00124     return jacobi(d,v,&nrot);
00125   }
00126 
00133   template <class T, int M>
00134   inline bool EigenSystem<T,M>::jacobi(Vec<T,M>& d, SMat<T,M>& v, int* nrot) {
00135     int j,iq,ip,i;
00136     T tresh,theta,tau,t,sm,s,h,g,c;
00137     Vec<T,M> b;
00138     Vec<T,M> z;
00139 
00140     for (ip=0;ip<M;ip++) { // Initialize to the identity matrix.
00141       for (iq=0;iq<M;iq++) v.x[ip*M+iq]=0.0;
00142       v.x[ip*M+ip]=1.0;
00143     }
00144     for (ip=0;ip<M;ip++) { // Initialize b and d to the diagonal of a.
00145       b.x[ip]=d.x[ip]=a.x[ip*M+ip];
00146       z.x[ip]=0.0; // This vector will accumulate terms
00147     }
00148     *nrot=0;
00149     for (i=0;i<=50;i++) {
00150       sm=0.0;
00151       for (ip=0;ip<M-1;ip++) { // Sum off-diagonal elements
00152   for (iq=ip+1;iq<M;iq++) sm += fabs(a.x[ip*M+iq]);
00153       }
00154       if (sm == 0.0) return true;
00155       if (i < 4) tresh=0.2*sm/(M*M); // ...on the  rst three sweeps.
00156       else tresh=0.0; // ...thereafter.
00157       for (ip=0;ip<M-1;ip++) {
00158   for (iq=ip+1;iq<M;iq++) {
00159     g=100.0*fabs(a.x[ip*M+iq]);
00160 
00161     if (i > 4 && (T)(fabs(d.x[ip])+g) == (T)fabs(d.x[ip])
00162         && (T)(fabs(d.x[iq])+g) == (T)fabs(d.x[iq]))
00163       a.x[ip*M+iq]=0.0;
00164     else if (fabs(a.x[ip*M+iq]) > tresh) {
00165       h=d.x[iq]-d.x[ip];
00166       if ((T)(fabs(h)+g) == (T)fabs(h)) t=(a.x[ip*M+iq])/h;
00167       else {
00168         theta=0.5*h/(a.x[ip*M+iq]); // Equation (11.1.10).
00169         t=1.0/(fabs(theta)+sqrt(1.0+theta*theta));
00170         if (theta < 0.0) t = -t;
00171       }
00172       c=1.0/sqrt(1+t*t);
00173       s=t*c;
00174       tau=s/(1.0+c);
00175       h=t*a.x[ip*M+iq];
00176       z.x[ip] -= h;
00177       z.x[iq] += h;
00178       d.x[ip] -= h;
00179       d.x[iq] += h;
00180       a.x[ip*M+iq]=0.0;
00181       for (j=0;j<=ip-1;j++) { // Case of rotations 1 d j < p.
00182         PUMA_JACOBI_ROTATE(a,j,ip,j,iq)
00183     }
00184       for (j=ip+1;j<=iq-1;j++) { // Case of rotations p < j < q.
00185         PUMA_JACOBI_ROTATE(a,ip,j,j,iq)
00186     }
00187       for (j=iq+1;j<M;j++) { // Case of rotations q < j d n.
00188         PUMA_JACOBI_ROTATE(a,ip,j,iq,j)
00189     }
00190       for (j=0;j<M;j++) {
00191         PUMA_JACOBI_ROTATE(v,j,ip,j,iq)
00192     }
00193       ++(*nrot);
00194     }
00195   }
00196       }
00197       for (ip=0;ip<M;ip++) {
00198   b.x[ip] += z.x[ip];
00199   d.x[ip]=b.x[ip]; // Update d with the sum of tapq,
00200   z.x[ip]=0.0; // and reinitialize z.
00201       }
00202     }
00203     return false;
00204   }
00205 
00206   template <class T, int M>
00207   inline void EigenSystem<T,M>::rotate(double& g,double& h,SMat<T,M>& a,
00208                                        const int i,const int j,const int k,const int l,
00209                                        const double s,const double tau) const {
00210     g=a(i,j);
00211     h=a(k,l);
00212     a(i,j)=g-s*(h+g*tau);
00213     a(k,l)=h+s*(g-h*tau);
00214   }
00215 
00216   template <class T, int M>
00217   inline void EigenSystem<T,M>::rotateT(double& g,double& h,SMat<T,M>& a,
00218                                         const int i,const int j,const int k,const int l,
00219                                         const double s,const double tau) const {
00220     g=a(i,j);
00221     h=a(k,l);
00222     a(i,j)=static_cast<T>(g-s*(h+g*tau));
00223     a(k,l)=static_cast<T>(h+s*(g-h*tau));
00224   }
00225 
00226 
00233   template <class T, int M>
00234   inline void EigenSystem<T,M>::householderReduce(Vec<T,M>& d, Vec<T,M>& e,
00235               bool vecs) {
00236     int l,k,j,i;
00237     T scale,hh,h,g,f;
00238 
00239     // add wrapper to handle weird 1-based Numerical Recipes in C indexing
00240     int n = M;
00241     T* dd = d.x - 1;
00242     T* ee = e.x - 1;
00243     T** aa = new T*[M]; aa--;
00244     for (i=1;i<=M;i++) aa[i] = &(a.x[(i-1)*M-1]);
00245 
00246     for (i=n;i>=2;i--) {
00247       l=i-1;
00248       h=scale=0.0;
00249       if (l > 1) {
00250   for (k=1;k<=l;k++)
00251     scale += fabs(aa[i][k]);
00252   if (scale == 0.0)
00253     ee[i]=aa[i][l];
00254   else {
00255     for (k=1;k<=l;k++) {
00256       aa[i][k] /= scale;
00257       h += aa[i][k]*aa[i][k];
00258     }
00259     f=aa[i][l];
00260     g=(f >= 0.0 ? -sqrt(h) : sqrt(h));
00261     ee[i]=scale*g;
00262     h -= f*g;
00263     aa[i][l]=f-g;
00264     f=0.0;
00265     for (j=1;j<=l;j++) {
00266       /* Next statement can be omitted if eigenvectors not wanted */
00267       if (vecs) aa[j][i]=aa[i][j]/h;
00268       g=0.0;
00269       for (k=1;k<=j;k++)
00270         g += aa[j][k]*aa[i][k];
00271       for (k=j+1;k<=l;k++)
00272         g += aa[k][j]*aa[i][k];
00273       ee[j]=g/h;
00274       f += ee[j]*aa[i][j];
00275     }
00276     hh=f/(h+h);
00277     for (j=1;j<=l;j++) {
00278       f=aa[i][j];
00279       ee[j]=g=ee[j]-hh*f;
00280       for (k=1;k<=j;k++)
00281         aa[j][k] -= (f*ee[k]+g*aa[i][k]);
00282     }
00283   }
00284       } else
00285   ee[i]=aa[i][l];
00286       dd[i]=h;
00287     }
00288     /* Next statement can be omitted if eigenvectors not wanted */
00289     if (vecs) dd[1]=0.0;
00290     ee[1]=0.0;
00291     /* Contents of this loop can be omitted if eigenvectors not
00292        wanted except for statement dd[i]=aa[i][i]; */
00293     for (i=1;i<=n;i++) {
00294       if (vecs) {
00295   l=i-1;
00296   if (dd[i]) {
00297     for (j=1;j<=l;j++) {
00298       g=0.0;
00299       for (k=1;k<=l;k++)
00300         g += aa[i][k]*aa[k][j];
00301       for (k=1;k<=l;k++)
00302         aa[k][j] -= g*aa[k][i];
00303     }
00304   }
00305       }
00306       dd[i]=aa[i][i];
00307       if (vecs) {
00308   aa[i][i]=1.0;
00309   for (j=1;j<=l;j++) aa[j][i]=aa[i][j]=0.0;
00310       }
00311     }
00312 
00313     // free memory from stupid wrapper
00314     aa++; delete [] aa;
00315   }
00316 
00323   template <class T, int M>
00324   inline void EigenSystem<T,M>::householderSolve(Vec<T,M>& d, Vec<T,M>& e, bool vecs) {
00325     int m,l,iter,i,k;
00326     T s,r,p,g,f,dd,c,b;
00327 
00328     // add wrapper to handle weird 1-based Numerical Recipes in C indexing
00329     int n = M;
00330     T* ddd = d.x - 1;
00331     T* ee = e.x - 1;
00332     T** aa = new T*[M]; aa--;
00333     for (i=1;i<=M;i++) aa[i] = &(a.x[(i-1)*M-1]);
00334 
00335     for (i=2;i<=n;i++) ee[i-1]=ee[i];
00336     ee[n]=0.0;
00337     for (l=1;l<=n;l++) {
00338       iter=0;
00339       do {
00340   for (m=l;m<=n-1;m++) {
00341     dd=fabs(ddd[m])+fabs(ddd[m+1]);
00342     if ((T)(fabs(ee[m])+dd) == dd) break;
00343   }
00344   if (m != l) {
00345     if (iter++ == 30) {
00346       std::stringstream ss;
00347       ss << "EigenSystem<" << M << ">::householderSolve: error, "
00348          << "too many interaions." << std::endl << std::flush;
00349       throw Exception(ss.str());
00350     }
00351     g=(ddd[l+1]-ddd[l])/(2.0*ee[l]);
00352     r=pythag(g,T(1));
00353     g=ddd[m]-ddd[l]+ee[l]/(g+PUMA_SIGN(r,g));
00354     s=c=1.0;
00355     p=0.0;
00356     for (i=m-1;i>=l;i--) {
00357       f=s*ee[i];
00358       b=c*ee[i];
00359       ee[i+1]=(r=pythag(f,g));
00360       if (r == 0.0) {
00361         ddd[i+1] -= p;
00362         ee[m]=0.0;
00363         break;
00364       }
00365       s=f/r;
00366       c=g/r;
00367       g=ddd[i+1]-p;
00368       r=(ddd[i]-g)*s+2.0*c*b;
00369       ddd[i+1]=g+(p=s*r);
00370       g=c*r-b;
00371       /* Next loop can be omitted if eigenvectors not wanted*/
00372       if (vecs) for (k=1;k<=n;k++) {
00373         f=aa[k][i+1];
00374         aa[k][i+1]=s*aa[k][i]+c*f;
00375         aa[k][i]=c*aa[k][i]-s*f;
00376       }
00377     }
00378     if (r == 0.0 && i >= l) continue;
00379     ddd[l] -= p;
00380     ee[l]=g;
00381     ee[m]=0.0;
00382   }
00383       } while (m != l);
00384     }
00385 
00386     // free memory from stupid wrapper
00387     aa++; delete [] aa;
00388   }
00389 
00390 
00397   template<class T, int M>
00398   inline void EigenSystem<T, M>::eigenSort(Vec<T, M>& d, SMat<T, M>& v)
00399   {
00400     int k, j, i;
00401     T p;
00402     for(i = 0; i<M-1; i++) {
00403       p = rtc_abs(d.x[k = i]);
00404       for(j = i+1; j<M; j++)
00405         if (rtc_abs(d.x[j])>=p)
00406           p = d.x[k = j];
00407       if (k!=i) {
00408         d.x[k] = d.x[i];
00409         d.x[i] = p;
00410         for(j = 0; j<M; j++) {
00411           p = v.x[j*M+i];
00412           v.x[j*M+i] = v.x[j*M+k];
00413           v.x[j*M+k] = p;
00414         }
00415       }
00416     }
00417   }
00418 
00422   template <class T, int M>
00423   inline void EigenSystem<T,M>::balance() {
00424     int last,j,i;
00425     T s,r,g,f,c,sqrdx;
00426     sqrdx=PUMA_RADIX*PUMA_RADIX;
00427     last=0;
00428     while (last == 0) {
00429       last=1;
00430       for (i=0;i<M;i++) { // Calculate row and column norms.
00431   r=c=0.0;
00432   for (j=0;j<M;j++) if (j != i) {
00433     c += fabs(a.x[j*M+i]);
00434     r += fabs(a.x[i*M+j]);
00435   }
00436   if (c && r) { //If both are nonzero,
00437     g=r/PUMA_RADIX;
00438     f=1.0;
00439     s=c+r;
00440     while (c<g) {
00441       f *= PUMA_RADIX;
00442       c *= sqrdx;
00443     }
00444     g=r*PUMA_RADIX;
00445     while (c>g) {
00446       f /= PUMA_RADIX;
00447       c /= sqrdx;
00448     }
00449     if ((c+r)/f < 0.95*s) {
00450       last=0;
00451       g=1.0/f;
00452       for (j=0;j<M;j++) a.x[i*M+j] *= g;
00453       for (j=0;j<M;j++) a.x[j*M+i] *= f;
00454     }
00455   }
00456       }
00457     }
00458   }
00459 
00464   template <class T, int M>
00465   inline void EigenSystem<T,M>::hessenbergReduce() {
00466     int m,j,i;
00467     T y,x;
00468 
00469     for (m=1;m<M-1;m++) { // m is called r + 1 in the text.
00470       x=0.0;
00471       i=m;
00472       for (j=m;j<M;j++) { // Find the pivot.
00473   if (fabs(a.x[j*M+m-1]) > fabs(x)) {
00474     x=a.x[j*M+m-1];
00475     i=j;
00476   }
00477       }
00478       if (i != m) { // Interchange rows and columns.
00479   for (j=m-1;j<M;j++) puma_swap(a.x[i*M+j],a.x[m*M+j]);
00480   for (j=0;j<M;j++) puma_swap(a.x[j*M+i],a.x[j*M+m]);
00481       }
00482       if (x) { // Carry out the elimination.
00483   for (i=m+1;i<M;i++) {
00484     if ((y=a.x[i*M+m-1]) != 0.0) {
00485       y /= x;
00486       a.x[i*M+m-1]=y;
00487       for (j=m;j<M;j++) a.x[i*M+j] -= y*a.x[m*M+j];
00488       for (j=0;j<M;j++) a.x[j*M+m] += y*a.x[j*M+i];
00489     }
00490   }
00491       }
00492     }
00493   }
00494 
00499   template <class T, int M>
00500   inline void EigenSystem<T,M>::qr(Vec<T,M>& wreal, Vec<T,M>& wimag) {
00501     int nn,m,l,k,j,its,i,mmin;
00502     T z,y,x,w,v,u,t,s,r,q,p,anorm;
00503 
00504     T* wr = wreal.x - 1;
00505     T* wi = wimag.x - 1;
00506     T** aa = new T*[M]; aa--;
00507     for (i=1;i<=M;i++) aa[i] = &(a.x[(i-1)*M-1]);
00508 
00509     anorm=0.0;
00510     for (i=1;i<=M;i++)
00511       for (j=rtc_max(i-1,1);j<=M;j++)
00512   anorm += fabs(aa[i][j]);
00513     nn=M;
00514     t=0.0;
00515     while (nn >= 1) {
00516       its=0;
00517       do {
00518   for (l=nn;l>=2;l--) {
00519     s=fabs(aa[l-1][l-1])+fabs(aa[l][l]);
00520     if (s == 0.0) s=anorm;
00521     if ((T)(fabs(aa[l][l-1]) + s) == s) {
00522       aa[l][l-1]=0.0;
00523       break;
00524     }
00525   }
00526   x=aa[nn][nn];
00527   if (l == nn) {
00528     wr[nn]=x+t;
00529     wi[nn--]=0.0;
00530   } else {
00531     y=aa[nn-1][nn-1];
00532     w=aa[nn][nn-1]*aa[nn-1][nn];
00533     if (l == (nn-1)) {
00534       p=0.5*(y-x);
00535       q=p*p+w;
00536       z=sqrt(fabs(q));
00537       x += t;
00538       if (q >= 0.0) { // ...a real pair.
00539         z=p+PUMA_SIGN(z,p);
00540         wr[nn-1]=wr[nn]=x+z;
00541         if (z) wr[nn]=x-w/z;
00542         wi[nn-1]=wi[nn]=0.0;
00543       } else { //...a complex pair.
00544         wr[nn-1]=wr[nn]=x+p;
00545         wi[nn-1]= -(wi[nn]=z);
00546       }
00547       nn -= 2;
00548     } else { // No roots found. Continue iteration.
00549       if (its == 30) {
00550         std::stringstream ss;
00551         ss << "EigenSystem<" << M << ">::qr: error, " << "too many iterations in hqr\n";
00552         throw Exception(ss.str());
00553       }
00554       if (its == 10 || its == 20) { // Form exceptional shift.
00555         t += x;
00556         for (i=1;i<=nn;i++) aa[i][i] -= x;
00557         s=fabs(aa[nn][nn-1])+fabs(aa[nn-1][nn-2]);
00558         y=x=0.75*s;
00559         w = -0.4375*s*s;
00560       }
00561       ++its;
00562       for (m=(nn-2);m>=l;m--) {
00563         z=aa[m][m];
00564         r=x-z;
00565         s=y-z;
00566         p=(r*s-w)/aa[m+1][m]+aa[m][m+1]; // Equation (11.6.23).
00567         q=aa[m+1][m+1]-z-r-s;
00568         r=aa[m+2][m+1];
00569         s=fabs(p)+fabs(q)+fabs(r);
00570         p /= s; q /= s; r /= s;
00571         if (m == l) break;
00572 
00573         u=fabs(aa[m][m-1])*(fabs(q)+fabs(r));
00574         v=fabs(p)*(fabs(aa[m-1][m-1])+fabs(z)+fabs(aa[m+1][m+1]));
00575         if ((T)(u+v) == v) break;
00576       }
00577       for (i=m+2;i<=nn;i++) {
00578         aa[i][i-2]=0.0;
00579         if (i != (m+2)) aa[i][i-3]=0.0;
00580       }
00581       for (k=m;k<=nn-1;k++) {
00582         if (k != m) {
00583     p=aa[k][k-1];
00584     q=aa[k+1][k-1];
00585     r=0.0;
00586     if (k != (nn-1)) r=aa[k+2][k-1];
00587     if ((x=fabs(p)+fabs(q)+fabs(r)) != 0.0) {
00588       p /= x;
00589       q /= x;
00590       r /= x;
00591     }
00592         }
00593         if ((s=PUMA_SIGN(sqrt(p*p+q*q+r*r),p)) != 0.0) {
00594     if (k == m) {
00595       if (l != m)
00596         aa[k][k-1] = -aa[k][k-1];
00597     } else
00598       aa[k][k-1] = -s*x;
00599     p += s;
00600     x=p/s;
00601     y=q/s;
00602     z=r/s;
00603     q /= p;
00604     r /= p;
00605     for (j=k;j<=nn;j++) {
00606       p=aa[k][j]+q*aa[k+1][j];
00607       if (k != (nn-1)) {
00608         p += r*aa[k+2][j];
00609         aa[k+2][j] -= p*z;
00610       }
00611       aa[k+1][j] -= p*y;
00612       aa[k][j] -= p*x;
00613     }
00614     mmin = nn<k+3 ? nn : k+3;
00615     for (i=l;i<=mmin;i++) {
00616       p=x*aa[i][k]+y*aa[i][k+1];
00617       if (k != (nn-1)) {
00618         p += z*aa[i][k+2];
00619         aa[i][k+2] -= p*r;
00620       }
00621       aa[i][k+1] -= p*q;
00622       aa[i][k] -= p;
00623     }
00624         }
00625       }
00626     }
00627         }
00628       } while (l < nn-1);
00629     }
00630     aa++; delete [] aa;
00631   }
00632 
00633 }; // end namspace puma
00634 
00635 #endif


rtc
Author(s): Benjamin Pitzer
autogenerated on Thu Jan 2 2014 11:04:53