1 files changed, 123 insertions, 274 deletions
diff --git a/gr-vocoder/lib/codec2/interp.c b/gr-vocoder/lib/codec2/interp.c
index fad4554f43..be89fc3154 100644
--- a/gr-vocoder/lib/codec2/interp.c
+++ b/gr-vocoder/lib/codec2/interp.c
@@ -29,13 +29,11 @@
 #include <math.h>
 #include <string.h>
 #include <stdio.h>
-#include <stdlib.h>
 
 #include "defines.h"
 #include "interp.h"
 #include "lsp.h"
 #include "quantise.h"
-#include "dump.h"
 
 float sample_log_amp(MODEL *model, float w);
 
@@ -113,23 +111,19 @@ float sample_log_amp(MODEL *model, float w)
 
     assert(w > 0.0); assert (w <= PI);
 
-    m = 0;
-    while ((m+1)*model->Wo < w) m++;
-    f = (w - m*model->Wo)/model->Wo;
+    m = floorf(w/model->Wo + 0.5);
+    f = (w - m*model->Wo)/w;
     assert(f <= 1.0);
 
     if (m < 1) {
-	log_amp = f*log10(model->A[1] + 1E-6);
+	log_amp = f*log10f(model->A[1] + 1E-6);
     }
     else if ((m+1) > model->L) {
-	log_amp = (1.0-f)*log10(model->A[model->L] + 1E-6);
+	log_amp = (1.0-f)*log10f(model->A[model->L] + 1E-6);
     }
     else {
-	log_amp = (1.0-f)*log10(model->A[m] + 1E-6) +
-                  f*log10(model->A[m+1] + 1E-6);
-	//printf("m=%d A[m] %f A[m+1] %f x %f %f %f\n", m, model->A[m],
-	//       model->A[m+1], pow(10.0, log_amp),
-	//       (1-f), f);
+	log_amp = (1.0-f)*log10f(model->A[m] + 1E-6) +
+                  f*log10f(model->A[m+1] + 1E-6);
     }
 
     return log_amp;
@@ -137,338 +131,193 @@ float sample_log_amp(MODEL *model, float w)
 
 /*---------------------------------------------------------------------------*\
 
-  FUNCTION....: sample_log_amp_quad()
+  FUNCTION....: interp_lsp()
   AUTHOR......: David Rowe
-  DATE CREATED: 9 March 2011
-
-  Samples the amplitude envelope at an arbitrary frequency w.  Uses
-  quadratic interpolation in the log domain to sample between harmonic
-  amplitudes.
-
-  y(x) = ax*x + bx + c
+  DATE CREATED: 10 Nov 2010
 
-  We assume three points are x=-1, x=0, x=1, which we map to m-1,m,m+1
+  Given two frames decribed by model parameters 20ms apart, determines
+  the model parameters of the 10ms frame between them.  Assumes
+  voicing is available for middle (interpolated) frame.  Outputs are
+  amplitudes and Wo for the interpolated frame.
 
-  c = y(0)
-  b = (y(1) - y(-1))/2
-  a = y(-1) + b - y(0)
+  This version uses interpolation of LSPs, seems to do a better job
+  with bg noise.
 
 \*---------------------------------------------------------------------------*/
 
