• Main Page
  • Related Pages
  • Modules
  • Data Structures
  • Files
  • Examples
  • File List
  • Globals

libavcodec/iirfilter.c

Go to the documentation of this file.
00001 /*
00002  * IIR filter
00003  * Copyright (c) 2008 Konstantin Shishkov
00004  *
00005  * This file is part of FFmpeg.
00006  *
00007  * FFmpeg is free software; you can redistribute it and/or
00008  * modify it under the terms of the GNU Lesser General Public
00009  * License as published by the Free Software Foundation; either
00010  * version 2.1 of the License, or (at your option) any later version.
00011  *
00012  * FFmpeg is distributed in the hope that it will be useful,
00013  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00015  * Lesser General Public License for more details.
00016  *
00017  * You should have received a copy of the GNU Lesser General Public
00018  * License along with FFmpeg; if not, write to the Free Software
00019  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
00020  */
00021 
00027 #include "iirfilter.h"
00028 #include <math.h>
00029 
00033 typedef struct FFIIRFilterCoeffs{
00034     int   order;
00035     float gain;
00036     int   *cx;
00037     float *cy;
00038 }FFIIRFilterCoeffs;
00039 
00043 typedef struct FFIIRFilterState{
00044     float x[1];
00045 }FFIIRFilterState;
00046 
00048 #define MAXORDER 30
00049 
00050 static int butterworth_init_coeffs(void *avc, struct FFIIRFilterCoeffs *c,
00051                                    enum IIRFilterMode filt_mode,
00052                                    int order, float cutoff_ratio,
00053                                    float stopband)
00054 {
00055     int i, j;
00056     double wa;
00057     double p[MAXORDER + 1][2];
00058 
00059     if (filt_mode != FF_FILTER_MODE_LOWPASS) {
00060         av_log(avc, AV_LOG_ERROR, "Butterworth filter currently only supports "
00061                "low-pass filter mode\n");
00062         return -1;
00063     }
00064     if (order & 1) {
00065         av_log(avc, AV_LOG_ERROR, "Butterworth filter currently only supports "
00066                "even filter orders\n");
00067         return -1;
00068     }
00069 
00070     wa = 2 * tan(M_PI * 0.5 * cutoff_ratio);
00071 
00072     c->cx[0] = 1;
00073     for(i = 1; i < (order >> 1) + 1; i++)
00074         c->cx[i] = c->cx[i - 1] * (order - i + 1LL) / i;
00075 
00076     p[0][0] = 1.0;
00077     p[0][1] = 0.0;
00078     for(i = 1; i <= order; i++)
00079         p[i][0] = p[i][1] = 0.0;
00080     for(i = 0; i < order; i++){
00081         double zp[2];
00082         double th = (i + (order >> 1) + 0.5) * M_PI / order;
00083         double a_re, a_im, c_re, c_im;
00084         zp[0] = cos(th) * wa;
00085         zp[1] = sin(th) * wa;
00086         a_re = zp[0] + 2.0;
00087         c_re = zp[0] - 2.0;
00088         a_im =
00089         c_im = zp[1];
00090         zp[0] = (a_re * c_re + a_im * c_im) / (c_re * c_re + c_im * c_im);
00091         zp[1] = (a_im * c_re - a_re * c_im) / (c_re * c_re + c_im * c_im);
00092 
00093         for(j = order; j >= 1; j--)
00094         {
00095             a_re = p[j][0];
00096             a_im = p[j][1];
00097             p[j][0] = a_re*zp[0] - a_im*zp[1] + p[j-1][0];
00098             p[j][1] = a_re*zp[1] + a_im*zp[0] + p[j-1][1];
00099         }
00100         a_re    = p[0][0]*zp[0] - p[0][1]*zp[1];
00101         p[0][1] = p[0][0]*zp[1] + p[0][1]*zp[0];
00102         p[0][0] = a_re;
00103     }
00104     c->gain = p[order][0];
00105     for(i = 0; i < order; i++){
00106         c->gain += p[i][0];
00107         c->cy[i] = (-p[i][0] * p[order][0] + -p[i][1] * p[order][1]) /
00108                    (p[order][0] * p[order][0] + p[order][1] * p[order][1]);
00109     }
00110     c->gain /= 1 << order;
00111 
00112     return 0;
00113 }
00114 
00115 static int biquad_init_coeffs(void *avc, struct FFIIRFilterCoeffs *c,
00116                               enum IIRFilterMode filt_mode, int order,
00117                               float cutoff_ratio, float stopband)
00118 {
00119     double cos_w0, sin_w0;
00120     double a0, x0, x1;
00121 
00122     if (filt_mode != FF_FILTER_MODE_HIGHPASS &&
00123         filt_mode != FF_FILTER_MODE_LOWPASS) {
00124         av_log(avc, AV_LOG_ERROR, "Biquad filter currently only supports "
00125                "high-pass and low-pass filter modes\n");
00126         return -1;
00127     }
00128     if (order != 2) {
00129         av_log(avc, AV_LOG_ERROR, "Biquad filter must have order of 2\n");
00130         return -1;
00131     }
00132 
00133     cos_w0 = cos(M_PI * cutoff_ratio);
00134     sin_w0 = sin(M_PI * cutoff_ratio);
00135 
00136     a0 = 1.0 + (sin_w0 / 2.0);
00137 
00138     if (filt_mode == FF_FILTER_MODE_HIGHPASS) {
00139         c->gain  =  ((1.0 + cos_w0) / 2.0)  / a0;
00140         x0       =  ((1.0 + cos_w0) / 2.0)  / a0;
00141         x1       = (-(1.0 + cos_w0))        / a0;
00142     } else { // FF_FILTER_MODE_LOWPASS
00143         c->gain  =  ((1.0 - cos_w0) / 2.0)  / a0;
00144         x0       =  ((1.0 - cos_w0) / 2.0)  / a0;
00145         x1       =   (1.0 - cos_w0)         / a0;
00146     }
00147     c->cy[0] = (-1.0 + (sin_w0 / 2.0)) / a0;
00148     c->cy[1] =  (2.0 *  cos_w0)        / a0;
00149 
00150     // divide by gain to make the x coeffs integers.
00151     // during filtering, the delay state will include the gain multiplication
00152     c->cx[0] = lrintf(x0 / c->gain);
00153     c->cx[1] = lrintf(x1 / c->gain);
00154 
00155     return 0;
00156 }
00157 
00158 av_cold struct FFIIRFilterCoeffs* ff_iir_filter_init_coeffs(void *avc,
00159                                                 enum IIRFilterType filt_type,
00160                                                 enum IIRFilterMode filt_mode,
00161                                                 int order, float cutoff_ratio,
00162                                                 float stopband, float ripple)
00163 {
00164     FFIIRFilterCoeffs *c;
00165     int ret = 0;
00166 
00167     if (order <= 0 || order > MAXORDER || cutoff_ratio >= 1.0)
00168         return NULL;
00169 
00170     FF_ALLOCZ_OR_GOTO(avc, c,     sizeof(FFIIRFilterCoeffs),
00171                       init_fail);
00172     FF_ALLOC_OR_GOTO (avc, c->cx, sizeof(c->cx[0]) * ((order >> 1) + 1),
00173                       init_fail);
00174     FF_ALLOC_OR_GOTO (avc, c->cy, sizeof(c->cy[0]) * order,
00175                       init_fail);
00176     c->order = order;
00177 
00178     switch (filt_type) {
00179     case FF_FILTER_TYPE_BUTTERWORTH:
00180         ret = butterworth_init_coeffs(avc, c, filt_mode, order, cutoff_ratio,
00181                                       stopband);
00182         break;
00183     case FF_FILTER_TYPE_BIQUAD:
00184         ret = biquad_init_coeffs(avc, c, filt_mode, order, cutoff_ratio,
00185                                  stopband);
00186         break;
00187     default:
00188         av_log(avc, AV_LOG_ERROR, "filter type is not currently implemented\n");
00189         goto init_fail;
00190     }
00191 
00192     if (!ret)
00193         return c;
00194 
00195 init_fail:
00196     ff_iir_filter_free_coeffs(c);
00197     return NULL;
00198 }
00199 
00200 av_cold struct FFIIRFilterState* ff_iir_filter_init_state(int order)
00201 {
00202     FFIIRFilterState* s = av_mallocz(sizeof(FFIIRFilterState) + sizeof(s->x[0]) * (order - 1));
00203     return s;
00204 }
00205 
00206 #define CONV_S16(dest, source) dest = av_clip_int16(lrintf(source));
00207 
00208 #define CONV_FLT(dest, source) dest = source;
00209 
00210 #define FILTER_BW_O4_1(i0, i1, i2, i3, fmt)         \
00211     in = *src0 * c->gain                            \
00212          + c->cy[0]*s->x[i0] + c->cy[1]*s->x[i1]    \
00213          + c->cy[2]*s->x[i2] + c->cy[3]*s->x[i3];   \
00214     res =  (s->x[i0] + in      )*1                  \
00215          + (s->x[i1] + s->x[i3])*4                  \
00216          +  s->x[i2]            *6;                 \
00217     CONV_##fmt(*dst0, res)                          \
00218     s->x[i0] = in;                                  \
00219     src0 += sstep;                                  \
00220     dst0 += dstep;
00221 
00222 #define FILTER_BW_O4(type, fmt) {           \
00223     int i;                                  \
00224     const type *src0 = src;                 \
00225     type       *dst0 = dst;                 \
00226     for (i = 0; i < size; i += 4) {         \
00227         float in, res;                      \
00228         FILTER_BW_O4_1(0, 1, 2, 3, fmt);    \
00229         FILTER_BW_O4_1(1, 2, 3, 0, fmt);    \
00230         FILTER_BW_O4_1(2, 3, 0, 1, fmt);    \
00231         FILTER_BW_O4_1(3, 0, 1, 2, fmt);    \
00232     }                                       \
00233 }
00234 
00235 #define FILTER_DIRECT_FORM_II(type, fmt) {                                  \
00236     int i;                                                                  \
00237     const type *src0 = src;                                                 \
00238     type       *dst0 = dst;                                                 \
00239     for (i = 0; i < size; i++) {                                            \
00240         int j;                                                              \
00241         float in, res;                                                      \
00242         in = *src0 * c->gain;                                               \
00243         for(j = 0; j < c->order; j++)                                       \
00244             in += c->cy[j] * s->x[j];                                       \
00245         res = s->x[0] + in + s->x[c->order >> 1] * c->cx[c->order >> 1];    \
00246         for(j = 1; j < c->order >> 1; j++)                                  \
00247             res += (s->x[j] + s->x[c->order - j]) * c->cx[j];               \
00248         for(j = 0; j < c->order - 1; j++)                                   \
00249             s->x[j] = s->x[j + 1];                                          \
00250         CONV_##fmt(*dst0, res)                                              \
00251         s->x[c->order - 1] = in;                                            \
00252         src0 += sstep;                                                      \
00253         dst0 += dstep;                                                      \
00254     }                                                                       \
00255 }
00256 
00257 #define FILTER_O2(type, fmt) {                                              \
00258     int i;                                                                  \
00259     const type *src0 = src;                                                 \
00260     type       *dst0 = dst;                                                 \
00261     for (i = 0; i < size; i++) {                                            \
00262         float in = *src0   * c->gain  +                                     \
00263                    s->x[0] * c->cy[0] +                                     \
00264                    s->x[1] * c->cy[1];                                      \
00265         CONV_##fmt(*dst0, s->x[0] + in + s->x[1] * c->cx[1])                \
00266         s->x[0] = s->x[1];                                                  \
00267         s->x[1] = in;                                                       \
00268         src0 += sstep;                                                      \
00269         dst0 += dstep;                                                      \
00270     }                                                                       \
00271 }
00272 
00273 void ff_iir_filter(const struct FFIIRFilterCoeffs *c,
00274                    struct FFIIRFilterState *s, int size,
00275                    const int16_t *src, int sstep, int16_t *dst, int dstep)
00276 {
00277     if (c->order == 2) {
00278         FILTER_O2(int16_t, S16)
00279     } else if (c->order == 4) {
00280         FILTER_BW_O4(int16_t, S16)
00281     } else {
00282         FILTER_DIRECT_FORM_II(int16_t, S16)
00283     }
00284 }
00285 
00286 void ff_iir_filter_flt(const struct FFIIRFilterCoeffs *c,
00287                        struct FFIIRFilterState *s, int size,
00288                        const float *src, int sstep, float *dst, int dstep)
00289 {
00290     if (c->order == 2) {
00291         FILTER_O2(float, FLT)
00292     } else if (c->order == 4) {
00293         FILTER_BW_O4(float, FLT)
00294     } else {
00295         FILTER_DIRECT_FORM_II(float, FLT)
00296     }
00297 }
00298 
00299 av_cold void ff_iir_filter_free_state(struct FFIIRFilterState *state)
00300 {
00301     av_free(state);
00302 }
00303 
00304 av_cold void ff_iir_filter_free_coeffs(struct FFIIRFilterCoeffs *coeffs)
00305 {
00306     if(coeffs){
00307         av_free(coeffs->cx);
00308         av_free(coeffs->cy);
00309     }
00310     av_free(coeffs);
00311 }
00312 
00313 #ifdef TEST
00314 #define FILT_ORDER 4
00315 #define SIZE 1024
00316 int main(void)
00317 {
00318     struct FFIIRFilterCoeffs *fcoeffs = NULL;
00319     struct FFIIRFilterState  *fstate  = NULL;
00320     float cutoff_coeff = 0.4;
00321     int16_t x[SIZE], y[SIZE];
00322     int i;
00323     FILE* fd;
00324 
00325     fcoeffs = ff_iir_filter_init_coeffs(NULL, FF_FILTER_TYPE_BUTTERWORTH,
00326                                         FF_FILTER_MODE_LOWPASS, FILT_ORDER,
00327                                         cutoff_coeff, 0.0, 0.0);
00328     fstate  = ff_iir_filter_init_state(FILT_ORDER);
00329 
00330     for (i = 0; i < SIZE; i++) {
00331         x[i] = lrint(0.75 * INT16_MAX * sin(0.5*M_PI*i*i/SIZE));
00332     }
00333 
00334     ff_iir_filter(fcoeffs, fstate, SIZE, x, 1, y, 1);
00335 
00336     fd = fopen("in.bin", "w");
00337     fwrite(x, sizeof(x[0]), SIZE, fd);
00338     fclose(fd);
00339 
00340     fd = fopen("out.bin", "w");
00341     fwrite(y, sizeof(y[0]), SIZE, fd);
00342     fclose(fd);
00343 
00344     ff_iir_filter_free_coeffs(fcoeffs);
00345     ff_iir_filter_free_state(fstate);
00346     return 0;
00347 }
00348 #endif /* TEST */

Generated on Wed Apr 11 2012 07:31:33 for FFmpeg by  doxygen 1.7.1