Impala Documentation

00001 #ifndef Impala_Core_Array_MakeGaussIIR1d_h
00002 #define Impala_Core_Array_MakeGaussIIR1d_h
00003 
00004 //#include "Data/Basis/CxMath.h"
00005 #include <iostream>
00006 #include <string>
00007 #include "Core/Array/Arrays.h"
00008 
00009 namespace Impala
00010 {
00011 namespace Core
00012 {
00013 namespace Array
00014 {
00015 
00016 
00017 /*
00018 Adapted to Horus: Jan-Mark Geusebroek
00019 Orignal Author: Lucas J. van Vliet
00020         Delft University of Technology
00021 
00022 Reference: van Vliet LJ, Young IT, Verbeek PW: Recursive Gaussian derivative
00023            filters.  In: Proceedings ICPR '98. IEEE Computer Society Press,
00024               1998, pp. 509--514.
00025 */
00026 
00027 #define MAXRECUR 5    /* maximum order of recursion */
00028 #define MAXDERI  2    /* maximum derivative order */
00029 
00030 
00031 struct complex {
00032     complex() : re(0), im(0) {}
00033     double      re;
00034     double      im;
00035 };
00036 
00037 
00038 static inline void
00039 MulComplexNumbers(complex *c1, complex *c2, complex *c3)
00040 {
00041 
00042    c3->re = c1->re * c2->re - c1->im * c2->im;
00043    c3->im = c1->re * c2->im + c1->im * c2->re;
00044 
00045 }
00046 
00047 
00048 static inline void
00049 AddComplexNumbers(complex *c1, complex *c2, complex *c3)
00050 {
00051 
00052    c3->re = c1->re + c2->re;
00053    c3->im = c1->im + c2->im;
00054 
00055 }
00056 
00057 
00058 static inline double
00059 ComputeStDev(int nn, complex *pp, double q)
00060 {
00061     double    var, re, im;
00062     double    modulus, phase;
00063     int        mm;
00064 
00065    /*
00066     * Compute the variance
00067     */
00068    var = 0.0;
00069    for (mm = 1; mm <= (nn - (nn % 2)); mm += 2)
00070    {
00071             modulus = sqrt(pp[mm].re * pp[mm].re + pp[mm].im * pp[mm].im);
00072             phase   = atan(pp[mm].im / pp[mm].re);
00073                 modulus = exp( log( modulus ) / q );
00074                 phase   = phase / q;
00075             re = modulus * cos(phase);
00076             im = modulus * sin(phase);
00077 
00078       var += ((4 * (re + (re - 2) * (re*re + im*im))) /
00079                 pow(1 - 2*re + re*re + im*im, 2.0));
00080    }
00081    if ( (nn % 2) == 1 )
00082    {
00083             modulus = sqrt(pp[mm].re * pp[mm].re + pp[mm].im * pp[mm].im);
00084             phase   = atan(pp[mm].im / pp[mm].re);
00085                 modulus = exp( log( modulus ) / q );
00086                 phase   = phase / q;
00087             re = modulus * cos(phase);
00088             im = modulus * sin(phase);
00089       var += (2 * re) / pow(re - 1, 2.0);
00090    }
00091 
00092    return( (double) sqrt( var ) );
00093 }
00094 
00095 
00096 static inline void
00097 FillPoleCoef(int nn, complex *pp, int order)
00098 {
00099 
00100    /*
00101     * Store order of recursive filters in pp[0]
00102     */
00103    pp[0].re = (double) nn; pp[0].im =  0.0;
00104 
00105       switch (nn)
00106       {
00107          case 5:
00108             switch (order)
00109             {
00110                case 2:
00111                   pp[1].re =  0.70381; pp[1].im =  1.38271;
00112                   pp[2].re =  0.70381; pp[2].im = -1.38271;
00113                   pp[3].re =  1.42239; pp[3].im =  0.77978;
00114                   pp[4].re =  1.42239; pp[4].im = -0.77978;
00115                   pp[5].re =  1.69319; pp[5].im =  0.0;
00116                   break;
00117 
00118                case 1:
00119                   pp[1].re =  0.70237; pp[1].im =  1.38717;
00120                   pp[2].re =  0.70237; pp[2].im = -1.38717;
00121                   pp[3].re =  1.43280; pp[3].im =  0.77903;
00122                   pp[4].re =  1.43280; pp[4].im = -0.77903;
00123                   pp[5].re =  1.70346; pp[5].im =  0.0;
00124                   break;
00125 
00126                case 0:
00127                default:
00128                   pp[1].re =  0.85991; pp[1].im =  1.45235;
00129                   pp[2].re =  0.85991; pp[2].im = -1.45235;
00130                   pp[3].re =  1.60953; pp[3].im =  0.83009;
00131                   pp[4].re =  1.60953; pp[4].im = -0.83009;
00132                   pp[5].re =  1.87040; pp[5].im =  0.0;
00133                   break;
00134             }
00135             break;
00136 
00137          case 4:
00138             switch (order)
00139             {
00140                case 2:
00141                   pp[1].re =  0.94576; pp[1].im =  1.21364;
00142                   pp[2].re =  0.94576; pp[2].im = -1.21364;
00143                   pp[3].re =  1.59892; pp[3].im =  0.42668;
00144                   pp[4].re =  1.59892; pp[4].im = -0.42668;
00145                   break;
00146 
00147                case 1:
00148                   pp[1].re =  1.04198; pp[1].im =  1.25046;
00149                   pp[2].re =  1.04198; pp[2].im = -1.25046;
00150                   pp[3].re =  1.69337; pp[3].im =  0.45006;
00151                   pp[4].re =  1.69337; pp[4].im = -0.45006;
00152                   break;
00153 
00154                case 0:
00155                default:
00156                   pp[1].re =  1.13231; pp[1].im =  1.28122;
00157                   pp[2].re =  1.13231; pp[2].im = -1.28122;
00158                   pp[3].re =  1.78532; pp[3].im =  0.46766;
00159                   pp[4].re =  1.78532; pp[4].im = -0.46766;
00160                   break;
00161             }
00162             break;
00163 
00164          case 3:
00165             switch (order)
00166             {
00167                case 2:
00168                   pp[1].re =  1.21969; pp[1].im =  0.91724;
00169                   pp[2].re =  1.21969; pp[2].im = -0.91724;
00170                   pp[3].re =  1.69485; pp[3].im =  0.0;
00171                   break;
00172 
00173                case 1:
00174                   pp[1].re =  1.32094; pp[1].im =  0.97057;
00175                   pp[2].re =  1.32094; pp[2].im = -0.97057;
00176                   pp[3].re =  1.77635; pp[3].im =  0.0;
00177                   break;
00178 
00179                case 0:
00180                default:
00181                   pp[1].re =  1.41650; pp[1].im =  1.00829;
00182                   pp[2].re =  1.41650; pp[2].im = -1.00829;
00183                   pp[3].re =  1.86131; pp[3].im =  0.0;
00184                   break;
00185             }
00186             break;
00187 
00188          case 2:
00189             pp[1].re =  1.69580; pp[1].im =  0.59955;
00190             pp[2].re =  1.69580; pp[2].im = -0.59955;
00191             break;
00192 
00193          case 1:
00194             pp[1].re =  2.00000; pp[1].im =  0.00000;
00195             break;
00196 
00197          default:
00198             break;
00199       }
00200     return;
00201 }
00202 
00203 
00204 static inline void
00205 FilterCoef(int mm, int nn, complex *pp, int start, int stop, complex tmp,
00206            complex *bb)
00207 {
00208    int      ii;
00209    complex tmp2;
00210 
00211    /*
00212     * Initialize temporary product to 1.
00213     * Initialize result to 0.
00214     */
00215    if ((start == mm) && (stop == nn))
00216    {
00217       tmp.re  = 1.0;
00218       tmp.im  = 0.0;
00219       bb[nn - mm].re = 0.0;
00220       bb[nn - mm].im = 0.0;
00221    }
00222 
00223    if (start > 1)
00224    {
00225       for (ii = start; ii <= stop; ii++)
00226       {
00227          MulComplexNumbers( &tmp, &pp[ii], &tmp2 );
00228          FilterCoef( mm, nn, pp, start-1, ii-1, tmp2, bb );
00229       }
00230    }
00231    else if (start == 1)
00232    {
00233       for (ii = start; ii <= stop; ii++)
00234       {
00235          MulComplexNumbers( &tmp, &pp[ii], &tmp2 );
00236          AddComplexNumbers( &tmp2, &bb[nn - mm], &bb[nn - mm] );
00237       }
00238    }
00239    else if ((start == 0) && (mm == 0))
00240    {
00241       bb[nn - mm].re = 1.0;
00242       bb[nn - mm].im = 0.0;
00243    }
00244 
00245     return;
00246 }
00247 
00248 inline Array2dScalarReal64*
00249 MakeGaussIIR1d(double sigma, int derivativeOrder, int filterOrder)
00250 {
00251     int size = 2*filterOrder+1;
00252     double* filter = new double[size];
00253 
00254    double        q, cc;
00255    double        phase, modulus;
00256    int          jj;
00257 
00258    int         nn, mm;
00259    complex      bb[MAXRECUR+1];
00260    complex      pp[MAXRECUR+1];
00261    complex      tmp;
00262    double        q0, qs, dq;
00263 
00264 
00265     if (filterOrder > MAXRECUR)
00266         filterOrder = MAXRECUR;
00267 
00268     if (derivativeOrder > MAXDERI) {
00269         std::cout << "MakeGaussIIR1d : derivative order too large" << std::endl;
00270         derivativeOrder = MAXDERI;
00271     }
00272 
00273    for (jj = 0; jj <= 2*filterOrder; jj++)
00274       filter[jj] = 0;
00275 
00276    /*
00277     * Fetch the desired poles, 
00278     * depending on the filter order and derivative order
00279     */
00280    FillPoleCoef( filterOrder, pp, derivativeOrder );
00281    nn = pp[0].re; /* filter order */
00282 
00283    /*
00284     * Compute the correct value for q (based on the poles in the z-domain)
00285      * that yields a recursive filter with exactly the right 
00286      * standard deviation.
00287     */
00288    q = sigma / 2.0; /* fit was done for sigma = 2 */
00289     for ( q0 = q, qs = ComputeStDev(nn, pp, q); fabs(sigma - qs)>0.000001; ) {
00290             dq = qs/2.0 - q;
00291             q = q0 - dq;
00292             qs = ComputeStDev(nn, pp, q);
00293     }
00294 
00295     /*
00296      * Scale the poles
00297      */
00298     for (mm = 1; mm <= nn; mm++)
00299     {
00300         modulus = sqrt(pp[mm].re * pp[mm].re + pp[mm].im * pp[mm].im);
00301         phase   = atan(pp[mm].im / pp[mm].re);
00302         modulus = exp( log( modulus ) / q );
00303         phase   = phase / q;
00304         pp[mm].re = modulus * cos(phase);
00305         pp[mm].im = modulus * sin(phase);
00306     }
00307 
00308     /*
00309      * Compute the actual filter coefficients
00310      */
00311     for (mm = 0; mm <= nn; mm++)
00312     {
00313         FilterCoef( mm, nn, pp, mm, nn, tmp, bb );
00314     }
00315     for (mm = 0; mm <= nn; mm++)
00316     {
00317         bb[nn - mm].re  *= pow(-1.0, (double) (nn-mm)) / bb[0].re;
00318         bb[nn - mm].im  *= pow(-1.0, (double) (nn-mm)) / bb[0].re;
00319 
00320         //filter[nn - mm+filterOrder] = HxVec3Double(
00321         //    -bb[nn - mm].re, -bb[nn - mm].re, -bb[nn - mm].re);
00322         filter[nn - mm+filterOrder] = -bb[nn - mm].re;
00323     }
00324 
00325     /*
00326      * Normalization
00327      */
00328     cc = 0.0;
00329     for (mm = 0; mm <= nn; mm++)
00330         cc += filter[mm+filterOrder];
00331     if (cc < 0.0)
00332         cc = -cc;
00333     filter[filterOrder] = cc;
00334     for (mm = 0; mm < filterOrder; mm++)
00335         filter[mm] = filter[2*filterOrder-mm];
00336 
00337     return ArrayCreate<Array2dScalarReal64>(size, 1, 0, 0, filter);
00338 }
00339 
00340 } // namespace Array
00341 } // namespace Core
00342 } // namespace Impala
00343 
00344 #endif
MakeGaussIIR1d.h