-float sample_log_amp_quad(MODEL *model, float w)
+void interpolate_lsp(
+  kiss_fft_cfg  fft_fwd_cfg,
+  MODEL *interp,    /* interpolated model params                     */
+  MODEL *prev,      /* previous frames model params                  */
+  MODEL *next,      /* next frames model params                      */
+  float *prev_lsps, /* previous frames LSPs                          */
+  float  prev_e,    /* previous frames LPC energy                    */
+  float *next_lsps, /* next frames LSPs                              */
+  float  next_e,    /* next frames LPC energy                        */
+  float *ak_interp, /* interpolated aks for this frame               */
+  float *lsps_interp/* interpolated lsps for this frame              */
+)
 {
-    int   m;
-    float a,b,c,x, log_amp;
+    int   i;
+    float e;
+    float snr;
 
-    assert(w > 0.0); assert (w <= PI);
+    /* trap corner case where V est is probably wrong */
 
-    m = floor(w/model->Wo + 0.5);
-    if (m < 2) m = 2;
-    if (m > (model->L-1)) m = model->L-1;
-    c = log10(model->A[m]+1E-6);
-    b = (log10(model->A[m+1]+1E-6) - log10(model->A[m-1]+1E-6))/2.0;
-    a = log10(model->A[m-1]+1E-6) + b - c;
-    x = (w - m*model->Wo)/model->Wo;
-
-    log_amp = a*x*x + b*x + c;
-    //printf("m=%d A[m-1] %f A[m] %f A[m+1] %f w %f x %f log_amp %f\n", m,
-    //	   model->A[m-1],
-    //	   model->A[m], model->A[m+1], w, x, pow(10.0, log_amp));
-    return log_amp;
-}
+    if (interp->voiced && !prev->voiced && !next->voiced) {
+	interp->voiced = 0;
+    }
 
-/*---------------------------------------------------------------------------*\
+    /* Wo depends on voicing of this and adjacent frames */
 
-  FUNCTION....: sample_log_amp_quad_nl()
-  AUTHOR......: David Rowe
-  DATE CREATED: 10 March 2011
+    if (interp->voiced) {
+	if (prev->voiced && next->voiced)
+	    interp->Wo = (prev->Wo + next->Wo)/2.0;
+	if (!prev->voiced && next->voiced)
+	    interp->Wo = next->Wo;
+	if (prev->voiced && !next->voiced)
+	    interp->Wo = prev->Wo;
+    }
+    else {
+	interp->Wo = TWO_PI/P_MAX;
+    }
+    interp->L = PI/interp->Wo;
 
-  Samples the amplitude envelope at an arbitrary frequency w.  Uses
-  quadratic interpolation in the log domain to sample between harmonic
-  amplitudes.  This version can handle non-linear steps along a freq
-  axis defined by arbitrary steps.
+    //printf("  interp: prev_v: %d next_v: %d prev_Wo: %f next_Wo: %f\n",
+    //	   prev->voiced, next->voiced, prev->Wo, next->Wo);
+    //printf("  interp: Wo: %1.5f  L: %d\n", interp->Wo, interp->L);
 
-  y(x) = ax*x + bx + c
+    /* interpolate LSPs */
 
-  We assume three points are (x_1,y_1), (0,y0) and (x1,y1).
+    for(i=0; i<LPC_ORD; i++) {
+	lsps_interp[i] = (prev_lsps[i] + next_lsps[i])/2.0;
+    }
 
-\*---------------------------------------------------------------------------*/
+    /* Interpolate LPC energy in log domain */
 
-float sample_log_amp_quad_nl(
-			     float w[],     /* frequency points            */
-			     float A[],     /* for these amplitude samples */
-			     int np,        /* number of frequency points  */
-			     float w_sample /* frequency of new samples    */
-)
-{
-    int   m,i;
-    float a,b,c,x, log_amp, best_dist;
-    float x_1, x1;
-    float y_1, y0, y1;
-
-    //printf("w_sample  %f\n", w_sample);
-    assert(w_sample >= 0.0); assert (w_sample <= 1.1*PI);
-
-    /* find closest point to centre quadratic interpolator */
-
-    best_dist = 1E32;
-    m = 0;
-    for (i=0; i<np; i++)
-	if (fabs(w[i] - w_sample) < best_dist) {
-	    best_dist = fabs(w[i] - w_sample);
-	    m = i;
-	}
-
-    /* stay one point away from edge of array */
-
-    if (m < 1) m = 1;
-    if (m > (np-2)) m = np - 2;
-
-    /* find polynomial coeffs */
-
-    x_1 = w[m-1]- w[m]; x1 = w[m+1] - w[m];
-    y_1 = log10(A[m-1]+1E-6);
-    y0  = log10(A[m]+1E-6);
-    y1  = log10(A[m+1]+1E-6);
-
-    c = y0;
-    a = (y_1*x1 - y1*x_1 + c*x_1 - c*x1)/(x_1*x_1*x1 - x1*x1*x_1);
-    b = (y1 -a*x1*x1 - c)/x1;
-    x = w_sample - w[m];
-
-    //printf("%f   %f  %f\n", w[0], w[1], w[2]);
-    //printf("%f %f  %f %f  %f %f\n", x_1, y_1, 0.0, y0, x1, y1);
-    log_amp = a*x*x + b*x + c;
-    //printf("a %f  b %f  c %f\n", a, b, c);
-    //printf("m=%d A[m-1] %f A[m] %f A[m+1] %f w_sample %f w[m] %f x %f log_amp %f\n", m,
-    //	   A[m-1],
-    //	   A[m], A[m+1], w_sample, w[m], x, log_amp);
-    //exit(0);
-    return log_amp;
-}
+    e = powf(10.0, (log10f(prev_e) + log10f(next_e))/2.0);
+    //printf("  interp: e: %f\n", e);
 
