SB-CELP decoder (continued)
[speexdsp.git] / libspeex / cb_search.c
index b3e73a2..dcdf72a 100644 (file)
 #include "filters.h"
 #include <math.h>
 #include <stdio.h>
+#include "stack_alloc.h"
+#include "vq.h"
+#include "matrix.h"
 
-#define EXC_CB_SIZE 128
-#define min(a,b) ((a) < (b) ? (a) : (b))
-extern float exc_gains_table[128][5];
+static float scal_gains4[16] = {
+   0.27713,
+   0.49282,
+   0.69570,
+   0.90786,
+   1.14235,
+   1.42798,
+   1.80756,
+   2.42801
+};
 
 /*---------------------------------------------------------------------------*\
                                                                              
@@ -84,7 +94,7 @@ int   nsf                       /* number of samples in subframe */
   bscore = 0.0;
   impulse[0] = 1.0;
 
-  /* Calculate impulse response of  A(z/g2) / ( A(z)*(z/g1) ) */
+  /* Calculate impulse response of  A(z/g1) / ( A(z)*(z/g2) ) */
   residue_zero(impulse, awk1, h, nsf, p);
   syn_filt_zero(h, ak, h, nsf, p);
   syn_filt_zero(h, awk2, h, nsf,p);
@@ -129,50 +139,90 @@ int   nsf                       /* number of samples in subframe */
 
 
 
