enhancer.c

Go to the documentation of this file.

 /******************************************************************

 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);
 }