diff options
Diffstat (limited to 'codecs/ilbc/enhancer.c')
| -rwxr-xr-x | codecs/ilbc/enhancer.c | 1265 |
1 files changed, 664 insertions, 601 deletions
diff --git a/codecs/ilbc/enhancer.c b/codecs/ilbc/enhancer.c index 86c4a85c0..3e381ec31 100755 --- a/codecs/ilbc/enhancer.c +++ b/codecs/ilbc/enhancer.c @@ -1,601 +1,664 @@ - -/****************************************************************** - - 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); -} - - + +/****************************************************************** + + iLBC Speech Coder ANSI-C Source Code + + enhancer.c + + Copyright (C) The Internet Society (2004). + All Rights Reserved. + +******************************************************************/ + +#include <math.h> +#include <string.h> +#include "iLBC_define.h" +#include "constants.h" +#include "filter.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 + *---------------------------------------------------------------*/ + +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 + *---------------------------------------------------------------*/ + +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 + *---------------------------------------------------------------*/ + + + +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 + *---------------------------------------------------------------*/ + +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]; + + /* 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 + *---------------------------------------------------------------*/ + +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_MAX]; /* 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 + *---------------------------------------------------------------*/ + +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 + *---------------------------------------------------------------*/ + +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=0, ilag, i, ioffset; + float cc, maxcc; + float ftmp1, ftmp2; + float *inPtr, *enh_bufPtr1, *enh_bufPtr2; + float plc_pred[ENH_BLOCKL]; + + float lpState[6], downsampled[(ENH_NBLOCKS*ENH_BLOCKL+120)/2]; + int inLen=ENH_NBLOCKS*ENH_BLOCKL+120; + int start, plc_blockl, inlag; + + enh_buf=iLBCdec_inst->enh_buf; + enh_period=iLBCdec_inst->enh_period; + + memmove(enh_buf, &enh_buf[iLBCdec_inst->blockl], + (ENH_BUFL-iLBCdec_inst->blockl)*sizeof(float)); + + memcpy(&enh_buf[ENH_BUFL-iLBCdec_inst->blockl], in, + iLBCdec_inst->blockl*sizeof(float)); + + if (iLBCdec_inst->mode==30) + plc_blockl=ENH_BLOCKL; + else + plc_blockl=40; + + /* when 20 ms frame, move processing one block */ + ioffset=0; + if (iLBCdec_inst->mode==20) ioffset=1; + + i=3-ioffset; + memmove(enh_period, &enh_period[i], + + + (ENH_NBLOCKS_TOT-i)*sizeof(float)); + + /* Set state information to the 6 samples right before + the samples to be downsampled. */ + + memcpy(lpState, + enh_buf+(ENH_NBLOCKS_EXTRA+ioffset)*ENH_BLOCKL-126, + 6*sizeof(float)); + + /* Down sample a factor 2 to save computations */ + + DownSample(enh_buf+(ENH_NBLOCKS_EXTRA+ioffset)*ENH_BLOCKL-120, + lpFilt_coefsTbl, inLen-ioffset*ENH_BLOCKL, + lpState, downsampled); + + /* Estimate the pitch in the down sampled domain. */ + for (iblock = 0; iblock<ENH_NBLOCKS-ioffset; 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+ioffset] = (float)lag*2; + + + } + + + /* PLC was performed on the previous packet */ + if (iLBCdec_inst->prev_enh_pl==1) { + + inlag=(int)enh_period[ENH_NBLOCKS_EXTRA+ioffset]; + + lag = inlag-1; + maxcc = xCorrCoef(in, in+lag, plc_blockl); + for (ilag=inlag; ilag<=inlag+1; ilag++) { + cc = xCorrCoef(in, in+ilag, plc_blockl); + + if (cc > maxcc) { + maxcc = cc; + lag = ilag; + } + } + + + + enh_period[ENH_NBLOCKS_EXTRA+ioffset-1]=(float)lag; + + /* compute new concealed residual for the old lookahead, + mix the forward PLC with a backward PLC from + the new frame */ + + inPtr=&in[lag-1]; + + enh_bufPtr1=&plc_pred[plc_blockl-1]; + + if (lag>plc_blockl) { + start=plc_blockl; + } else { + start=lag; + } + + for (isample = start; isample>0; isample--) { + *enh_bufPtr1-- = *inPtr--; + } + + enh_bufPtr2=&enh_buf[ENH_BUFL-1-iLBCdec_inst->blockl]; + for (isample = (plc_blockl-1-lag); isample>=0; isample--) +{ + *enh_bufPtr1-- = *enh_bufPtr2--; + } + + /* limit energy change */ + ftmp2=0.0; + ftmp1=0.0; + for (i=0;i<plc_blockl;i++) { + ftmp2+=enh_buf[ENH_BUFL-1-iLBCdec_inst->blockl-i]* + enh_buf[ENH_BUFL-1-iLBCdec_inst->blockl-i]; + ftmp1+=plc_pred[i]*plc_pred[i]; + } + ftmp1=(float)sqrt(ftmp1/(float)plc_blockl); + ftmp2=(float)sqrt(ftmp2/(float)plc_blockl); + if (ftmp1>(float)2.0*ftmp2 && ftmp1>0.0) { + for (i=0;i<plc_blockl-10;i++) { + plc_pred[i]*=(float)2.0*ftmp2/ftmp1; + } + for (i=plc_blockl-10;i<plc_blockl;i++) { + plc_pred[i]*=(float)(i-plc_blockl+10)* + ((float)1.0-(float)2.0*ftmp2/ftmp1)/(float)(10)+ + (float)2.0*ftmp2/ftmp1; + } + } + + enh_bufPtr1=&enh_buf[ENH_BUFL-1-iLBCdec_inst->blockl]; + for (i=0; i<plc_blockl; i++) { + ftmp1 = (float) (i+1) / (float) (plc_blockl+1); + *enh_bufPtr1 *= ftmp1; + *enh_bufPtr1 += ((float)1.0-ftmp1)* + plc_pred[plc_blockl-1-i]; + enh_bufPtr1--; + } + + + } + + if (iLBCdec_inst->mode==20) { + /* Enhancer with 40 samples delay */ + for (iblock = 0; iblock<2; iblock++) { + enhancer(out+iblock*ENH_BLOCKL, enh_buf, + ENH_BUFL, (5+iblock)*ENH_BLOCKL+40, + ENH_ALPHA0, enh_period, enh_plocsTbl, + ENH_NBLOCKS_TOT); + } + } else if (iLBCdec_inst->mode==30) { + /* Enhancer with 80 samples delay */ + for (iblock = 0; iblock<3; 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); +} + + |