-
-
 void split_cb_search(
 float target[],                        /* target vector */
 float ak[],                    /* LPCs for this subframe */
 float awk1[],                  /* Weighted LPCs for this subframe */
 float awk2[],                  /* Weighted LPCs for this subframe */
-float codebook[][8],           /* overlapping codebook */
-int   entries,                 /* number of entries to search */
-float *gain,                   /* gain of optimum entries */
-int   *index,                  /* index of optimum entries */
+void *par,                      /* Codebook/search parameters*/
 int   p,                        /* number of LPC coeffs */
 int   nsf,                      /* number of samples in subframe */
 float *exc,
-FrameBits *bits
+FrameBits *bits,
+float *stack
 )
 {
    int i,j;
-   float resp[EXC_CB_SIZE][8], E[EXC_CB_SIZE], Ee[EXC_CB_SIZE];
-   float t[40], r[40], e[40];
-   float gains[5];
-   int ind[5];
-   for (i=0;i<40;i++)
+   float *resp, *E, *Ee;
+   float *t, *r, *e, *tresp;
+   float *gains;
+   int *ind;
+   float *shape_cb;
+   int shape_cb_size, subvect_size, nb_subvect;
+   float exc_energy=0;
+   split_cb_params *params;
+
+   params = (split_cb_params *) par;
+   subvect_size = params->subvect_size;
+   nb_subvect = params->nb_subvect;
+   shape_cb_size = 1<<params->shape_bits;
+   shape_cb = params->shape_cb;
+   resp = PUSH(stack, shape_cb_size*subvect_size);
+   tresp = PUSH(stack, shape_cb_size*nsf);
+   E = PUSH(stack, shape_cb_size);
+   Ee = PUSH(stack, shape_cb_size);
+   t = PUSH(stack, nsf);
+   r = PUSH(stack, nsf);
+   e = PUSH(stack, nsf);
+   gains = PUSH(stack, nb_subvect);
+   ind = (int*)PUSH(stack, nb_subvect);
+
+   syn_filt_zero(target, awk1, e, nsf, p);
+   residue_zero(e, ak, e, nsf, p);
+   residue_zero(e, awk2, e, nsf, p);
+   for (i=0;i<nsf;i++)
+      exc_energy += e[i]*e[i];
+   exc_energy=sqrt(.125*exc_energy);
+
+   /* Quantize global (average) gain */
+   {
+      float q;
+      int id;
+      q=log(exc_energy+.1);
+      q=floor(.5+2*(q-2));
+      if (q<0)
+         q=0;
+      if (q>15)
+         q=15;
+      id = (int)q;
+      frame_bits_pack(bits, id, 4);
+      exc_energy=exp(.5*q+2);
+   }
+
+   for (i=0;i<nsf;i++)
       t[i]=target[i];
-   for (i=0;i<EXC_CB_SIZE;i++)
+   for (i=0;i<shape_cb_size;i++)
    {
-      residue_zero(codebook[i], awk1, resp[i], 8, p);
-      syn_filt_zero(resp[i], ak, resp[i], 8, p);
-      syn_filt_zero(resp[i], awk2, resp[i], 8,p);
+      float *res = resp+i*subvect_size;
+      residue_zero(shape_cb+i*subvect_size, awk1, res, subvect_size, p);
+      syn_filt_zero(res, ak, res, subvect_size, p);
+      syn_filt_zero(res, awk2, res, subvect_size,p);
       E[i]=0;
-      for(j=0;j<8;j++)
-         E[i]+=resp[i][j]*resp[i][j];
+      for(j=0;j<subvect_size;j++)
+         E[i]+=res[j]*res[j];
       Ee[i]=0;
-      for(j=0;j<8;j++)
-         Ee[i]+=codebook[i][j]*codebook[i][j];
+      for(j=0;j<subvect_size;j++)
+         Ee[i]+=shape_cb[i*subvect_size+j]*shape_cb[i*subvect_size+j];
       
    }
-   for (i=0;i<5;i++)
+
+   for (i=0;i<nb_subvect;i++)
    {
       int best_index=0;
       float g, corr, best_gain=0, score, best_score=-1;
-      for (j=0;j<EXC_CB_SIZE;j++)
+      for (j=0;j<shape_cb_size;j++)
       {
-         corr=xcorr(resp[j],t+8*i,8);
+         corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
          score=corr*corr/(.001+E[j]);
          g = corr/(.001+E[j]);
          if (score>best_score)
@@ -182,111 +232,133 @@ FrameBits *bits
             best_gain=corr/(.001+E[j]);
          }
       }
-      frame_bits_pack(bits,best_index,8);
-      ind[i]=best_index;
-      gains[i]=best_gain*Ee[ind[i]];
-      if (0) { /* Simulating scalar quantization of the gain*/
-         float sign=1;
-         printf("before: %f\n", best_gain);
+      frame_bits_pack(bits,best_index,params->shape_bits);
+      {
+         int s=0, best_id, j;
+         float best_dist;
+         best_gain /= .01+exc_energy;
          if (best_gain<0)
          {
-            sign=-1;
+            best_gain=-best_gain;
+            s=1;
          }
-         best_gain = fabs(best_gain)+.1;
-         best_gain = log(best_gain);
-         if (best_gain>8)
-            best_gain=8;
-         if (best_gain<0)
-            best_gain=0;
-         /*best_gain=.125*floor(8*best_gain+.5);*/
-         best_gain=.25*floor(4*best_gain+.5);
-         best_gain=sign*exp(best_gain);
-         if (fabs(best_gain)<1.01)
-            best_gain=0;
-         printf("after: %f\n", best_gain);
+         best_dist=(best_gain-scal_gains4[0])*(best_gain-scal_gains4[0]);
+         best_id=0;
+         for (j=1;j<8;j++)
+         {
+            float dist;
+            dist=(best_gain-scal_gains4[j])*(best_gain-scal_gains4[j]);
+            if (dist<best_dist)
+            {
+               best_id=j;
+               best_dist=dist;
+            }
+         }
+         best_gain=scal_gains4[best_id];
+         /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
+         if (s)
+            best_gain=-best_gain;
+         best_gain *= exc_energy;
+         frame_bits_pack(bits,s,1);
+         frame_bits_pack(bits,best_id,3);
       }
+      ind[i]=best_index;
+      gains[i]=best_gain;
 
-      printf ("search: %d %f %f %f\n", best_index, best_gain, best_gain*best_gain*E[best_index], best_score);
-      for (j=0;j<40;j++)
+      for (j=0;j<nsf;j++)
          e[j]=0;
-      for (j=0;j<8;j++)
-         e[8*i+j]=best_gain*codebook[best_index][j];
-      residue_zero(e, awk1, r, 40, p);
-      syn_filt_zero(r, ak, r, 40, p);
-      syn_filt_zero(r, awk2, r, 40,p);
-      for (j=0;j<40;j++)
+      for (j=0;j<subvect_size;j++)
+         e[subvect_size*i+j]=best_gain*shape_cb[best_index*subvect_size+j];
+      residue_zero(e, awk1, r, nsf, p);
+      syn_filt_zero(r, ak, r, nsf, p);
+      syn_filt_zero(r, awk2, r, nsf,p);
+      for (j=0;j<nsf;j++)
+         tresp[i*nsf+j]=r[j];
+      for (j=0;j<nsf;j++)
          t[j]-=r[j];
-
-      /*FIXME: Should move that out of the loop if we are to vector-quantize the gains*/
-      /*for (j=0;j<40;j++)
-        exc[j]+=e[j];*/
    }
+   
+   for (i=0;i<nb_subvect;i++)
+      for (j=0;j<subvect_size;j++)
+         e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
+
+   for (j=0;j<nsf;j++)
+      exc[j]+=e[j];
+   residue_zero(e, awk1, r, nsf, p);
+   syn_filt_zero(r, ak, r, nsf, p);
+   syn_filt_zero(r, awk2, r, nsf,p);
+   for (j=0;j<nsf;j++)
+      target[j]-=r[j];
+
+   
+
+
+   POP(stack);
+   POP(stack);
+   POP(stack);
+   POP(stack);
+   POP(stack);
+   POP(stack);
+   POP(stack);
+   POP(stack);
+}
+
+
 
