enhancer.c

Ilibc 语音编解码库算法。语音质量高。接口简单。

   
   /******************************************************************
   
       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;