• Home
  • Line#
  • Scopes#
  • Navigate#
  • Raw
  • Download
1 /* Copyright (C) 2006-2008 CSIRO, Jean-Marc Valin, Xiph.Org Foundation
2 
3    File: scal.c
4    Shaped comb-allpass filter for channel decorrelation
5 
6    Redistribution and use in source and binary forms, with or without
7    modification, are permitted provided that the following conditions are
8    met:
9 
10    1. Redistributions of source code must retain the above copyright notice,
11    this list of conditions and the following disclaimer.
12 
13    2. Redistributions in binary form must reproduce the above copyright
14    notice, this list of conditions and the following disclaimer in the
15    documentation and/or other materials provided with the distribution.
16 
17    3. The name of the author may not be used to endorse or promote products
18    derived from this software without specific prior written permission.
19 
20    THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
21    IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
22    OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23    DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
24    INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
25    (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26    SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
27    HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
28    STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29    ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30    POSSIBILITY OF SUCH DAMAGE.
31 */
32 
33 /*
34 The algorithm implemented here is described in:
35 
36 * J.-M. Valin, Perceptually-Motivated Nonlinear Channel Decorrelation For
37   Stereo Acoustic Echo Cancellation, Accepted for Joint Workshop on
38   Hands­free Speech Communication and Microphone Arrays (HSCMA), 2008.
39   http://people.xiph.org/~jm/papers/valin_hscma2008.pdf
40 
41 */
42 
43 #ifdef HAVE_CONFIG_H
44 #include "config.h"
45 #endif
46 
47 #include "speex/speex_echo.h"
48 #include "vorbis_psy.h"
49 #include "arch.h"
50 #include "os_support.h"
51 #include "smallft.h"
52 #include <math.h>
53 #include <stdlib.h>
54 
55 #ifndef M_PI
56 #define M_PI           3.14159265358979323846  /* pi */
57 #endif
58 
59 #define ALLPASS_ORDER 20
60 
61 struct SpeexDecorrState_ {
62    int rate;
63    int channels;
64    int frame_size;
65 #ifdef VORBIS_PSYCHO
66    VorbisPsy *psy;
67    struct drft_lookup lookup;
68    float *wola_mem;
69    float *curve;
70 #endif
71    float *vorbis_win;
72    int    seed;
73    float *y;
74 
75    /* Per-channel stuff */
76    float *buff;
77    float (*ring)[ALLPASS_ORDER];
78    int *ringID;
79    int *order;
80    float *alpha;
81 };
82 
83 
84 
speex_decorrelate_new(int rate,int channels,int frame_size)85 EXPORT SpeexDecorrState *speex_decorrelate_new(int rate, int channels, int frame_size)
86 {
87    int i, ch;
88    SpeexDecorrState *st = speex_alloc(sizeof(SpeexDecorrState));
89    st->rate = rate;
90    st->channels = channels;
91    st->frame_size = frame_size;
92 #ifdef VORBIS_PSYCHO
93    st->psy = vorbis_psy_init(rate, 2*frame_size);
94    spx_drft_init(&st->lookup, 2*frame_size);
95    st->wola_mem = speex_alloc(frame_size*sizeof(float));
96    st->curve = speex_alloc(frame_size*sizeof(float));
97 #endif
98    st->y = speex_alloc(frame_size*sizeof(float));
99 
100    st->buff = speex_alloc(channels*2*frame_size*sizeof(float));
101    st->ringID = speex_alloc(channels*sizeof(int));
102    st->order = speex_alloc(channels*sizeof(int));
103    st->alpha = speex_alloc(channels*sizeof(float));
104    st->ring = speex_alloc(channels*ALLPASS_ORDER*sizeof(float));
105 
106    /*FIXME: The +20 is there only as a kludge for ALL_PASS_OLA*/
107    st->vorbis_win = speex_alloc((2*frame_size+20)*sizeof(float));
108    for (i=0;i<2*frame_size;i++)
109       st->vorbis_win[i] = sin(.5*M_PI* sin(M_PI*i/(2*frame_size))*sin(M_PI*i/(2*frame_size)) );
110    st->seed = rand();
111 
112    for (ch=0;ch<channels;ch++)
113    {
114       for (i=0;i<ALLPASS_ORDER;i++)
115          st->ring[ch][i] = 0;
116       st->ringID[ch] = 0;
117       st->alpha[ch] = 0;
118       st->order[ch] = 10;
119    }
120    return st;
121 }
122 
uni_rand(int * seed)123 static float uni_rand(int *seed)
124 {
125    const unsigned int jflone = 0x3f800000;
126    const unsigned int jflmsk = 0x007fffff;
127    union {int i; float f;} ran;
128    *seed = 1664525 * *seed + 1013904223;
129    ran.i = jflone | (jflmsk & *seed);
130    ran.f -= 1.5;
131    return 2*ran.f;
132 }
133 
irand(int * seed)134 static unsigned int irand(int *seed)
135 {
136    *seed = 1664525 * *seed + 1013904223;
137    return ((unsigned int)*seed)>>16;
138 }
139 
140 
speex_decorrelate(SpeexDecorrState * st,const spx_int16_t * in,spx_int16_t * out,int strength)141 EXPORT void speex_decorrelate(SpeexDecorrState *st, const spx_int16_t *in, spx_int16_t *out, int strength)
142 {
143    int ch;
144    float amount;
145 
146    if (strength<0)
147       strength = 0;
148    if (strength>100)
149       strength = 100;
150 
151    amount = .01*strength;
152    for (ch=0;ch<st->channels;ch++)
153    {
154       int i;
155       int N=2*st->frame_size;
156       float beta, beta2;
157       float *x;
158       float max_alpha = 0;
159 
160       float *buff;
161       float *ring;
162       int ringID;
163       int order;
164       float alpha;
165 
166       buff = st->buff+ch*2*st->frame_size;
167       ring = st->ring[ch];
168       ringID = st->ringID[ch];
169       order = st->order[ch];
170       alpha = st->alpha[ch];
171 
172       for (i=0;i<st->frame_size;i++)
173          buff[i] = buff[i+st->frame_size];
174       for (i=0;i<st->frame_size;i++)
175          buff[i+st->frame_size] = in[i*st->channels+ch];
176 
177       x = buff+st->frame_size;
178 
179       if (amount>1)
180          beta = 1-sqrt(.4*amount);
181       else
182          beta = 1-0.63246*amount;
183       if (beta<0)
184          beta = 0;
185 
186       beta2 = beta;
187       for (i=0;i<st->frame_size;i++)
188       {
189          st->y[i] = alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[st->frame_size+i+order]
190                + x[i-ALLPASS_ORDER]*st->vorbis_win[st->frame_size+i]
191                - alpha*(ring[ringID]
192                - beta*ring[ringID+1>=order?0:ringID+1]);
193          ring[ringID++]=st->y[i];
194          st->y[i] *= st->vorbis_win[st->frame_size+i];
195          if (ringID>=order)
196             ringID=0;
197       }
198       order = order+(irand(&st->seed)%3)-1;
199       if (order < 5)
200          order = 5;
201       if (order > 10)
202          order = 10;
203       /*order = 5+(irand(&st->seed)%6);*/
204       max_alpha = pow(.96+.04*(amount-1),order);
205       if (max_alpha > .98/(1.+beta2))
206          max_alpha = .98/(1.+beta2);
207 
208       alpha = alpha + .4*uni_rand(&st->seed);
209       if (alpha > max_alpha)
210          alpha = max_alpha;
211       if (alpha < -max_alpha)
212          alpha = -max_alpha;
213       for (i=0;i<ALLPASS_ORDER;i++)
214          ring[i] = 0;
215       ringID = 0;
216       for (i=0;i<st->frame_size;i++)
217       {
218          float tmp =  alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[i+order]
219                + x[i-ALLPASS_ORDER]*st->vorbis_win[i]
220                - alpha*(ring[ringID]
221                - beta*ring[ringID+1>=order?0:ringID+1]);
222          ring[ringID++]=tmp;
223          tmp *= st->vorbis_win[i];
224          if (ringID>=order)
225             ringID=0;
226          st->y[i] += tmp;
227       }
228 
229 #ifdef VORBIS_PSYCHO
230       float frame[N];
231       float scale = 1./N;
232       for (i=0;i<2*st->frame_size;i++)
233          frame[i] = buff[i];
234    //float coef = .5*0.78130;
235       float coef = M_PI*0.075063 * 0.93763 * amount * .8 * 0.707;
236       compute_curve(st->psy, buff, st->curve);
237       for (i=1;i<st->frame_size;i++)
238       {
239          float x1,x2;
240          float gain;
241          do {
242             x1 = uni_rand(&st->seed);
243             x2 = uni_rand(&st->seed);
244          } while (x1*x1+x2*x2 > 1.);
245          gain = coef*sqrt(.1+st->curve[i]);
246          frame[2*i-1] = gain*x1;
247          frame[2*i] = gain*x2;
248       }
249       frame[0] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[0]);
250       frame[2*st->frame_size-1] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[st->frame_size-1]);
251       spx_drft_backward(&st->lookup,frame);
252       for (i=0;i<2*st->frame_size;i++)
253          frame[i] *= st->vorbis_win[i];
254 #endif
255 
256       for (i=0;i<st->frame_size;i++)
257       {
258 #ifdef VORBIS_PSYCHO
259          float tmp = st->y[i] + frame[i] + st->wola_mem[i];
260          st->wola_mem[i] = frame[i+st->frame_size];
261 #else
262          float tmp = st->y[i];
263 #endif
264          if (tmp>32767)
265             tmp = 32767;
266          if (tmp < -32767)
267             tmp = -32767;
268          out[i*st->channels+ch] = tmp;
269       }
270 
271       st->ringID[ch] = ringID;
272       st->order[ch] = order;
273       st->alpha[ch] = alpha;
274 
275    }
276 }
277 
speex_decorrelate_destroy(SpeexDecorrState * st)278 EXPORT void speex_decorrelate_destroy(SpeexDecorrState *st)
279 {
280 #ifdef VORBIS_PSYCHO
281    vorbis_psy_destroy(st->psy);
282    speex_free(st->wola_mem);
283    speex_free(st->curve);
284 #endif
285    speex_free(st->buff);
286    speex_free(st->ring);
287    speex_free(st->ringID);
288    speex_free(st->alpha);
289    speex_free(st->vorbis_win);
290    speex_free(st->order);
291    speex_free(st->y);
292    speex_free(st);
293 }
294