294 lines
8.3 KiB
C
294 lines
8.3 KiB
C
/* Copyright (C) 2006-2008 CSIRO, Jean-Marc Valin, Xiph.Org Foundation
|
||
|
||
File: scal.c
|
||
Shaped comb-allpass filter for channel decorrelation
|
||
|
||
Redistribution and use in source and binary forms, with or without
|
||
modification, are permitted provided that the following conditions are
|
||
met:
|
||
|
||
1. Redistributions of source code must retain the above copyright notice,
|
||
this list of conditions and the following disclaimer.
|
||
|
||
2. Redistributions in binary form must reproduce the above copyright
|
||
notice, this list of conditions and the following disclaimer in the
|
||
documentation and/or other materials provided with the distribution.
|
||
|
||
3. The name of the author may not be used to endorse or promote products
|
||
derived from this software without specific prior written permission.
|
||
|
||
THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
|
||
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
|
||
OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||
DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
|
||
INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
|
||
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
|
||
HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
|
||
STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||
POSSIBILITY OF SUCH DAMAGE.
|
||
*/
|
||
|
||
/*
|
||
The algorithm implemented here is described in:
|
||
|
||
* J.-M. Valin, Perceptually-Motivated Nonlinear Channel Decorrelation For
|
||
Stereo Acoustic Echo Cancellation, Accepted for Joint Workshop on
|
||
Handsfree Speech Communication and Microphone Arrays (HSCMA), 2008.
|
||
http://people.xiph.org/~jm/papers/valin_hscma2008.pdf
|
||
|
||
*/
|
||
|
||
#ifdef HAVE_CONFIG_H
|
||
#include "config.h"
|
||
#endif
|
||
|
||
#include "speex/speex_echo.h"
|
||
#include "vorbis_psy.h"
|
||
#include "arch.h"
|
||
#include "os_support.h"
|
||
#include "smallft.h"
|
||
#include <math.h>
|
||
#include <stdlib.h>
|
||
|
||
#ifndef M_PI
|
||
#define M_PI 3.14159265358979323846 /* pi */
|
||
#endif
|
||
|
||
#define ALLPASS_ORDER 20
|
||
|
||
struct SpeexDecorrState_ {
|
||
int rate;
|
||
int channels;
|
||
int frame_size;
|
||
#ifdef VORBIS_PSYCHO
|
||
VorbisPsy *psy;
|
||
struct drft_lookup lookup;
|
||
float *wola_mem;
|
||
float *curve;
|
||
#endif
|
||
float *vorbis_win;
|
||
int seed;
|
||
float *y;
|
||
|
||
/* Per-channel stuff */
|
||
float *buff;
|
||
float (*ring)[ALLPASS_ORDER];
|
||
int *ringID;
|
||
int *order;
|
||
float *alpha;
|
||
};
|
||
|
||
|
||
|
||
EXPORT SpeexDecorrState *speex_decorrelate_new(int rate, int channels, int frame_size)
|
||
{
|
||
int i, ch;
|
||
SpeexDecorrState *st = speex_alloc(sizeof(SpeexDecorrState));
|
||
st->rate = rate;
|
||
st->channels = channels;
|
||
st->frame_size = frame_size;
|
||
#ifdef VORBIS_PSYCHO
|
||
st->psy = vorbis_psy_init(rate, 2*frame_size);
|
||
spx_drft_init(&st->lookup, 2*frame_size);
|
||
st->wola_mem = speex_alloc(frame_size*sizeof(float));
|
||
st->curve = speex_alloc(frame_size*sizeof(float));
|
||
#endif
|
||
st->y = speex_alloc(frame_size*sizeof(float));
|
||
|
||
st->buff = speex_alloc(channels*2*frame_size*sizeof(float));
|
||
st->ringID = speex_alloc(channels*sizeof(int));
|
||
st->order = speex_alloc(channels*sizeof(int));
|
||
st->alpha = speex_alloc(channels*sizeof(float));
|
||
st->ring = speex_alloc(channels*ALLPASS_ORDER*sizeof(float));
|
||
|
||
/*FIXME: The +20 is there only as a kludge for ALL_PASS_OLA*/
|
||
st->vorbis_win = speex_alloc((2*frame_size+20)*sizeof(float));
|
||
for (i=0;i<2*frame_size;i++)
|
||
st->vorbis_win[i] = sin(.5*M_PI* sin(M_PI*i/(2*frame_size))*sin(M_PI*i/(2*frame_size)) );
|
||
st->seed = rand();
|
||
|
||
for (ch=0;ch<channels;ch++)
|
||
{
|
||
for (i=0;i<ALLPASS_ORDER;i++)
|
||
st->ring[ch][i] = 0;
|
||
st->ringID[ch] = 0;
|
||
st->alpha[ch] = 0;
|
||
st->order[ch] = 10;
|
||
}
|
||
return st;
|
||
}
|
||
|
||
static float uni_rand(int *seed)
|
||
{
|
||
const unsigned int jflone = 0x3f800000;
|
||
const unsigned int jflmsk = 0x007fffff;
|
||
union {int i; float f;} ran;
|
||
*seed = 1664525 * *seed + 1013904223;
|
||
ran.i = jflone | (jflmsk & *seed);
|
||
ran.f -= 1.5;
|
||
return 2*ran.f;
|
||
}
|
||
|
||
static unsigned int irand(int *seed)
|
||
{
|
||
*seed = 1664525 * *seed + 1013904223;
|
||
return ((unsigned int)*seed)>>16;
|
||
}
|
||
|
||
|
||
EXPORT void speex_decorrelate(SpeexDecorrState *st, const spx_int16_t *in, spx_int16_t *out, int strength)
|
||
{
|
||
int ch;
|
||
float amount;
|
||
|
||
if (strength<0)
|
||
strength = 0;
|
||
if (strength>100)
|
||
strength = 100;
|
||
|
||
amount = .01*strength;
|
||
for (ch=0;ch<st->channels;ch++)
|
||
{
|
||
int i;
|
||
int N=2*st->frame_size;
|
||
float beta, beta2;
|
||
float *x;
|
||
float max_alpha = 0;
|
||
|
||
float *buff;
|
||
float *ring;
|
||
int ringID;
|
||
int order;
|
||
float alpha;
|
||
|
||
buff = st->buff+ch*2*st->frame_size;
|
||
ring = st->ring[ch];
|
||
ringID = st->ringID[ch];
|
||
order = st->order[ch];
|
||
alpha = st->alpha[ch];
|
||
|
||
for (i=0;i<st->frame_size;i++)
|
||
buff[i] = buff[i+st->frame_size];
|
||
for (i=0;i<st->frame_size;i++)
|
||
buff[i+st->frame_size] = in[i*st->channels+ch];
|
||
|
||
x = buff+st->frame_size;
|
||
|
||
if (amount>1)
|
||
beta = 1-sqrt(.4*amount);
|
||
else
|
||
beta = 1-0.63246*amount;
|
||
if (beta<0)
|
||
beta = 0;
|
||
|
||
beta2 = beta;
|
||
for (i=0;i<st->frame_size;i++)
|
||
{
|
||
st->y[i] = alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[st->frame_size+i+order]
|
||
+ x[i-ALLPASS_ORDER]*st->vorbis_win[st->frame_size+i]
|
||
- alpha*(ring[ringID]
|
||
- beta*ring[ringID+1>=order?0:ringID+1]);
|
||
ring[ringID++]=st->y[i];
|
||
st->y[i] *= st->vorbis_win[st->frame_size+i];
|
||
if (ringID>=order)
|
||
ringID=0;
|
||
}
|
||
order = order+(irand(&st->seed)%3)-1;
|
||
if (order < 5)
|
||
order = 5;
|
||
if (order > 10)
|
||
order = 10;
|
||
/*order = 5+(irand(&st->seed)%6);*/
|
||
max_alpha = pow(.96+.04*(amount-1),order);
|
||
if (max_alpha > .98/(1.+beta2))
|
||
max_alpha = .98/(1.+beta2);
|
||
|
||
alpha = alpha + .4*uni_rand(&st->seed);
|
||
if (alpha > max_alpha)
|
||
alpha = max_alpha;
|
||
if (alpha < -max_alpha)
|
||
alpha = -max_alpha;
|
||
for (i=0;i<ALLPASS_ORDER;i++)
|
||
ring[i] = 0;
|
||
ringID = 0;
|
||
for (i=0;i<st->frame_size;i++)
|
||
{
|
||
float tmp = alpha*(x[i-ALLPASS_ORDER+order]-beta*x[i-ALLPASS_ORDER+order-1])*st->vorbis_win[i+order]
|
||
+ x[i-ALLPASS_ORDER]*st->vorbis_win[i]
|
||
- alpha*(ring[ringID]
|
||
- beta*ring[ringID+1>=order?0:ringID+1]);
|
||
ring[ringID++]=tmp;
|
||
tmp *= st->vorbis_win[i];
|
||
if (ringID>=order)
|
||
ringID=0;
|
||
st->y[i] += tmp;
|
||
}
|
||
|
||
#ifdef VORBIS_PSYCHO
|
||
float frame[N];
|
||
float scale = 1./N;
|
||
for (i=0;i<2*st->frame_size;i++)
|
||
frame[i] = buff[i];
|
||
//float coef = .5*0.78130;
|
||
float coef = M_PI*0.075063 * 0.93763 * amount * .8 * 0.707;
|
||
compute_curve(st->psy, buff, st->curve);
|
||
for (i=1;i<st->frame_size;i++)
|
||
{
|
||
float x1,x2;
|
||
float gain;
|
||
do {
|
||
x1 = uni_rand(&st->seed);
|
||
x2 = uni_rand(&st->seed);
|
||
} while (x1*x1+x2*x2 > 1.);
|
||
gain = coef*sqrt(.1+st->curve[i]);
|
||
frame[2*i-1] = gain*x1;
|
||
frame[2*i] = gain*x2;
|
||
}
|
||
frame[0] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[0]);
|
||
frame[2*st->frame_size-1] = coef*uni_rand(&st->seed)*sqrt(.1+st->curve[st->frame_size-1]);
|
||
spx_drft_backward(&st->lookup,frame);
|
||
for (i=0;i<2*st->frame_size;i++)
|
||
frame[i] *= st->vorbis_win[i];
|
||
#endif
|
||
|
||
for (i=0;i<st->frame_size;i++)
|
||
{
|
||
#ifdef VORBIS_PSYCHO
|
||
float tmp = st->y[i] + frame[i] + st->wola_mem[i];
|
||
st->wola_mem[i] = frame[i+st->frame_size];
|
||
#else
|
||
float tmp = st->y[i];
|
||
#endif
|
||
if (tmp>32767)
|
||
tmp = 32767;
|
||
if (tmp < -32767)
|
||
tmp = -32767;
|
||
out[i*st->channels+ch] = tmp;
|
||
}
|
||
|
||
st->ringID[ch] = ringID;
|
||
st->order[ch] = order;
|
||
st->alpha[ch] = alpha;
|
||
|
||
}
|
||
}
|
||
|
||
EXPORT void speex_decorrelate_destroy(SpeexDecorrState *st)
|
||
{
|
||
#ifdef VORBIS_PSYCHO
|
||
vorbis_psy_destroy(st->psy);
|
||
speex_free(st->wola_mem);
|
||
speex_free(st->curve);
|
||
#endif
|
||
speex_free(st->buff);
|
||
speex_free(st->ring);
|
||
speex_free(st->ringID);
|
||
speex_free(st->alpha);
|
||
speex_free(st->vorbis_win);
|
||
speex_free(st->order);
|
||
speex_free(st->y);
|
||
speex_free(st);
|
||
}
|