Added a post-filter for narrowband (and thus 0-4 kHz in wideband)
[speexdsp.git] / libspeex / nb_celp.c
index dd35820..871f15f 100644 (file)
@@ -28,6 +28,9 @@
 #include "cb_search.h"
 #include "filters.h"
 #include "stack_alloc.h"
+#include "vq.h"
+#include "speex_bits.h"
+#include "post_filter.h"
 
 #ifndef M_PI
 #define M_PI           3.14159265358979323846  /* pi */
@@ -182,8 +185,7 @@ void nb_encoder_destroy(void *state)
    free(st);
 }
 
-
-void nb_encode(void *state, float *in, FrameBits *bits)
+void nb_encode(void *state, float *in, SpeexBits *bits)
 {
    EncState *st;
    int i, sub, roots;
@@ -229,9 +231,10 @@ void nb_encode(void *state, float *in, FrameBits *bits)
    /* x-domain to angle domain*/
    for (i=0;i<st->lpcSize;i++)
       st->lsp[i] = acos(st->lsp[i]);
-   
+   /*print_vec(st->lsp, 10, "LSP:");*/
    /* LSP Quantization */
    st->lsp_quant(st->lsp, st->qlsp, st->lpcSize, bits);
+
    /*for (i=0;i<st->lpcSize;i++)
      st->qlsp[i]=st->lsp[i];*/
    /*printf ("LSP ");
@@ -283,6 +286,19 @@ void nb_encode(void *state, float *in, FrameBits *bits)
       for (i=0;i<st->lpcSize;i++)
          st->interp_qlsp[i] = (1-tmp)*st->old_qlsp[i] + tmp*st->qlsp[i];
 
+      if (0) {
+         float *h=PUSH(st->stack, 8);
+         for (i=0;i<8;i++)
+            h[i]=0;
+         h[0]=1;
+         
+         residue_zero(h, st->bw_lpc1, h, 8, st->lpcSize);
+         syn_filt_zero(h, st->interp_qlpc, h, 8, st->lpcSize);
+         syn_filt_zero(h, st->bw_lpc2, h, 8, st->lpcSize);
+         print_vec(h, 8, "lpc_resp");
+         POP(st->stack);
+      }
+      
       /* Compute interpolated LPCs (quantized and unquantized) */
       for (i=0;i<st->lpcSize;i++)
          st->interp_lsp[i] = cos(st->interp_lsp[i]);
@@ -388,7 +404,7 @@ void nb_encode(void *state, float *in, FrameBits *bits)
       syn_filt_zero(target, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
       residue_zero(res, st->interp_qlpc, st->buf2, st->subframeSize, st->lpcSize);
       residue_zero(st->buf2, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize);
-      if (1||(snr>9 && (rand()%6==0)))
+      /*if (1||(snr>9 && (rand()%6==0)))
       {
          float ener=0;
          printf ("exc ");
@@ -401,24 +417,88 @@ void nb_encode(void *state, float *in, FrameBits *bits)
          }
          printf ("\n");
       printf ("innovation_energy = %f\n", ener);
+      }*/
+      if (rand()%5==0 && snr>5)
+      {
+         float ener=0, sign=1;
+         if (rand()%2)
+            sign=-1;
+         for (i=0;i<st->subframeSize;i++)
+         {
+            ener+=st->buf2[i]*st->buf2[i];
+         }
+         ener=sign/sqrt(.01+ener/st->subframeSize);
+         for (i=0;i<st->subframeSize;i++)
+         {
+            if (i%10==0)
+               printf ("\nexc ");
+            printf ("%f ", ener*st->buf2[i]);
+         }
+         printf ("\n");
       }
+
       for (i=0;i<st->subframeSize;i++)
          exc[i]+=st->buf2[i];
 #else
-      /* Perform a split-codebook search */
+      if (0)
+      {
+      /* Perform innovation search */
       st->innovation_quant(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2,
                            st->innovation_params, st->lpcSize,
                            st->subframeSize, exc, bits, st->stack);
+      }
+      else
+      {
+         float *innov;
+         float ener=0, ener_1;
+         innov=PUSH(st->stack, st->subframeSize);
+         for (i=0;i<st->subframeSize;i++)
+            innov[i]=0;
+         syn_filt_zero(target, st->bw_lpc1, res, st->subframeSize, st->lpcSize);
+         residue_zero(res, st->interp_qlpc, st->buf2, st->subframeSize, st->lpcSize);
+         residue_zero(st->buf2, st->bw_lpc2, st->buf2, st->subframeSize, st->lpcSize);
+         for (i=0;i<st->subframeSize;i++)
+            ener+=st->buf2[i]*st->buf2[i];
+         ener=sqrt(.1+ener/st->subframeSize);
 
+         {
+            int qe = (int)(floor(7*log(ener)));
+            if (qe<0)
+               qe=0;
+            if (qe>63)
+               qe=63;
+            ener = exp(qe/7.0);
+            speex_bits_pack(bits, qe, 6);
+         }
+         ener_1 = 1/ener;
+         
+         for (i=0;i<st->subframeSize;i++)
+            target[i]*=ener_1;
+#if 1
+         st->innovation_quant(target, st->interp_qlpc, st->bw_lpc1, st->bw_lpc2, 
+                                st->innovation_params, st->lpcSize, st->subframeSize, 
+                                innov, bits, st->stack);
+         
+         for (i=0;i<st->subframeSize;i++)
+            exc[i] += innov[i]*ener;
+#else
+         for (i=0;i<st->subframeSize;i++)
+            exc[i] += st->buf2[i];
+#endif
+         POP(st->stack);
+         for (i=0;i<st->subframeSize;i++)
+            target[i]*=ener;
+
+      }
 #endif
       /* Compute weighted noise energy and SNR */
       enoise=0;
       for (i=0;i<st->subframeSize;i++)
          enoise += target[i]*target[i];
       snr = 10*log10((esig+1)/(enoise+1));
-#ifdef DEBUG
+
       printf ("seg SNR = %f\n", snr);
-#endif
+
       /*Keep the previous memory*/
       for (i=0;i<st->lpcSize;i++)
          mem[i]=st->mem_sp[i];
@@ -508,10 +588,14 @@ void *nb_decoder_init(SpeexMode *m)
    st->frame = st->inBuf + st->bufSize - st->windowSize;
    st->excBuf = malloc(st->bufSize*sizeof(float));
    st->exc = st->excBuf + st->bufSize - st->windowSize;
+   st->exc2Buf = malloc(st->bufSize*sizeof(float));
+   st->exc2 = st->exc2Buf + st->bufSize - st->windowSize;
    for (i=0;i<st->bufSize;i++)
       st->inBuf[i]=0;
    for (i=0;i<st->bufSize;i++)
       st->excBuf[i]=0;
+   for (i=0;i<st->bufSize;i++)
+      st->exc2Buf[i]=0;
 
    st->interp_qlpc = malloc((st->lpcSize+1)*sizeof(float));
    st->qlsp = malloc(st->lpcSize*sizeof(float));
@@ -530,6 +614,7 @@ void nb_decoder_destroy(void *state)
    st=state;
    free(st->inBuf);
    free(st->excBuf);
+   free(st->exc2Buf);
    free(st->interp_qlpc);
    free(st->qlsp);
    free(st->old_qlsp);
@@ -541,15 +626,18 @@ void nb_decoder_destroy(void *state)
    free(state);
 }
 
-void nb_decode(void *state, FrameBits *bits, float *out)
+void nb_decode(void *state, SpeexBits *bits, float *out, int lost)
 {
    DecState *st;
    int i, sub;
+   int pitch;
+   float pitch_gain;
 
    st=state;
 
    memmove(st->inBuf, st->inBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
    memmove(st->excBuf, st->excBuf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
+   memmove(st->exc2Buf, st->exc2Buf+st->frameSize, (st->bufSize-st->frameSize)*sizeof(float));
 
 
    st->lsp_unquant(st->qlsp, st->lpcSize, bits);
@@ -563,7 +651,7 @@ void nb_decode(void *state, FrameBits *bits, float *out)
    for (sub=0;sub<st->nbSubframes;sub++)
    {
       int offset;
-      float *sp, *exc, tmp;
+      float *sp, *exc, *exc2, tmp;
       
       /* Offset relative to start of frame */
       offset = st->subframeSize*sub;
@@ -571,6 +659,8 @@ void nb_decode(void *state, FrameBits *bits, float *out)
       sp=st->frame+offset;
       /* Excitation */
       exc=st->exc+offset;
+      /* Excitation after post-filter*/
+      exc2=st->exc2+offset;
 
       /* LSP interpolation (quantized and unquantized) */
       tmp = (.5 + sub)/st->nbSubframes;
@@ -595,13 +685,38 @@ void nb_decode(void *state, FrameBits *bits, float *out)
          exc[i]=0;
 
       /*Adaptive codebook contribution*/
-      st->ltp_unquant(exc, st->min_pitch, st->max_pitch, st->ltp_params, st->subframeSize, bits, st->stack);
-       
-      /*Fixed codebook contribution*/
-      st->innovation_unquant(exc, st->innovation_params, st->subframeSize, bits, st->stack);
+      st->ltp_unquant(exc, st->min_pitch, st->max_pitch, st->ltp_params, st->subframeSize, &pitch, &pitch_gain, bits, st->stack, lost);
+      
+
+      {
+         int q_energy;
+         float ener;
+         float *innov;
+         
+         innov = PUSH(st->stack, st->subframeSize);
+         for (i=0;i<st->subframeSize;i++)
+            innov[i]=0;
+
+         q_energy = speex_bits_unpack_unsigned(bits, 6);
+         ener = exp(q_energy/7.0);
+         /*printf ("unquant_energy: %d %f\n", q_energy, ener);*/
+         
+         /*Fixed codebook contribution*/
+         st->innovation_unquant(innov, st->innovation_params, st->subframeSize, bits, st->stack);
+
+         for (i=0;i<st->subframeSize;i++)
+            exc[i]+=ener*innov[i];
+
+         POP(st->stack);
+      }
+
+      for (i=0;i<st->subframeSize;i++)
+         exc2[i]=exc[i];
+      /*nb_post_filter(exc, exc2, st->interp_qlpc, st->lpcSize, st->subframeSize,
+        pitch, pitch_gain, st->stack);*/
 
       /*Compute decoded signal*/
-      syn_filt_mem(exc, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
+      syn_filt_mem(exc2, st->interp_qlpc, sp, st->subframeSize, st->lpcSize, st->mem_sp);
 
    }