+
+void split_cb_unquant(
+float *exc,
+void *par,                      /* non-overlapping codebook */
+int   nsf,                      /* number of samples in subframe */
+FrameBits *bits,
+float *stack
+)
+{
+   int i,j;
+   int *ind;
+   float *gains;
+   float *sign;
+   float *shape_cb, exc_energy;
+   int shape_cb_size, subvect_size, nb_subvect;
+   split_cb_params *params;
+
+   params = (split_cb_params *) par;
+   subvect_size = params->subvect_size;
+   nb_subvect = params->nb_subvect;
+   shape_cb_size = 1<<params->shape_bits;
+   shape_cb = params->shape_cb;
+   
+   ind = (int*)PUSH(stack, nb_subvect);
+   gains = PUSH(stack, nb_subvect);
+   sign = PUSH(stack, nb_subvect);
+
+   /* Decode global (average) gain */
    {
-      int best_vq_index=0, max_index;
-      float max_gain=0, log_max, min_dist=0, sign[5];
+      int id;
+      id = frame_bits_unpack_unsigned(bits, 4);
+      exc_energy=exp(.5*id+2);
+   }
 
-      for (i=0;i<5;i++)
-      {
-         if (gains[i]<0)
-         {
-            gains[i]=-gains[i];
-            sign[i]=-1;
-         } else {
-            sign[i]=1;
-         }
-      }
-      for (i=0;i<5;i++)
-         if (gains[i]>max_gain)
-            max_gain=gains[i];
-      log_max=log(max_gain+1);
-      max_index = (int)(floor(.5+log_max-3));
-      if (max_index>7)
-         max_index=7;
-      if (max_index<0)
-         max_index=0;
-      max_gain=1/exp(max_index+3.0);
-      for (i=0;i<5;i++)
-        gains[i]*=max_gain;
-      frame_bits_pack(bits,max_index,3);
-
-      if (max_index>2)
-      {
-      for (i=0;i<5;i++)
-         printf ("%f ", gains[i]);
-      printf ("cbgains: \n");
-      }
-      /*Vector quantize gains[i]*/
-      for (i=0;i<256;i++)
-      {
-         float dist=0;
-         for (j=0;j<5;j++)
-            dist += (exc_gains_table[i][j]-gains[j])*(exc_gains_table[i][j]-gains[j]);
-         if (i==0 || dist<min_dist)
-         {
-            min_dist=dist;
-            best_vq_index=i;
-         }
-      }
-      frame_bits_pack(bits,best_vq_index,8);
-      printf ("best_gains_vq_index %d %f %d\n", best_vq_index, min_dist, max_index);
-#if 1
-      for (i=0;i<5;i++)
-         gains[i]= sign[i]*exc_gains_table[best_vq_index][i]/max_gain/(Ee[ind[i]]+.001);
-#else 
-      for (i=0;i<5;i++)
-         gains[i]= sign[i]*gains[i]/max_gain/(Ee[ind[i]]+.001);
-#endif      
-      for (i=0;i<5;i++)
-         for (j=0;j<8;j++)
-            exc[8*i+j]+=gains[i]*codebook[ind[i]][j];
+   for (i=0;i<nb_subvect;i++)
+   {
+      int gain_id;
+      ind[i] = frame_bits_unpack_unsigned(bits, params->shape_bits);
+      if (frame_bits_unpack_unsigned(bits, 1))
+         sign[i]=-1;
+      else
+         sign[i]=1;
+      
+      gain_id = frame_bits_unpack_unsigned(bits, 3);
+      gains[i]=scal_gains4[gain_id];
+      gains[i] *= sign[i];
+      gains[i] *= exc_energy;
    }
-   /*for (i=0;i<5;i++)
-      printf ("%f ", gains[i]);
-      printf ("cbgains: \n");*/
-   /*TODO: Perform joint optimization of gains and quantization with prediction*/
+
+   for (i=0;i<nb_subvect;i++)
+      for (j=0;j<subvect_size;j++)
+         exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
    
-   for (i=0;i<40;i++)
-      target[i]=t[i];
+   POP(stack);
+   POP(stack);
+   POP(stack);
 }
-