-#define M_MAX 40
+    /* convert back to amplitudes */
+
+    lsp_to_lpc(lsps_interp, ak_interp, LPC_ORD);
+    aks_to_M2(fft_fwd_cfg, ak_interp, LPC_ORD, interp, e, &snr, 0, 0, 1, 1, LPCPF_BETA, LPCPF_GAMMA);
+    //printf("  interp: ak[1]: %f A[1] %f\n", ak_interp[1], interp->A[1]);
+}
 
-float fres[] = {100,   200,  300,  400,  500,  600,  700,  800,  900, 1000,
-		1200, 1400, 1600, 1850, 2100, 2350, 2600, 2900, 3400, 3800};
 
 /*---------------------------------------------------------------------------*\
 
-  FUNCTION....: resample_amp_nl()
+  FUNCTION....: interp_Wo()
   AUTHOR......: David Rowe
-  DATE CREATED: 7 March 2011
+  DATE CREATED: 22 May 2012
 
-  Converts the current model with L {Am} samples spaced Wo apart to
-  RES_POINTS samples spaced Wo/RES_POINTS apart.  Then subtracts
-  from the previous frames samples to get the delta.
+  Interpolates centre 10ms sample of Wo and L samples given two
+  samples 20ms apart. Assumes voicing is available for centre
+  (interpolated) frame.
 
 \*---------------------------------------------------------------------------*/
 
-void resample_amp_fixed(MODEL *model,
-			float w[], float A[],
-			float wres[], float Ares[],
-			float AresdB_prev[],
-			float AresdB[],
-			float deltat[])
+void interp_Wo(
+  MODEL *interp,    /* interpolated model params                     */
+  MODEL *prev,      /* previous frames model params                  */
+  MODEL *next       /* next frames model params                      */
+	       )
 {
-    int   i;
-
-    for(i=1; i<=model->L; i++) {
-	w[i-1] = i*model->Wo;
-	A[i-1] = model->A[i];
-    }
-
-    for(i=0; i<RES_POINTS; i++) {
-	wres[i] = fres[i]*PI/4000.0;
-    }
-
-    for(i=0; i<RES_POINTS; i++) {
-	Ares[i] = pow(10.0,sample_log_amp_quad_nl(w, A, model->L, wres[i]));
-    }
-
-    /* work out delta T vector for this frame */
-
-    for(i=0; i<RES_POINTS; i++) {
-	AresdB[i] = 20.0*log10(Ares[i]);
-	deltat[i] = AresdB[i] - AresdB_prev[i];
-    }
-
+    interp_Wo2(interp, prev, next, 0.5);
 }
 
 /*---------------------------------------------------------------------------*\
 
-  FUNCTION....: resample_amp_nl()
+  FUNCTION....: interp_Wo2()
   AUTHOR......: David Rowe
-  DATE CREATED: 7 March 2011
-
-  Converts the current model with L {Am} samples spaced Wo apart to M
-  samples spaced Wo/M apart.  Then converts back to L {Am} samples.
-  used to prototype constant rate Amplitude encoding ideas.
+  DATE CREATED: 22 May 2012
 
-  Returns the SNR in dB.
+  Weighted interpolation of two Wo samples.
 
 \*---------------------------------------------------------------------------*/
 
