37b7bccc61
#72 - Should eliminate probs on VMWI #41 - Dynamic add survives reload #73 - Make festival honor its arguments #93 - Make events on FXO interfaces more logical #26 - Prefer "bindaddr" to logical address for registrations ??? - Record crashes AGI git-svn-id: https://origsvn.digium.com/svn/asterisk/trunk@1338 65c4cc65-6c06-0410-ace0-fbb531ad65f3
602 lines
18 KiB
C
Executable file
602 lines
18 KiB
C
Executable file
|
|
/******************************************************************
|
|
|
|
iLBC Speech Coder ANSI-C Source Code
|
|
|
|
enhancer.c
|
|
|
|
Copyright (c) 2001,
|
|
Global IP Sound AB.
|
|
All rights reserved.
|
|
|
|
******************************************************************/
|
|
|
|
#include <math.h>
|
|
#include <string.h>
|
|
#include "iLBC_define.h"
|
|
#include "constants.h"
|
|
#include "filter.h"
|
|
#include "enhancer.h"
|
|
|
|
/*----------------------------------------------------------------*
|
|
* Find index in array such that the array element with said
|
|
* index is the element of said array closest to "value"
|
|
* according to the squared-error criterion
|
|
*---------------------------------------------------------------*/
|
|
|
|
static void NearestNeighbor(
|
|
int *index, /* (o) index of array element closest to value */
|
|
float *array, /* (i) data array */
|
|
float value,/* (i) value */
|
|
int arlength/* (i) dimension of data array */
|
|
){
|
|
int i;
|
|
float bestcrit,crit;
|
|
|
|
crit=array[0]-value;
|
|
bestcrit=crit*crit;
|
|
*index=0;
|
|
for(i=1;i<arlength;i++){
|
|
crit=array[i]-value;
|
|
crit=crit*crit;
|
|
|
|
if(crit<bestcrit){
|
|
bestcrit=crit;
|
|
*index=i;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*----------------------------------------------------------------*
|
|
* compute cross correlation between sequences
|
|
*---------------------------------------------------------------*/
|
|
|
|
static void mycorr1(
|
|
float* corr, /* (o) correlation of seq1 and seq2 */
|
|
float* seq1, /* (i) first sequence */
|
|
int dim1, /* (i) dimension first seq1 */
|
|
const float *seq2, /* (i) second sequence */
|
|
int dim2 /* (i) dimension seq2 */
|
|
){
|
|
int i,j;
|
|
|
|
for(i=0;i<=dim1-dim2; i++){
|
|
corr[i]=0.0;
|
|
for(j=0;j<dim2; j++){
|
|
corr[i] += seq1[i+j] * seq2[j];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*----------------------------------------------------------------*
|
|
* upsample finite array assuming zeros outside bounds
|
|
*---------------------------------------------------------------*/
|
|
|
|
static void enh_upsample(
|
|
float* useq1, /* (o) upsampled output sequence */
|
|
float* seq1,/* (i) unupsampled sequence */
|
|
int dim1, /* (i) dimension seq1 */
|
|
int hfl /* (i) polyphase filter length=2*hfl+1 */
|
|
){
|
|
float *pu,*ps;
|
|
int i,j,k,q,filterlength,hfl2;
|
|
const float *polyp[ENH_UPS0]; /* pointers to polyphase columns */
|
|
const float *pp;
|
|
|
|
/* define pointers for filter */
|
|
|
|
filterlength=2*hfl+1;
|
|
|
|
if( filterlength > dim1){
|
|
hfl2=(int) (dim1/2);
|
|
for(j=0;j<ENH_UPS0; j++) {
|
|
polyp[j]=polyphaserTbl+j*filterlength+hfl-hfl2;
|
|
}
|
|
hfl=hfl2;
|
|
filterlength=2*hfl+1;
|
|
}
|
|
else {
|
|
for(j=0;j<ENH_UPS0; j++) {
|
|
polyp[j]=polyphaserTbl+j*filterlength;
|
|
}
|
|
}
|
|
|
|
/* filtering: filter overhangs left side of sequence */
|
|
|
|
pu=useq1;
|
|
for(i=hfl;i<filterlength; i++){
|
|
for(j=0;j<ENH_UPS0; j++){
|
|
*pu=0.0;
|
|
pp = polyp[j];
|
|
ps = seq1+i;
|
|
for(k=0;k<=i;k++) {
|
|
*pu += *ps-- * *pp++;
|
|
}
|
|
pu++;
|
|
}
|
|
}
|
|
|
|
/* filtering: simple convolution=inner products */
|
|
|
|
for(i=filterlength; i<dim1; i++){
|
|
for(j=0;j<ENH_UPS0; j++){
|
|
*pu=0.0;
|
|
pp = polyp[j];
|
|
ps = seq1+i;
|
|
for(k=0;k<filterlength;k++) {
|
|
*pu += *ps-- * *pp++;
|
|
}
|
|
pu++;
|
|
}
|
|
}
|
|
|
|
/* filtering: filter overhangs right side of sequence */
|
|
|
|
for(q=1; q<=hfl;q++){
|
|
for(j=0;j<ENH_UPS0; j++){
|
|
*pu=0.0;
|
|
pp = polyp[j]+q;
|
|
ps = seq1+dim1-1;
|
|
for(k=0;k<filterlength-q;k++) {
|
|
*pu += *ps-- * *pp++;
|
|
}
|
|
pu++;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
/*----------------------------------------------------------------*
|
|
* find segment starting near idata+estSegPos that has highest
|
|
* correlation with idata+centerStartPos through
|
|
* idata+centerStartPos+ENH_BLOCKL-1 segment is found at a
|
|
* resolution of ENH_UPSO times the original of the original
|
|
* sampling rate
|
|
*---------------------------------------------------------------*/
|
|
|
|
static void refiner(
|
|
float *seg, /* (o) segment array */
|
|
float *updStartPos, /* (o) updated start point */
|
|
float* idata, /* (i) original data buffer */
|
|
int idatal, /* (i) dimension of idata */
|
|
int centerStartPos, /* (i) beginning center segment */
|
|
float estSegPos,/* (i) estimated beginning other segment */
|
|
float period /* (i) estimated pitch period */
|
|
){
|
|
int estSegPosRounded,searchSegStartPos,searchSegEndPos,corrdim;
|
|
int tloc,tloc2,i,st,en,fraction;
|
|
float vect[ENH_VECTL],corrVec[ENH_CORRDIM],maxv;
|
|
float corrVecUps[ENH_CORRDIM*ENH_UPS0];
|
|
(void)period;
|
|
|
|
/* defining array bounds */
|
|
|
|
estSegPosRounded=(int)(estSegPos - 0.5);
|
|
|
|
searchSegStartPos=estSegPosRounded-ENH_SLOP;
|
|
|
|
if (searchSegStartPos<0) {
|
|
searchSegStartPos=0;
|
|
}
|
|
searchSegEndPos=estSegPosRounded+ENH_SLOP;
|
|
|
|
if(searchSegEndPos+ENH_BLOCKL >= idatal) {
|
|
searchSegEndPos=idatal-ENH_BLOCKL-1;
|
|
}
|
|
corrdim=searchSegEndPos-searchSegStartPos+1;
|
|
|
|
/* compute upsampled correlation (corr33) and find
|
|
location of max */
|
|
|
|
mycorr1(corrVec,idata+searchSegStartPos,
|
|
corrdim+ENH_BLOCKL-1,idata+centerStartPos,ENH_BLOCKL);
|
|
enh_upsample(corrVecUps,corrVec,corrdim,ENH_FL0);
|
|
tloc=0; maxv=corrVecUps[0];
|
|
for(i=1;i<ENH_UPS0*corrdim; i++){
|
|
|
|
if(corrVecUps[i]>maxv){
|
|
tloc=i;
|
|
maxv=corrVecUps[i];
|
|
}
|
|
}
|
|
|
|
/* make vector can be upsampled without ever running outside
|
|
bounds */
|
|
|
|
*updStartPos= (float)searchSegStartPos +
|
|
(float)tloc/(float)ENH_UPS0+(float)1.0;
|
|
tloc2=(int)(tloc/ENH_UPS0);
|
|
|
|
if (tloc>tloc2*ENH_UPS0) {
|
|
tloc2++;
|
|
}
|
|
st=searchSegStartPos+tloc2-ENH_FL0;
|
|
|
|
if(st<0){
|
|
memset(vect,0,-st*sizeof(float));
|
|
memcpy(&vect[-st],idata, (ENH_VECTL+st)*sizeof(float));
|
|
}
|
|
else{
|
|
en=st+ENH_VECTL;
|
|
|
|
if(en>idatal){
|
|
memcpy(vect, &idata[st],
|
|
(ENH_VECTL-(en-idatal))*sizeof(float));
|
|
memset(&vect[ENH_VECTL-(en-idatal)], 0,
|
|
(en-idatal)*sizeof(float));
|
|
}
|
|
else {
|
|
memcpy(vect, &idata[st], ENH_VECTL*sizeof(float));
|
|
}
|
|
}
|
|
fraction=tloc2*ENH_UPS0-tloc;
|
|
|
|
/* compute the segment (this is actually a convolution) */
|
|
|
|
mycorr1(seg,vect,ENH_VECTL,polyphaserTbl+(2*ENH_FL0+1)*fraction,
|
|
2*ENH_FL0+1);
|
|
}
|
|
|
|
/*----------------------------------------------------------------*
|
|
* find the smoothed output data
|
|
*---------------------------------------------------------------*/
|
|
|
|
static void smath(
|
|
float *odata, /* (o) smoothed output */
|
|
float *sseq,/* (i) said second sequence of waveforms */
|
|
int hl, /* (i) 2*hl+1 is sseq dimension */
|
|
float alpha0/* (i) max smoothing energy fraction */
|
|
){
|
|
int i,k;
|
|
float w00,w10,w11,A,B,C,*psseq,err,errs;
|
|
float surround[BLOCKL]; /* shape contributed by other than
|
|
current */
|
|
float wt[2*ENH_HL+1]; /* waveform weighting to get surround
|
|
shape */
|
|
float denom;
|
|
|
|
/* create shape of contribution from all waveforms except the
|
|
current one */
|
|
|
|
for(i=1;i<=2*hl+1; i++) {
|
|
wt[i-1] = (float)0.5*(1 - (float)cos(2*PI*i/(2*hl+2)));
|
|
}
|
|
wt[hl]=0.0; /* for clarity, not used */
|
|
for(i=0;i<ENH_BLOCKL; i++) {
|
|
surround[i]=sseq[i]*wt[0];
|
|
}
|
|
for(k=1;k<hl; k++){
|
|
psseq=sseq+k*ENH_BLOCKL;
|
|
for(i=0;i<ENH_BLOCKL; i++) {
|
|
surround[i]+=psseq[i]*wt[k];
|
|
}
|
|
}
|
|
for(k=hl+1;k<=2*hl; k++){
|
|
psseq=sseq+k*ENH_BLOCKL;
|
|
for(i=0;i<ENH_BLOCKL; i++) {
|
|
surround[i]+=psseq[i]*wt[k];
|
|
}
|
|
}
|
|
|
|
/* compute some inner products */
|
|
|
|
w00 = w10 = w11 = 0.0;
|
|
psseq=sseq+hl*ENH_BLOCKL; /* current block */
|
|
for(i=0;i<ENH_BLOCKL;i++) {
|
|
w00+=psseq[i]*psseq[i];
|
|
w11+=surround[i]*surround[i];
|
|
w10+=surround[i]*psseq[i];
|
|
}
|
|
|
|
if( fabs(w11) < 1.0) {
|
|
w11=1.0;
|
|
}
|
|
C = (float)sqrt( w00/w11);
|
|
|
|
/* first try enhancement without power-constraint */
|
|
|
|
errs=0.0;
|
|
psseq=sseq+hl*ENH_BLOCKL;
|
|
for(i=0;i<ENH_BLOCKL;i++) {
|
|
odata[i]=C*surround[i];
|
|
err=psseq[i]-odata[i];
|
|
errs+=err*err;
|
|
}
|
|
|
|
/* if constraint violated by first try, add constraint */
|
|
|
|
if( errs > alpha0 * w00){
|
|
if( w00 < 1) {
|
|
w00=1;
|
|
}
|
|
denom = (w11*w00-w10*w10)/(w00*w00);
|
|
|
|
if( denom > 0.0001){ /* eliminates numerical problems
|
|
for if smooth */
|
|
A = (float)sqrt( (alpha0- alpha0*alpha0/4)/denom);
|
|
B = -alpha0/2 - A * w10/w00;
|
|
B = B+1;
|
|
}
|
|
else{ /* essentially no difference between cycles;
|
|
smoothing not needed */
|
|
A= 0.0;
|
|
B= 1.0;
|
|
}
|
|
|
|
/* create smoothed sequence */
|
|
|
|
psseq=sseq+hl*ENH_BLOCKL;
|
|
for(i=0;i<ENH_BLOCKL;i++) {
|
|
odata[i]=A*surround[i]+B*psseq[i];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*----------------------------------------------------------------*
|
|
* get the pitch-synchronous sample sequence
|
|
*---------------------------------------------------------------*/
|
|
|
|
static void getsseq(
|
|
float *sseq, /* (o) the pitch-synchronous sequence */
|
|
float *idata, /* (i) original data */
|
|
int idatal, /* (i) dimension of data */
|
|
int centerStartPos, /* (i) where current block starts */
|
|
float *period, /* (i) rough-pitch-period array */
|
|
float *plocs, /* (i) where periods of period array
|
|
are taken */
|
|
int periodl, /* (i) dimension period array */
|
|
int hl /* (i)( 2*hl+1 is the number of sequences */
|
|
){
|
|
int i,centerEndPos,q;
|
|
float blockStartPos[2*ENH_HL+1];
|
|
int lagBlock[2*ENH_HL+1];
|
|
float plocs2[ENH_PLOCSL];
|
|
float *psseq;
|
|
|
|
centerEndPos=centerStartPos+ENH_BLOCKL-1;
|
|
|
|
/* present */
|
|
|
|
NearestNeighbor(lagBlock+hl,plocs,
|
|
(float)0.5*(centerStartPos+centerEndPos),periodl);
|
|
|
|
blockStartPos[hl]=(float)centerStartPos;
|
|
psseq=sseq+ENH_BLOCKL*hl;
|
|
memcpy(psseq, idata+centerStartPos, ENH_BLOCKL*sizeof(float));
|
|
|
|
/* past */
|
|
|
|
for(q=hl-1;q>=0;q--) {
|
|
blockStartPos[q]=blockStartPos[q+1]-period[lagBlock[q+1]];
|
|
NearestNeighbor(lagBlock+q,plocs,
|
|
blockStartPos[q]+ENH_BLOCKL_HALF-period[lagBlock[q+1]],
|
|
periodl);
|
|
|
|
|
|
if(blockStartPos[q]-ENH_OVERHANG>=0) {
|
|
refiner(sseq+q*ENH_BLOCKL,blockStartPos+q,idata,idatal,
|
|
centerStartPos,blockStartPos[q],
|
|
period[lagBlock[q+1]]);
|
|
} else {
|
|
|
|
psseq=sseq+q*ENH_BLOCKL;
|
|
memset(psseq, 0, ENH_BLOCKL*sizeof(float));
|
|
}
|
|
}
|
|
|
|
/* future */
|
|
|
|
for(i=0;i<periodl;i++) {
|
|
plocs2[i]=plocs[i]-period[i];
|
|
}
|
|
for(q=hl+1;q<=2*hl;q++) {
|
|
NearestNeighbor(lagBlock+q,plocs2,
|
|
blockStartPos[q-1]+ENH_BLOCKL_HALF,periodl);
|
|
|
|
blockStartPos[q]=blockStartPos[q-1]+period[lagBlock[q]];
|
|
if( blockStartPos[q]+ENH_BLOCKL+ENH_OVERHANG<idatal) {
|
|
|
|
refiner(sseq+ENH_BLOCKL*q,blockStartPos+q,idata,idatal,
|
|
centerStartPos,blockStartPos[q],period[lagBlock[q]]);
|
|
|
|
}
|
|
else {
|
|
psseq=sseq+q*ENH_BLOCKL;
|
|
memset(psseq, 0, ENH_BLOCKL*sizeof(float));
|
|
}
|
|
}
|
|
}
|
|
|
|
/*----------------------------------------------------------------*
|
|
* perform enhancement on idata+centerStartPos through
|
|
* idata+centerStartPos+ENH_BLOCKL-1
|
|
*---------------------------------------------------------------*/
|
|
|
|
static void enhancer(
|
|
float *odata, /* (o) smoothed block, dimension blockl */
|
|
float *idata, /* (i) data buffer used for enhancing */
|
|
int idatal, /* (i) dimension idata */
|
|
int centerStartPos, /* (i) first sample current block
|
|
within idata */
|
|
float alpha0, /* (i) max correction-energy-fraction
|
|
(in [0,1]) */
|
|
float *period, /* (i) pitch period array */
|
|
float *plocs, /* (i) locations where period array
|
|
values valid */
|
|
int periodl /* (i) dimension of period and plocs */
|
|
){
|
|
float sseq[(2*ENH_HL+1)*ENH_BLOCKL];
|
|
|
|
/* get said second sequence of segments */
|
|
|
|
getsseq(sseq,idata,idatal,centerStartPos,period,
|
|
plocs,periodl,ENH_HL);
|
|
|
|
/* compute the smoothed output from said second sequence */
|
|
|
|
smath(odata,sseq,ENH_HL,alpha0);
|
|
|
|
}
|
|
|
|
/*----------------------------------------------------------------*
|
|
* cross correlation
|
|
*---------------------------------------------------------------*/
|
|
|
|
float xCorrCoef(
|
|
float *target, /* (i) first array */
|
|
float *regressor, /* (i) second array */
|
|
int subl /* (i) dimension arrays */
|
|
){
|
|
int i;
|
|
float ftmp1, ftmp2;
|
|
|
|
ftmp1 = 0.0;
|
|
ftmp2 = 0.0;
|
|
for (i=0; i<subl; i++) {
|
|
ftmp1 += target[i]*regressor[i];
|
|
ftmp2 += regressor[i]*regressor[i];
|
|
}
|
|
|
|
if (ftmp1 > 0.0) {
|
|
return (float)(ftmp1*ftmp1/ftmp2);
|
|
}
|
|
else {
|
|
return (float)0.0;
|
|
}
|
|
}
|
|
|
|
/*----------------------------------------------------------------*
|
|
* interface for enhancer
|
|
*---------------------------------------------------------------*/
|
|
|
|
int enhancerInterface(
|
|
float *out, /* (o) enhanced signal */
|
|
float *in, /* (i) unenhanced signal */
|
|
iLBC_Dec_Inst_t *iLBCdec_inst /* (i) buffers etc */
|
|
){
|
|
float *enh_buf, *enh_period;
|
|
int iblock, isample;
|
|
int lag, ilag, i;
|
|
float cc, maxcc;
|
|
float ftmp1, ftmp2, gain;
|
|
float *inPtr, *enh_bufPtr1, *enh_bufPtr2;
|
|
|
|
float lpState[6], downsampled[(ENH_NBLOCKS*ENH_BLOCKL+120)/2];
|
|
int inLen=ENH_NBLOCKS*ENH_BLOCKL+120;
|
|
int start;
|
|
|
|
enh_buf=iLBCdec_inst->enh_buf;
|
|
enh_period=iLBCdec_inst->enh_period;
|
|
|
|
|
|
memmove(enh_buf, &enh_buf[ENH_NBLOCKS*ENH_BLOCKL],
|
|
(ENH_NBLOCKS_EXTRA*ENH_BLOCKL)*sizeof(float));
|
|
|
|
memcpy(&enh_buf[ENH_NBLOCKS_EXTRA*ENH_BLOCKL], in,
|
|
(ENH_NBLOCKS*ENH_BLOCKL)*sizeof(float));
|
|
|
|
if (iLBCdec_inst->prev_enh_pl==1) {
|
|
/* PLC was performed on the previous packet */
|
|
|
|
lag = 20;
|
|
maxcc = xCorrCoef(in, in+lag, ENH_BLOCKL);
|
|
for (ilag=21; ilag<120; ilag++) {
|
|
cc = xCorrCoef(in, in+ilag, ENH_BLOCKL);
|
|
|
|
if (cc > maxcc) {
|
|
maxcc = cc;
|
|
lag = ilag;
|
|
}
|
|
}
|
|
|
|
ftmp1 = 0.0;
|
|
ftmp2 = 0.0;
|
|
for (i=0; i<ENH_BLOCKL; i++) {
|
|
ftmp1 += in[i]*in[i+lag];
|
|
ftmp2 += in[i+lag]*in[i+lag];
|
|
}
|
|
|
|
if (ftmp1 > 0.0) {
|
|
gain=(float)(ftmp1/ftmp2);
|
|
}
|
|
else {
|
|
gain=(float)0.0;
|
|
}
|
|
if (gain>1.0) {
|
|
gain=1.0;
|
|
} else if (gain<-1.0) {
|
|
gain=-1.0;
|
|
}
|
|
|
|
inPtr=&in[lag-1];
|
|
|
|
enh_bufPtr1=&enh_buf[ENH_NBLOCKS_EXTRA*ENH_BLOCKL-1];
|
|
|
|
if (lag>ENH_BLOCKL) {
|
|
start=ENH_BLOCKL;
|
|
} else {
|
|
start=lag;
|
|
}
|
|
|
|
for (isample = start; isample>0; isample--) {
|
|
*enh_bufPtr1-- = gain*(*inPtr--);
|
|
}
|
|
|
|
enh_bufPtr2=&enh_buf[ENH_NBLOCKS_EXTRA*ENH_BLOCKL-1];
|
|
for (isample = (ENH_BLOCKL-1-lag); isample>=0; isample--) {
|
|
*enh_bufPtr1-- = gain*(*enh_bufPtr2--);
|
|
}
|
|
|
|
}
|
|
|
|
memmove(enh_period, &enh_period[ENH_NBLOCKS],
|
|
ENH_NBLOCKS_EXTRA*sizeof(float));
|
|
|
|
|
|
/* Set state information to the 6 samples right before
|
|
the samples to be downsampled. */
|
|
|
|
memcpy(lpState, enh_buf+ENH_NBLOCKS_EXTRA*ENH_BLOCKL-126,
|
|
6*sizeof(float));
|
|
|
|
/* Down sample a factor 2 to save computations */
|
|
|
|
DownSample(enh_buf+ENH_NBLOCKS_EXTRA*ENH_BLOCKL-120,
|
|
lpFilt_coefsTbl, inLen,
|
|
lpState, downsampled);
|
|
|
|
/* Estimate the pitch in the down sampled domain. */
|
|
for(iblock = 0; iblock<ENH_NBLOCKS; iblock++){
|
|
|
|
lag = 10;
|
|
maxcc = xCorrCoef(downsampled+60+iblock*
|
|
ENH_BLOCKL_HALF, downsampled+60+iblock*
|
|
ENH_BLOCKL_HALF-lag, ENH_BLOCKL_HALF);
|
|
for (ilag=11; ilag<60; ilag++) {
|
|
cc = xCorrCoef(downsampled+60+iblock*
|
|
ENH_BLOCKL_HALF, downsampled+60+iblock*
|
|
ENH_BLOCKL_HALF-ilag, ENH_BLOCKL_HALF);
|
|
|
|
if (cc > maxcc) {
|
|
maxcc = cc;
|
|
lag = ilag;
|
|
}
|
|
}
|
|
|
|
/* Store the estimated lag in the non-downsampled domain */
|
|
enh_period[iblock+ENH_NBLOCKS_EXTRA] = (float)lag*2;
|
|
}
|
|
|
|
for(iblock = 0; iblock<ENH_NBLOCKS; iblock++){
|
|
|
|
enhancer(out+iblock*ENH_BLOCKL, enh_buf,
|
|
ENH_BUFL, (4+iblock)*ENH_BLOCKL,
|
|
ENH_ALPHA0, enh_period, enh_plocsTbl,
|
|
ENH_NBLOCKS_TOT);
|
|
|
|
}
|
|
return (lag*2);
|
|
}
|
|
|
|
|