converted theta and prior_ratio to fixed-point.
[speexdsp.git] / libspeex / preprocess.c
1 /* Copyright (C) 2003 Epic Games (written by Jean-Marc Valin)
2    Copyright (C) 2004-2006 Epic Games 
3    
4    File: preprocess.c
5    Preprocessor with denoising based on the algorithm by Ephraim and Malah
6
7    Redistribution and use in source and binary forms, with or without
8    modification, are permitted provided that the following conditions are
9    met:
10
11    1. Redistributions of source code must retain the above copyright notice,
12    this list of conditions and the following disclaimer.
13
14    2. Redistributions in binary form must reproduce the above copyright
15    notice, this list of conditions and the following disclaimer in the
16    documentation and/or other materials provided with the distribution.
17
18    3. The name of the author may not be used to endorse or promote products
19    derived from this software without specific prior written permission.
20
21    THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
22    IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
23    OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24    DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
25    INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
26    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
28    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
29    STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31    POSSIBILITY OF SUCH DAMAGE.
32 */
33
34
35 /*
36    Recommended papers:
37    
38    Y. Ephraim and D. Malah, "Speech enhancement using minimum mean-square error
39    short-time spectral amplitude estimator". IEEE Transactions on Acoustics, 
40    Speech and Signal Processing, vol. ASSP-32, no. 6, pp. 1109-1121, 1984.
41    
42    Y. Ephraim and D. Malah, "Speech enhancement using minimum mean-square error
43    log-spectral amplitude estimator". IEEE Transactions on Acoustics, Speech and 
44    Signal Processing, vol. ASSP-33, no. 2, pp. 443-445, 1985.
45    
46    I. Cohen and B. Berdugo, "Speech enhancement for non-stationary noise environments".
47    Signal Processing, vol. 81, no. 2, pp. 2403-2418, 2001.
48
49    Stefan Gustafsson, Rainer Martin, Peter Jax, and Peter Vary. "A psychoacoustic 
50    approach to combined acoustic echo cancellation and noise reduction". IEEE 
51    Transactions on Speech and Audio Processing, 2002.
52    
53    J.-M. Valin, J. Rouat, and F. Michaud, "Microphone array post-filter for separation
54    of simultaneous non-stationary sources". In Proceedings IEEE International 
55    Conference on Acoustics, Speech, and Signal Processing, 2004.
56 */
57
58 #ifdef HAVE_CONFIG_H
59 #include "config.h"
60 #endif
61
62 #include <math.h>
63 #include "speex/speex_preprocess.h"
64 #include "speex/speex_echo.h"
65 #include "misc.h"
66 #include "fftwrap.h"
67 #include "filterbank.h"
68 #include "math_approx.c"
69
70 #ifndef M_PI
71 #define M_PI 3.14159263
72 #endif
73
74 #define LOUDNESS_EXP 2.5
75
76 #define NB_BANDS 24
77
78 #define SPEEX_PROB_START_DEFAULT       0.35f
79 #define SPEEX_PROB_CONTINUE_DEFAULT    0.20f
80 #define NOISE_SUPPRESS_DEFAULT       -25
81 #define ECHO_SUPPRESS_DEFAULT        -45
82 #define ECHO_SUPPRESS_ACTIVE_DEFAULT -15
83
84 #ifndef NULL
85 #define NULL 0
86 #endif
87
88 #define SQR(x) ((x)*(x))
89 #define SQR16(x) (MULT16_16((x),(x)))
90 #define SQR16_Q15(x) (MULT16_16_Q15((x),(x)))
91
92 #define FLOOR(x) x = floor(x);
93
94 #ifdef FIXED_POINT
95 static inline spx_word16_t DIV32_16_Q8(spx_word32_t a, spx_word32_t b)
96 {
97    if (SHR32(a,7) >= b)
98    {
99       return 32767;
100    } else {
101       if (b>=QCONST32(1,23))
102       {
103          a = SHR32(a,8);
104          b = SHR32(b,8);
105       }
106       if (b>=QCONST32(1,19))
107       {
108          a = SHR32(a,4);
109          b = SHR32(b,4);
110       }
111       if (b>=QCONST32(1,15))
112       {
113          a = SHR32(a,4);
114          b = SHR32(b,4);
115       }
116       a = SHL32(a,8);
117       return PDIV32_16(a,b);
118    }
119    
120 }
121 static inline spx_word16_t DIV32_16_Q15(spx_word32_t a, spx_word32_t b)
122 {
123    if (SHR32(a,15) >= b)
124    {
125       return 32767;
126    } else {
127       if (b>=QCONST32(1,23))
128       {
129          a = SHR32(a,8);
130          b = SHR32(b,8);
131       }
132       if (b>=QCONST32(1,19))
133       {
134          a = SHR32(a,4);
135          b = SHR32(b,4);
136       }
137       if (b>=QCONST32(1,15))
138       {
139          a = SHR32(a,4);
140          b = SHR32(b,4);
141       }
142       a = SHL32(a,15);      
143       return DIV32_16(a,b);
144    }
145 }
146 #define SNR_SCALING 256.f
147 #define SNR_SCALING_1 0.0039062f
148 #define SNR_SHIFT 8
149
150 #define FRAC_SCALING 32767.f
151 #define FRAC_SCALING_1 3.0518e-05
152 #define FRAC_SHIFT 1
153
154 #define EXPIN_SCALING 2048.f
155 #define EXPIN_SCALING_1 0.00048828f
156 #define EXPIN_SHIFT 11
157 #define EXPOUT_SCALING_1 1.5259e-05
158
159 #define NOISE_SHIFT 7
160
161 #else
162
163 #define DIV32_16_Q8(a,b) ((a)/(b))
164 #define DIV32_16_Q15(a,b) ((a)/(b))
165 #define SNR_SCALING 1.f
166 #define SNR_SCALING_1 1.f
167 #define SNR_SHIFT 0
168 #define FRAC_SCALING 1.f
169 #define FRAC_SCALING_1 1.f
170 #define FRAC_SHIFT 0
171 #define NOISE_SHIFT 0
172
173 #define EXPIN_SCALING 1.f
174 #define EXPIN_SCALING_1 1.f
175 #define EXPOUT_SCALING_1 1.f
176
177 #endif
178
179 /** Speex pre-processor state. */
180 struct SpeexPreprocessState_ {
181    /* Basic info */
182    int    frame_size;        /**< Number of samples processed each time */
183    int    ps_size;           /**< Number of points in the power spectrum */
184    int    sampling_rate;     /**< Sampling rate of the input/output */
185    int    nbands;
186    FilterBank *bank;
187    
188    /* Parameters */
189    int    denoise_enabled;
190    int    agc_enabled;
191    float  agc_level;
192    int    vad_enabled;
193    int    dereverb_enabled;
194    float  reverb_decay;
195    float  reverb_level;
196    float  speech_prob_start;
197    float  speech_prob_continue;
198    int    noise_suppress;
199    int    echo_suppress;
200    int    echo_suppress_active;
201    SpeexEchoState *echo_state;
202    
203    /* DSP-related arrays */
204    spx_word16_t *frame;      /**< Processing frame (2*ps_size) */
205    spx_word16_t *ft;         /**< Processing frame in freq domain (2*ps_size) */
206    spx_word32_t *ps;         /**< Current power spectrum */
207    spx_word16_t *gain2;      /**< Adjusted gains */
208    spx_word16_t *gain_floor; /**< Minimum gain allowed */
209    spx_word16_t *window;     /**< Analysis/Synthesis window */
210    spx_word32_t *noise;      /**< Noise estimate */
211    spx_word32_t *reverb_estimate; /**< Estimate of reverb energy */
212    spx_word32_t *old_ps;     /**< Power spectrum for last frame */
213    spx_word16_t *gain;       /**< Ephraim Malah gain */
214    spx_word16_t *prior;      /**< A-priori SNR */
215    spx_word16_t *post;       /**< A-posteriori SNR */
216
217    spx_word32_t *S;          /**< Smoothed power spectrum */
218    spx_word32_t *Smin;       /**< See Cohen paper */
219    spx_word32_t *Stmp;       /**< See Cohen paper */
220    int *update_prob;       /**< Propability of speech presence for noise update */
221
222    spx_word16_t *zeta;       /**< Smoothed a priori SNR */
223
224    float *loudness_weight;   /**< Perceptual loudness curve */
225
226    spx_word32_t *echo_noise;
227    spx_word32_t *residual_echo;
228
229    /* Misc */
230    spx_word16_t *inbuf;      /**< Input buffer (overlapped analysis) */
231    spx_word16_t *outbuf;     /**< Output buffer (for overlap and add) */
232
233    int    was_speech;
234    float  loudness;          /**< loudness estimate */
235    float  loudness2;         /**< loudness estimate */
236    int    nb_adapt;          /**< Number of frames used for adaptation so far */
237    int    nb_loudness_adapt; /**< Number of frames used for loudness adaptation so far */
238    int    min_count;         /**< Number of frames processed so far */
239    void  *fft_lookup;        /**< Lookup table for the FFT */
240 #ifdef FIXED_POINT
241    int    frame_shift;
242 #endif
243 };
244
245
246 static void conj_window(spx_word16_t *w, int len)
247 {
248    int i;
249    for (i=0;i<len;i++)
250    {
251       float tmp;
252       float x=4*((float)i)/len;
253       int inv=0;
254       if (x<1)
255       {
256       } else if (x<2)
257       {
258          x=2-x;
259          inv=1;
260       } else if (x<3)
261       {
262          x=x-2;
263          inv=1;
264       } else {
265          x=4-x;
266       }
267       x*=1.9979;
268       tmp=(.5-.5*cos(x))*(.5-.5*cos(x));
269       if (inv)
270          tmp=1-tmp;
271       w[i]=Q15_ONE*sqrt(tmp);
272    }
273 }
274
275 /* This function approximates the gain function 
276    y = gamma(1.25)^2 * M(-.25;1;-x) / sqrt(x)  
277    which multiplied by xi/(1+xi) is the optimal gain
278    in the loudness domain ( sqrt[amplitude] )
279 */
280 static inline float hypergeom_gain(float x)
281 {
282    int ind;
283    float integer, frac;
284    static const float table[21] = {
285       0.82157f, 1.02017f, 1.20461f, 1.37534f, 1.53363f, 1.68092f, 1.81865f,
286       1.94811f, 2.07038f, 2.18638f, 2.29688f, 2.40255f, 2.50391f, 2.60144f,
287       2.69551f, 2.78647f, 2.87458f, 2.96015f, 3.04333f, 3.12431f, 3.20326f};
288       
289    integer = floor(2*x);
290    ind = (int)integer;
291    if (ind<0)
292       return 1;
293    if (ind>19)
294       return 1+.1296/x;
295    frac = 2*x-integer;
296    return ((1-frac)*table[ind] + frac*table[ind+1])/sqrt(x+.0001f);
297 }
298
299 static inline float qcurve(spx_word16_t x)
300 {
301    return 1.f/(1.f+.15f/(SNR_SCALING_1*x));
302 }
303
304 SpeexPreprocessState *speex_preprocess_state_init(int frame_size, int sampling_rate)
305 {
306    int i;
307    int N, N3, N4, M;
308
309    SpeexPreprocessState *st = (SpeexPreprocessState *)speex_alloc(sizeof(SpeexPreprocessState));
310    st->frame_size = frame_size;
311
312    /* Round ps_size down to the nearest power of two */
313 #if 0
314    i=1;
315    st->ps_size = st->frame_size;
316    while(1)
317    {
318       if (st->ps_size & ~i)
319       {
320          st->ps_size &= ~i;
321          i<<=1;
322       } else {
323          break;
324       }
325    }
326    
327    
328    if (st->ps_size < 3*st->frame_size/4)
329       st->ps_size = st->ps_size * 3 / 2;
330 #else
331    st->ps_size = st->frame_size;
332 #endif
333
334    N = st->ps_size;
335    N3 = 2*N - st->frame_size;
336    N4 = st->frame_size - N3;
337    
338    st->sampling_rate = sampling_rate;
339    st->denoise_enabled = 1;
340    st->agc_enabled = 0;
341    st->agc_level = 8000;
342    st->vad_enabled = 0;
343    st->dereverb_enabled = 0;
344    st->reverb_decay = .0;
345    st->reverb_level = .0;
346    st->noise_suppress = NOISE_SUPPRESS_DEFAULT;
347    st->echo_suppress = ECHO_SUPPRESS_DEFAULT;
348    st->echo_suppress_active = ECHO_SUPPRESS_ACTIVE_DEFAULT;
349
350    st->speech_prob_start = SPEEX_PROB_START_DEFAULT;
351    st->speech_prob_continue = SPEEX_PROB_CONTINUE_DEFAULT;
352
353    st->echo_state = NULL;
354    
355    st->nbands = NB_BANDS;
356    M = st->nbands;
357    st->bank = filterbank_new(M, sampling_rate, N, 1);
358    
359    st->frame = (spx_word16_t*)speex_alloc(2*N*sizeof(float));
360    st->window = (spx_word16_t*)speex_alloc(2*N*sizeof(float));
361    st->ft = (spx_word16_t*)speex_alloc(2*N*sizeof(float));
362    
363    st->ps = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
364    st->noise = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
365    st->echo_noise = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
366    st->residual_echo = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
367    st->reverb_estimate = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
368    st->old_ps = (spx_word32_t*)speex_alloc((N+M)*sizeof(float));
369    st->prior = (spx_word16_t*)speex_alloc((N+M)*sizeof(float));
370    st->post = (spx_word16_t*)speex_alloc((N+M)*sizeof(float));
371    st->gain = (spx_word16_t*)speex_alloc((N+M)*sizeof(float));
372    st->gain2 = (spx_word16_t*)speex_alloc((N+M)*sizeof(float));
373    st->gain_floor = (spx_word16_t*)speex_alloc((N+M)*sizeof(float));
374    st->zeta = (spx_word16_t*)speex_alloc((N+M)*sizeof(float));
375    
376    st->S = (spx_word32_t*)speex_alloc(N*sizeof(float));
377    st->Smin = (spx_word32_t*)speex_alloc(N*sizeof(float));
378    st->Stmp = (spx_word32_t*)speex_alloc(N*sizeof(float));
379    st->update_prob = (int*)speex_alloc(N*sizeof(float));
380    
381    st->loudness_weight = (float*)speex_alloc(N*sizeof(float));
382    st->inbuf = (spx_word16_t*)speex_alloc(N3*sizeof(float));
383    st->outbuf = (spx_word16_t*)speex_alloc(N3*sizeof(float));
384
385    conj_window(st->window, 2*N3);
386    for (i=2*N3;i<2*st->ps_size;i++)
387       st->window[i]=Q15_ONE;
388    
389    if (N4>0)
390    {
391       for (i=N3-1;i>=0;i--)
392       {
393          st->window[i+N3+N4]=st->window[i+N3];
394          st->window[i+N3]=1;
395       }
396    }
397    for (i=0;i<N+M;i++)
398    {
399       st->noise[i]=QCONST32(1.f,NOISE_SHIFT);
400       st->reverb_estimate[i]=0.;
401       st->old_ps[i]=1.;
402       st->gain[i]=Q15_ONE;
403       st->post[i]=Q15_ONE;
404       st->prior[i]=Q15_ONE;
405    }
406
407    for (i=0;i<N;i++)
408       st->update_prob[i] = 1;
409    for (i=0;i<N3;i++)
410    {
411       st->inbuf[i]=0;
412       st->outbuf[i]=0;
413    }
414
415    for (i=0;i<N;i++)
416    {
417       float ff=((float)i)*.5*sampling_rate/((float)N);
418       st->loudness_weight[i] = .35f-.35f*ff/16000.f+.73f*exp(-.5f*(ff-3800)*(ff-3800)/9e5f);
419       if (st->loudness_weight[i]<.01f)
420          st->loudness_weight[i]=.01f;
421       st->loudness_weight[i] *= st->loudness_weight[i];
422    }
423
424    st->was_speech = 0;
425    st->loudness = pow(6000,LOUDNESS_EXP);
426    st->loudness2 = 6000;
427    st->nb_loudness_adapt = 0;
428
429    st->fft_lookup = spx_fft_init(2*N);
430
431    st->nb_adapt=0;
432    st->min_count=0;
433    return st;
434 }
435
436 void speex_preprocess_state_destroy(SpeexPreprocessState *st)
437 {
438    speex_free(st->frame);
439    speex_free(st->ft);
440    speex_free(st->ps);
441    speex_free(st->gain2);
442    speex_free(st->gain_floor);
443    speex_free(st->window);
444    speex_free(st->noise);
445    speex_free(st->reverb_estimate);
446    speex_free(st->old_ps);
447    speex_free(st->gain);
448    speex_free(st->prior);
449    speex_free(st->post);
450    speex_free(st->loudness_weight);
451    speex_free(st->echo_noise);
452    speex_free(st->residual_echo);
453
454    speex_free(st->S);
455    speex_free(st->Smin);
456    speex_free(st->Stmp);
457    speex_free(st->update_prob);
458    speex_free(st->zeta);
459
460    speex_free(st->inbuf);
461    speex_free(st->outbuf);
462
463    spx_fft_destroy(st->fft_lookup);
464    filterbank_destroy(st->bank);
465    speex_free(st);
466 }
467
468 static void speex_compute_agc(SpeexPreprocessState *st)
469 {
470    int i;
471    int N = st->ps_size;
472    float scale=.5f/N;
473    float agc_gain;
474    int freq_start, freq_end;
475    float active_bands = 0;
476
477    freq_start = (int)(300.0f*2*N/st->sampling_rate);
478    freq_end   = (int)(2000.0f*2*N/st->sampling_rate);
479    for (i=freq_start;i<freq_end;i++)
480    {
481       if (st->S[i] > 20.f*st->Smin[i]+1000.f)
482          active_bands+=1;
483    }
484    active_bands /= (freq_end-freq_start+1);
485
486    if (active_bands > .2f)
487    {
488       float loudness=0.f;
489       float rate, rate2=.2f;
490       st->nb_loudness_adapt++;
491       rate=2.0f/(1+st->nb_loudness_adapt);
492       if (rate < .05f)
493          rate = .05f;
494       if (rate < .1f && pow(loudness, LOUDNESS_EXP) > st->loudness)
495          rate = .1f;
496       if (rate < .2f && pow(loudness, LOUDNESS_EXP) > 3.f*st->loudness)
497          rate = .2f;
498       if (rate < .4f && pow(loudness, LOUDNESS_EXP) > 10.f*st->loudness)
499          rate = .4f;
500
501       for (i=2;i<N;i++)
502       {
503          loudness += scale*st->ps[i] * FRAC_SCALING_1*FRAC_SCALING_1*st->gain2[i] * st->gain2[i] * st->loudness_weight[i];
504       }
505       loudness=sqrt(loudness);
506       /*if (loudness < 2*pow(st->loudness, 1.0/LOUDNESS_EXP) &&
507         loudness*2 > pow(st->loudness, 1.0/LOUDNESS_EXP))*/
508       st->loudness = (1-rate)*st->loudness + (rate)*pow(loudness, LOUDNESS_EXP);
509       
510       st->loudness2 = (1-rate2)*st->loudness2 + rate2*pow(st->loudness, 1.0f/LOUDNESS_EXP);
511
512       loudness = pow(st->loudness, 1.0f/LOUDNESS_EXP);
513
514       /*fprintf (stderr, "%f %f %f\n", loudness, st->loudness2, rate);*/
515    }
516    
517    agc_gain = st->agc_level/st->loudness2;
518    /*fprintf (stderr, "%f %f %f %f\n", active_bands, st->loudness, st->loudness2, agc_gain);*/
519    if (agc_gain>200)
520       agc_gain = 200;
521
522    for (i=0;i<N;i++)
523       st->gain2[i] *= agc_gain;
524    
525 }
526
527 static void preprocess_analysis(SpeexPreprocessState *st, spx_int16_t *x)
528 {
529    int i;
530    int N = st->ps_size;
531    int N3 = 2*N - st->frame_size;
532    int N4 = st->frame_size - N3;
533    spx_word32_t *ps=st->ps;
534
535    /* 'Build' input frame */
536    for (i=0;i<N3;i++)
537       st->frame[i]=st->inbuf[i];
538    for (i=0;i<st->frame_size;i++)
539       st->frame[N3+i]=x[i];
540    
541    /* Update inbuf */
542    for (i=0;i<N3;i++)
543       st->inbuf[i]=x[N4+i];
544
545    /* Windowing */
546    for (i=0;i<2*N;i++)
547       st->frame[i] = MULT16_16_Q15(st->frame[i], st->window[i]);
548
549 #ifdef FIXED_POINT
550    {
551       spx_word16_t max_val=0;
552       for (i=0;i<2*N;i++)
553          max_val = MAX16(max_val, ABS16(st->frame[i]));
554       st->frame_shift = 14-spx_ilog2(EXTEND32(max_val));
555       for (i=0;i<2*N;i++)
556          st->frame[i] = SHL16(st->frame[i], st->frame_shift);
557    }
558 #endif
559    
560    /* Perform FFT */
561    spx_fft(st->fft_lookup, st->frame, st->ft);
562          
563    /* Power spectrum */
564    ps[0]=1;
565    for (i=1;i<N;i++)
566       ps[i]=1+MULT16_16(st->ft[2*i-1],st->ft[2*i-1]) + MULT16_16(st->ft[2*i],st->ft[2*i]);
567    for (i=0;i<N;i++)
568       st->ps[i] = PSHR32(st->ps[i], 2*st->frame_shift);
569
570    filterbank_compute_bank32(st->bank, ps, ps+N);
571 }
572
573 static void update_noise_prob(SpeexPreprocessState *st)
574 {
575    int i;
576    int min_range;
577    int N = st->ps_size;
578
579    for (i=1;i<N-1;i++)
580       st->S[i] = 100.f+ .8f*st->S[i] + .05f*st->ps[i-1]+.1f*st->ps[i]+.05f*st->ps[i+1];
581    st->S[0] = 100.f+ .8f*st->S[0] + .2*st->ps[0];
582    st->S[N-1] = 100.f+ .8f*st->S[N-1] + .2*st->ps[N-1];
583    
584    if (st->nb_adapt==1)
585    {
586       for (i=0;i<N;i++)
587          st->Smin[i] = st->Stmp[i] = 0.f;
588    }
589
590    if (st->nb_adapt < 100)
591       min_range = 15;
592    else if (st->nb_adapt < 1000)
593       min_range = 50;
594    else if (st->nb_adapt < 10000)
595       min_range = 150;
596    else
597       min_range = 300;
598    if (st->min_count > min_range)
599    {
600       st->min_count = 0;
601       for (i=0;i<N;i++)
602       {
603          st->Smin[i] = MIN32(st->Stmp[i], st->S[i]);
604          st->Stmp[i] = st->S[i];
605       }
606    } else {
607       for (i=0;i<N;i++)
608       {
609          st->Smin[i] = MIN32(st->Smin[i], st->S[i]);
610          st->Stmp[i] = MIN32(st->Stmp[i], st->S[i]);      
611       }
612    }
613    for (i=0;i<N;i++)
614    {
615       if (st->S[i] > 2.5*st->Smin[i])
616          st->update_prob[i] = 1;
617       else
618          st->update_prob[i] = 0;
619       /*fprintf (stderr, "%f ", st->S[i]/st->Smin[i]);*/
620       /*fprintf (stderr, "%f ", st->update_prob[i]);*/
621    }
622
623 }
624
625 #define NOISE_OVERCOMPENS 1.
626
627 void speex_echo_get_residual(SpeexEchoState *st, spx_word32_t *Yout, int len);
628
629 int speex_preprocess(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
630 {
631    return speex_preprocess_run(st, x);
632 }
633
634 int speex_preprocess_run(SpeexPreprocessState *st, spx_int16_t *x)
635 {
636    int i;
637    int M;
638    int N = st->ps_size;
639    int N3 = 2*N - st->frame_size;
640    int N4 = st->frame_size - N3;
641    spx_word32_t *ps=st->ps;
642    float Zframe=0, Pframe;
643    float beta, beta_1;
644    float echo_floor;
645    float noise_floor;
646    
647    st->nb_adapt++;
648    st->min_count++;
649    
650    beta =1.0f/st->nb_adapt;
651    if (beta < .03f)
652       beta=.03f;
653    beta_1 = 1.0f-beta;
654    M = st->nbands;
655    /* Deal with residual echo if provided */
656    if (st->echo_state)
657    {
658       speex_echo_get_residual(st->echo_state, st->residual_echo, N);
659       for (i=0;i<N;i++)
660          st->echo_noise[i] = MAX32(.6f*st->echo_noise[i], st->residual_echo[i]);
661       filterbank_compute_bank32(st->bank, st->echo_noise, st->echo_noise+N);
662    } else {
663       for (i=0;i<N+M;i++)
664          st->echo_noise[i] = 0;
665    }
666    preprocess_analysis(st, x);
667
668    update_noise_prob(st);
669
670    /* Noise estimation always updated for the 10 first frames */
671    /*if (st->nb_adapt<10)
672    {
673       for (i=1;i<N-1;i++)
674          st->update_prob[i] = 0;
675    }
676    */
677    
678    /* Update the noise estimate for the frequencies where it can be */
679    for (i=0;i<N;i++)
680    {
681       if (!st->update_prob[i] || st->ps[i] < PSHR32(st->noise[i], NOISE_SHIFT))
682          st->noise[i] = beta_1*st->noise[i] + beta*NOISE_OVERCOMPENS*SHL32(st->ps[i],NOISE_SHIFT);
683    }
684    filterbank_compute_bank32(st->bank, st->noise, st->noise+N);
685
686    /* Special case for first frame */
687    if (st->nb_adapt==1)
688       for (i=0;i<N+M;i++)
689          st->old_ps[i] = ps[i];
690
691    /* Compute a posteriori SNR */
692    for (i=0;i<N+M;i++)
693    {
694       spx_word16_t gamma;
695       
696       /* Total noise estimate including residual echo and reverberation */
697       spx_word32_t tot_noise = 1.f+ PSHR32(st->noise[i],NOISE_SHIFT) + st->echo_noise[i] + st->reverb_estimate[i];
698       
699       /* A posteriori SNR = ps/noise - 1*/
700       st->post[i] = SUB16(DIV32_16_Q8(ps[i],tot_noise), QCONST16(1.f,SNR_SHIFT));
701       st->post[i]=MIN16(st->post[i], QCONST16(100.f,SNR_SHIFT));
702       
703       /* Computing update gamma = .1 + .9*(old/(old+noise))^2 */
704       gamma = QCONST16(.1f,15)+MULT16_16_Q15(QCONST16(.89f,15),SQR16_Q15(DIV32_16_Q15(st->old_ps[i],ADD32(st->old_ps[i],tot_noise))));
705       
706       /* A priori SNR update = gamma*max(0,post) + (1-gamma)*old/noise */
707       st->prior[i] = PSHR16(ADD32(MULT16_16(gamma,MAX16(0,st->post[i])), MULT16_16(Q15_ONE-gamma,DIV32_16_Q8(st->old_ps[i],tot_noise))), 15);
708       st->prior[i]=MIN16(st->prior[i], QCONST16(100.f,SNR_SHIFT));
709    }
710
711    /*print_vec(st->post, N+M, "");*/
712
713    /* Recursive average of the a priori SNR. A bit smoothed for the psd components */
714    st->zeta[0] = .7f*st->zeta[0] + .3f*st->prior[0];
715    for (i=1;i<N-1;i++)
716       st->zeta[i] = .7f*st->zeta[i] + .3f*(.5f*st->prior[i]+.25f*st->prior[i+1]+.25f*st->prior[i-1]);
717    for (i=N-1;i<N+M;i++)
718       st->zeta[i] = .7f*st->zeta[i] + .3f*st->prior[i];
719
720    /* Speech probability of presence for the entire frame is based on the average filterbank a priori SNR */
721    Zframe = 0;
722    for (i=N;i<N+M;i++)
723       Zframe += st->zeta[i];
724    Zframe /= st->nbands;
725    Pframe = .1+.9*qcurve(Zframe);
726    
727    noise_floor = exp(.2302585f*st->noise_suppress);
728    echo_floor = exp(.2302585f* (st->echo_suppress*(1-Pframe) + st->echo_suppress_active*Pframe));
729    
730    /* Compute Ephraim & Malah gain speech probability of presence for each critical band (Bark scale) 
731       Technically this is actually wrong because the EM gaim assumes a slightly different probability 
732       distribution */
733    for (i=N;i<N+M;i++)
734    {
735       spx_word32_t theta;
736       float MM;
737       spx_word16_t prior_ratio;
738       float q;
739       float P1;
740
741       /* Compute the gain floor based on different floors for the background noise and residual echo */
742       st->gain_floor[i] = FRAC_SCALING*sqrt((noise_floor*PSHR32(st->noise[i],NOISE_SHIFT) + echo_floor*st->echo_noise[i])/(1+PSHR32(st->noise[i],NOISE_SHIFT) + st->echo_noise[i]));
743       prior_ratio = FRAC_SCALING*st->prior[i]/(SNR_SCALING+st->prior[i]);
744       theta = MULT16_32_P15(prior_ratio, QCONST32(1.f,EXPIN_SHIFT)+SHL32(EXTEND32(st->post[i]),EXPIN_SHIFT-SNR_SHIFT));
745
746       MM = hypergeom_gain(EXPIN_SCALING_1*theta);
747       /* Gain with bound */
748       st->gain[i] = MIN16(FRAC_SCALING, prior_ratio * MM);
749       
750       /* Save old Bark power spectrum */
751       st->old_ps[i] = .2*st->old_ps[i] + .8*FRAC_SCALING_1*FRAC_SCALING_1*st->gain[i]*st->gain[i]*ps[i];
752
753       P1 = .2+.8*qcurve (st->zeta[i]);
754       q = 1-Pframe*P1;
755 #ifdef FIXED_POINT
756       theta = MIN32(theta, 32767);
757 #endif
758       st->gain2[i]=FRAC_SCALING/(1.f + (q/(1.f-q))*(1.f+SNR_SCALING_1*st->prior[i])*EXPOUT_SCALING_1*spx_exp(-EXTRACT16(theta)));
759    }
760    filterbank_compute_psd16(st->bank,st->gain2+N, st->gain2);
761    filterbank_compute_psd16(st->bank,st->gain+N, st->gain);
762    
763    /* Use 1 for linear gain resolution (best) or 0 for Bark gain resolution (faster) */
764    if (1)
765    {
766       filterbank_compute_psd16(st->bank,st->gain_floor+N, st->gain_floor);
767    
768       /* Compute gain according to the Ephraim-Malah algorithm */
769       for (i=0;i<N;i++)
770       {
771          float MM;
772          float theta;
773          float prior_ratio;
774          float p;
775          float g;
776          
777          /* Wiener filter gain */
778          prior_ratio = st->prior[i]/(SNR_SCALING+st->prior[i]);
779          theta = (1.f+SNR_SCALING_1*st->post[i])*prior_ratio;
780          p = FRAC_SCALING_1*st->gain2[i];
781          
782          /* Optimal estimator for loudness domain */
783          MM = hypergeom_gain(theta);
784          g = prior_ratio * MM;
785          
786          /* Constrain the gain to be close to the Bark scale gain */
787          if (g > 3.f*FRAC_SCALING_1*st->gain[i])
788             g = 3.f*FRAC_SCALING_1*st->gain[i];
789          st->gain[i] = MIN16(Q15_ONE, FRAC_SCALING*g);
790          
791          /* Save old power spectrum */
792          st->old_ps[i] = .2*st->old_ps[i] + .8*FRAC_SCALING_1*FRAC_SCALING_1*st->gain[i]*st->gain[i]*ps[i];
793          
794          /* Apply gain floor */
795          if (st->gain[i] < st->gain_floor[i])
796             st->gain[i] = st->gain_floor[i];
797
798          /* Exponential decay model for reverberation (unused) */
799          /*st->reverb_estimate[i] = st->reverb_decay*st->reverb_estimate[i] + st->reverb_decay*st->reverb_level*st->gain[i]*st->gain[i]*st->ps[i];*/
800          
801          /* Take into account speech probability of presence (loudness domain MMSE estimator) */
802          st->gain2[i]=FRAC_SCALING*SQR(p*sqrt(FRAC_SCALING_1*st->gain[i])+sqrt(FRAC_SCALING_1*st->gain_floor[i])*(1-p));
803
804          /* Use this if you want a log-domain MMSE estimator instead */
805          /*st->gain2[i] = pow(st->gain[i], p) * pow(st->gain_floor[i],1.f-p);*/
806          
807       }
808    } else {
809       for (i=N;i<N+M;i++)
810       {
811          float p = FRAC_SCALING_1*st->gain2[i];
812          if (st->gain[i] < st->gain_floor[i])
813             st->gain[i] = st->gain_floor[i];
814          st->gain2[i]=FRAC_SCALING*(p*sqrt(FRAC_SCALING_1*st->gain[i])+sqrt(FRAC_SCALING_1*st->gain_floor[i])*(1-p)) * (p*sqrt(FRAC_SCALING_1*st->gain[i])+sqrt(FRAC_SCALING_1*st->gain_floor[i])*(1-p));
815       }
816       filterbank_compute_psd16(st->bank,st->gain2+N, st->gain2);
817       
818    }
819    
820    if (!st->denoise_enabled)
821    {
822       for (i=0;i<N+M;i++)
823          st->gain2[i]=FRAC_SCALING;
824    }
825    
826    if (st->agc_enabled)
827       speex_compute_agc(st);
828
829    /* Apply computed gain */
830    for (i=1;i<N;i++)
831    {
832       st->ft[2*i-1] *= FRAC_SCALING_1*st->gain2[i];
833       st->ft[2*i] *= FRAC_SCALING_1*st->gain2[i];
834    }
835    st->ft[0] *= FRAC_SCALING_1*st->gain2[0];
836    st->ft[2*N-1] *= FRAC_SCALING_1*st->gain2[N-1];
837
838    /* Inverse FFT with 1/N scaling */
839    spx_ifft(st->fft_lookup, st->ft, st->frame);
840    for (i=0;i<2*N;i++)
841       st->frame[i] = PSHR16(st->frame[i], st->frame_shift);
842
843    {
844       float max_sample=0;
845       for (i=0;i<2*N;i++)
846          if (fabs(st->frame[i])>max_sample)
847             max_sample = fabs(st->frame[i]);
848       if (max_sample>28000.f)
849       {
850          float damp = 28000.f/max_sample;
851          for (i=0;i<2*N;i++)
852             st->frame[i] *= damp;
853       }
854    }
855
856    for (i=0;i<2*N;i++)
857       st->frame[i] = MULT16_16_Q15(st->frame[i], st->window[i]);
858
859    /* Perform overlap and add */
860    for (i=0;i<N3;i++)
861       x[i] = st->outbuf[i] + st->frame[i];
862    for (i=0;i<N4;i++)
863       x[N3+i] = st->frame[N3+i];
864    
865    /* Update outbuf */
866    for (i=0;i<N3;i++)
867       st->outbuf[i] = st->frame[st->frame_size+i];
868
869    if (st->vad_enabled)
870    {
871       if (Pframe > st->speech_prob_start || (st->was_speech && Pframe > st->speech_prob_continue))
872       {
873          st->was_speech=1;
874          return 1;
875       } else
876       {
877          st->was_speech=0;
878          return 0;
879       }
880    } else {
881       return 1;
882    }
883 }
884
885 void speex_preprocess_estimate_update(SpeexPreprocessState *st, spx_int16_t *x, spx_int32_t *echo)
886 {
887    int i;
888    int N = st->ps_size;
889    int N3 = 2*N - st->frame_size;
890    int M;
891    spx_word32_t *ps=st->ps;
892
893    M = st->nbands;
894    st->min_count++;
895    
896    preprocess_analysis(st, x);
897
898    update_noise_prob(st);
899    
900    for (i=1;i<N-1;i++)
901    {
902       if (!st->update_prob[i] || st->ps[i] < PSHR32(st->noise[i],NOISE_SHIFT))
903       {
904          st->noise[i] = .95f*st->noise[i] + .1f*st->ps[i];
905       }
906    }
907
908    for (i=0;i<N3;i++)
909       st->outbuf[i] = MULT16_16_Q15(x[st->frame_size-N3+i],st->window[st->frame_size+i]);
910
911    /* Save old power spectrum */
912    for (i=0;i<N+M;i++)
913       st->old_ps[i] = ps[i];
914
915    for (i=1;i<N;i++)
916       st->reverb_estimate[i] *= st->reverb_decay;
917 }
918
919
920 int speex_preprocess_ctl(SpeexPreprocessState *state, int request, void *ptr)
921 {
922    int i;
923    SpeexPreprocessState *st;
924    st=(SpeexPreprocessState*)state;
925    switch(request)
926    {
927    case SPEEX_PREPROCESS_SET_DENOISE:
928       st->denoise_enabled = (*(int*)ptr);
929       break;
930    case SPEEX_PREPROCESS_GET_DENOISE:
931       (*(int*)ptr) = st->denoise_enabled;
932       break;
933
934    case SPEEX_PREPROCESS_SET_AGC:
935       st->agc_enabled = (*(int*)ptr);
936       break;
937    case SPEEX_PREPROCESS_GET_AGC:
938       (*(int*)ptr) = st->agc_enabled;
939       break;
940
941    case SPEEX_PREPROCESS_SET_AGC_LEVEL:
942       st->agc_level = (*(float*)ptr);
943       if (st->agc_level<1)
944          st->agc_level=1;
945       if (st->agc_level>32768)
946          st->agc_level=32768;
947       break;
948    case SPEEX_PREPROCESS_GET_AGC_LEVEL:
949       (*(float*)ptr) = st->agc_level;
950       break;
951
952    case SPEEX_PREPROCESS_SET_VAD:
953       speex_warning("The VAD has been removed pending a complete rewrite");
954       st->vad_enabled = (*(spx_int32_t*)ptr);
955       break;
956    case SPEEX_PREPROCESS_GET_VAD:
957       (*(spx_int32_t*)ptr) = st->vad_enabled;
958       break;
959    
960    case SPEEX_PREPROCESS_SET_DEREVERB:
961       st->dereverb_enabled = (*(int*)ptr);
962       for (i=0;i<st->ps_size;i++)
963          st->reverb_estimate[i]=0;
964       break;
965    case SPEEX_PREPROCESS_GET_DEREVERB:
966       (*(int*)ptr) = st->dereverb_enabled;
967       break;
968
969    case SPEEX_PREPROCESS_SET_DEREVERB_LEVEL:
970       st->reverb_level = (*(float*)ptr);
971       break;
972    case SPEEX_PREPROCESS_GET_DEREVERB_LEVEL:
973       (*(float*)ptr) = st->reverb_level;
974       break;
975    
976    case SPEEX_PREPROCESS_SET_DEREVERB_DECAY:
977       st->reverb_decay = (*(float*)ptr);
978       break;
979    case SPEEX_PREPROCESS_GET_DEREVERB_DECAY:
980       (*(float*)ptr) = st->reverb_decay;
981       break;
982
983    case SPEEX_PREPROCESS_SET_PROB_START:
984       st->speech_prob_start = (*(int*)ptr) / 100.0;
985       if ( st->speech_prob_start > 1 || st->speech_prob_start < 0 )
986          st->speech_prob_start = SPEEX_PROB_START_DEFAULT;
987       break;
988    case SPEEX_PREPROCESS_GET_PROB_START:
989       (*(int*)ptr) = st->speech_prob_start * 100;
990       break;
991
992    case SPEEX_PREPROCESS_SET_PROB_CONTINUE:
993       st->speech_prob_continue = (*(int*)ptr) / 100.0;
994       if ( st->speech_prob_continue > 1 || st->speech_prob_continue < 0 )
995          st->speech_prob_continue = SPEEX_PROB_CONTINUE_DEFAULT;
996       break;
997    case SPEEX_PREPROCESS_GET_PROB_CONTINUE:
998       (*(int*)ptr) = st->speech_prob_continue * 100;
999       break;
1000
1001    case SPEEX_PREPROCESS_SET_NOISE_SUPPRESS:
1002       st->noise_suppress = -ABS(*(spx_int32_t*)ptr);
1003       break;
1004    case SPEEX_PREPROCESS_GET_NOISE_SUPPRESS:
1005       (*(spx_int32_t*)ptr) = st->noise_suppress;
1006       break;
1007    case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS:
1008       st->echo_suppress = -ABS(*(spx_int32_t*)ptr);
1009       break;
1010    case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS:
1011       (*(spx_int32_t*)ptr) = st->echo_suppress;
1012       break;
1013    case SPEEX_PREPROCESS_SET_ECHO_SUPPRESS_ACTIVE:
1014       st->echo_suppress_active = -ABS(*(spx_int32_t*)ptr);
1015       break;
1016    case SPEEX_PREPROCESS_GET_ECHO_SUPPRESS_ACTIVE:
1017       (*(spx_int32_t*)ptr) = st->echo_suppress_active;
1018       break;
1019    case SPEEX_PREPROCESS_SET_ECHO_STATE:
1020       st->echo_state = (SpeexEchoState*)ptr;
1021       break;
1022    case SPEEX_PREPROCESS_GET_ECHO_STATE:
1023       ptr = (void*)st->echo_state;
1024       break;
1025
1026    default:
1027       speex_warning_int("Unknown speex_preprocess_ctl request: ", request);
1028       return -1;
1029    }
1030    return 0;
1031 }