-float resample_amp_nl(MODEL *model, int m, float AresdB_prev[])
+void interp_Wo2(
+  MODEL *interp,    /* interpolated model params                     */
+  MODEL *prev,      /* previous frames model params                  */
+  MODEL *next,      /* next frames model params                      */
+  float  weight
+)
 {
-    int   i;
-    float w[MAX_AMP], A[MAX_AMP];
-    float wres[MAX_AMP], Ares[MAX_AMP], AresdB[MAX_AMP];
-    float signal, noise, snr;
-    float new_A;
-    float deltat[MAX_AMP], deltat_q[MAX_AMP], AresdB_q[MAX_AMP];
-
-    resample_amp_fixed(model, w, A, wres, Ares, AresdB_prev, AresdB, deltat);
-
-    /* quantise delta T vector */
-
-    for(i=0; i<RES_POINTS; i++) {
-	noise = 3.0*(1.0 - 2.0*rand()/RAND_MAX);
-	//noise = 0.0;
-	deltat_q[i] = deltat[i] + noise;
-    }
+    /* trap corner case where voicing est is probably wrong */
 
-    /* recover Ares vector */
-
-    for(i=0; i<RES_POINTS; i++) {
-	AresdB_q[i] = AresdB_prev[i] + deltat_q[i];
-	Ares[i] = pow(10.0, AresdB_q[i]/20.0);
-	//printf("%d %f %f\n", i, AresdB[i], AresdB_q[i]);
+    if (interp->voiced && !prev->voiced && !next->voiced) {
+	interp->voiced = 0;
     }
 
-    /* update memory based on version at decoder */
+    /* Wo depends on voicing of this and adjacent frames */
 
-    for(i=0; i<RES_POINTS; i++) {
-	AresdB_prev[i] = AresdB_q[i];
+    if (interp->voiced) {
+	if (prev->voiced && next->voiced)
+	    interp->Wo = (1.0 - weight)*prev->Wo + weight*next->Wo;
+	if (!prev->voiced && next->voiced)
+	    interp->Wo = next->Wo;
+	if (prev->voiced && !next->voiced)
+	    interp->Wo = prev->Wo;
     }
-
-#ifdef DUMP
-    dump_resample(wres,Ares,M_MAX);
-#endif
-
-    signal = noise = 0.0;
-
-    for(i=1; i<model->L; i++) {
-	new_A = pow(10.0,sample_log_amp_quad_nl(wres, Ares, RES_POINTS, model->Wo*i));
-	signal += pow(model->A[i], 2.0);
-	noise  += pow(model->A[i] - new_A, 2.0);
-	//printf("%f %f\n", model->A[i], new_A);
-	model->A[i] = new_A;
+    else {
+	interp->Wo = TWO_PI/P_MAX;
     }
-
-    snr = 10.0*log10(signal/noise);
-    printf("snr = %3.2f\n", snr);
-    //exit(0);
-    return snr;
+    interp->L = PI/interp->Wo;
 }
 
+
 /*---------------------------------------------------------------------------*\
 
-  FUNCTION....: resample_amp()
+  FUNCTION....: interp_energy()
   AUTHOR......: David Rowe
-  DATE CREATED: 10 March 2011
-
-  Converts the current model with L {Am} samples spaced Wo apart to M
-  samples with a non-linear spacing.  Then converts back to L {Am}
-  samples.  used to prototype constant rate Amplitude encoding ideas.
+  DATE CREATED: 22 May 2012
 
-  Returns the SNR in dB.
+  Interpolates centre 10ms sample of energy given two samples 20ms
+  apart.
 
 \*---------------------------------------------------------------------------*/
 
-float resample_amp(MODEL *model, int m)
+float interp_energy(float prev_e, float next_e)
 {
-    int   i;
-    MODEL model_m;
-    float new_A, signal, noise, snr, log_amp_dB;
-    float n_db = 0.0;
-
-    model_m.Wo = PI/(float)m;
-    model_m.L = PI/model_m.Wo;
-
-    for(i=1; i<=model_m.L; i++) {
-	log_amp_dB    = 20.0*sample_log_amp_quad(model, i*model_m.Wo);
-	log_amp_dB   += n_db*(1.0 - 2.0*rand()/RAND_MAX);
-	model_m.A[i]  = pow(10,log_amp_dB/20.0);
-    }
-
-    //dump_resample(&model_m);
-
-    signal = noise = 0.0;
+    return powf(10.0, (log10f(prev_e) + log10f(next_e))/2.0);
 
-    for(i=1; i<model->L/4; i++) {
-	new_A = pow(10,sample_log_amp_quad(&model_m, i*model->Wo));
-	signal += pow(model->A[i], 2.0);
-	noise  += pow(model->A[i] - new_A, 2.0);
-	//printf("%f %f\n", model->A[i], new_A);
-	model->A[i] = new_A;
-    }
-
-    snr = 10.0*log10(signal/noise);
-    //printf("snr = %3.2f\n", snr);
-    //exit(0);
-    return snr;
 }
 
+
 /*---------------------------------------------------------------------------*\
 
-  FUNCTION....: interp_lsp()
+  FUNCTION....: interp_energy2()
   AUTHOR......: David Rowe
-  DATE CREATED: 10 Nov 2010
-
-  Given two frames decribed by model parameters 20ms apart, determines
-  the model parameters of the 10ms frame between them.  Assumes
-  voicing is available for middle (interpolated) frame.  Outputs are
-  amplitudes and Wo for the interpolated frame.
+  DATE CREATED: 22 May 2012
 
-  This version uses interpolation of LSPs, seems to do a better job
-  with bg noise.
+  Interpolates centre 10ms sample of energy given two samples 20ms
+  apart.
 
 \*---------------------------------------------------------------------------*/
 
-void interpolate_lsp(
-  MODEL *interp,    /* interpolated model params                     */
-  MODEL *prev,      /* previous frames model params                  */
-  MODEL *next,      /* next frames model params                      */
-  float *prev_lsps, /* previous frames LSPs                          */
-  float  prev_e,    /* previous frames LPC energy                    */
-  float *next_lsps, /* next frames LSPs                              */
-  float  next_e,    /* next frames LPC energy                        */
-  float *ak_interp  /* interpolated aks for this frame                */
-		     )
+float interp_energy2(float prev_e, float next_e, float weight)
 {
-    //int   l,i;
-    int   i;
-    float lsps[LPC_ORD],e;
-    float snr;
+    return powf(10.0, (1.0 - weight)*log10f(prev_e) + weight*log10f(next_e));
 
-    /* Wo depends on voicing of this and adjacent frames */
+}
 
-    if (interp->voiced) {
-	if (prev->voiced && next->voiced)
-	    interp->Wo = (prev->Wo + next->Wo)/2.0;
-	if (!prev->voiced && next->voiced)
-	    interp->Wo = next->Wo;
-	if (prev->voiced && !next->voiced)
-	    interp->Wo = prev->Wo;
-    }
-    else {
-	interp->Wo = TWO_PI/P_MAX;
-    }
-    interp->L = PI/interp->Wo;
 
-    /* interpolate LSPs */
+/*---------------------------------------------------------------------------*\
 
-    for(i=0; i<LPC_ORD; i++) {
-	lsps[i] = (prev_lsps[i] + next_lsps[i])/2.0;
-    }
+  FUNCTION....: interpolate_lsp_ver2()
+  AUTHOR......: David Rowe
+  DATE CREATED: 22 May 2012
 
-    /* Interpolate LPC energy in log domain */
+  Weighted interpolation of LSPs.
 
-    e = pow(10.0, (log10(prev_e) + log10(next_e))/2.0);
+\*---------------------------------------------------------------------------*/
 
-    /* convert back to amplitudes */
+void interpolate_lsp_ver2(float interp[], float prev[],  float next[], float weight)
+{
+    int i;
 
-    lsp_to_lpc(lsps, ak_interp, LPC_ORD);
-    aks_to_M2(ak_interp, LPC_ORD, interp, e, &snr, 0);
+    for(i=0; i<LPC_ORD; i++)
+	interp[i] = (1.0 - weight)*prev[i] + weight*next[i];
 }
+