30b1131560df00c9985a55acaa9926bf4cd01053
[speexdsp.git] / libspeex / cb_search.c
1 /*-----------------------------------------------------------------------*\
2
3     FILE........: GAINSHAPE.C
4     TYPE........: C Module
5     AUTHOR......: David Rowe
6     COMPANY.....: Voicetronix
7     DATE CREATED: 19/2/02
8
9     General gain-shape codebook search.
10
11 \*-----------------------------------------------------------------------*/
12
13 /* Modified by Jean-Marc Valin 2002
14
15    This library is free software; you can redistribute it and/or
16    modify it under the terms of the GNU Lesser General Public
17    License as published by the Free Software Foundation; either
18    version 2.1 of the License, or (at your option) any later version.
19    
20    This library is distributed in the hope that it will be useful,
21    but WITHOUT ANY WARRANTY; without even the implied warranty of
22    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
23    Lesser General Public License for more details.
24    
25    You should have received a copy of the GNU Lesser General Public
26    License along with this library; if not, write to the Free Software
27    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
28 */
29
30
31
32 #include <stdlib.h>
33 #include <cb_search.h>
34 #include "filters.h"
35 #include <math.h>
36 #include <stdio.h>
37 #include "stack_alloc.h"
38 #include "vq.h"
39
40 extern float exc_gains_wb2_table[];
41 /*---------------------------------------------------------------------------*\
42                                                                              
43  void overlap_cb_search()                                                             
44                                                                               
45  Searches a gain/shape codebook consisting of overlapping entries for the    
46  closest vector to the target.  Gives identical results to search() above   
47  buts uses fast end correction algorithm for the synthesis of response       
48  vectors.                                                                     
49                                                                              
50 \*---------------------------------------------------------------------------*/
51
52 float overlap_cb_search(
53 float target[],                 /* target vector */
54 float ak[],                     /* LPCs for this subframe */
55 float awk1[],                   /* Weighted LPCs for this subframe */
56 float awk2[],                   /* Weighted LPCs for this subframe */
57 float codebook[],               /* overlapping codebook */
58 int   entries,                  /* number of overlapping entries to search */
59 float *gain,                    /* gain of optimum entry */
60 int   *index,                   /* index of optimum entry */
61 int   p,                        /* number of LPC coeffs */
62 int   nsf                       /* number of samples in subframe */
63 )
64 {
65   float *resp;                  /* zero state response to current entry */
66   float *h;                     /* impulse response of synthesis filter */
67   float *impulse;               /* excitation vector containing one impulse */
68   float d,e,g,score;            /* codebook searching variables */
69   float bscore;                 /* score of "best" vector so far */
70   int i,k;                      /* loop variables */
71
72   /* Initialise */
73   
74   resp = (float*)malloc(sizeof(float)*nsf);
75   h = (float*)malloc(sizeof(float)*nsf);
76   impulse = (float*)malloc(sizeof(float)*nsf);
77
78   for(i=0; i<nsf; i++)
79     impulse[i] = 0.0;
80    
81   *gain = 0.0;
82   *index = 0;
83   bscore = 0.0;
84   impulse[0] = 1.0;
85
86   /* Calculate impulse response of  A(z/g2) / ( A(z)*(z/g1) ) */
87   residue_zero(impulse, awk1, h, nsf, p);
88   syn_filt_zero(h, ak, h, nsf, p);
89   syn_filt_zero(h, awk2, h, nsf,p);
90   
91   /* Calculate codebook zero-response */
92   residue_zero(&codebook[entries-1],awk1,resp,nsf,p);
93   syn_filt_zero(resp,ak,resp,nsf,p);
94   syn_filt_zero(resp,awk2,resp,nsf,p);
95     
96   /* Search codebook backwards using end correction for synthesis */
97   
98   for(k=entries-1; k>=0; k--) {
99
100     d = 0.0; e = 0.0;
101     for(i=0; i<nsf; i++) {
102       d += target[i]*resp[i];
103       e += resp[i]*resp[i];
104     }
105     g = d/(e+.1);
106     score = g*d;
107     /*printf ("score: %f %f %f %f\n", target[0],d,e,score);*/
108     if (score >= bscore) {
109       bscore = score;
110       *gain = g;
111       *index = k;
112     }
113     
114     /* Synthesise next entry */
115     
116     if (k) {
117       for(i=nsf-1; i>=1; i--)
118         resp[i] = resp[i-1] + codebook[k-1]*h[i];
119       resp[0] = codebook[k-1]*h[0];
120     }
121   }
122
123   free(resp);
124   free(h);
125   free(impulse);
126   return bscore;
127 }
128
129
130 void split_cb_search(
131 float target[],                 /* target vector */
132 float ak[],                     /* LPCs for this subframe */
133 float awk1[],                   /* Weighted LPCs for this subframe */
134 float awk2[],                   /* Weighted LPCs for this subframe */
135 void *par,                      /* Codebook/search parameters*/
136 int   p,                        /* number of LPC coeffs */
137 int   nsf,                      /* number of samples in subframe */
138 float *exc,
139 FrameBits *bits,
140 float *stack
141 )
142 {
143    int i,j;
144    float *resp, *E, *Ee;
145    float *t, *r, *e;
146    float *gains;
147    int *ind;
148    float *shape_cb, *gain_cb;
149    int shape_cb_size, gain_cb_size, subvect_size, nb_subvect;
150    split_cb_params *params;
151
152    params = (split_cb_params *) par;
153    subvect_size = params->subvect_size;
154    nb_subvect = params->nb_subvect;
155    shape_cb_size = 1<<params->shape_bits;
156    shape_cb = params->shape_cb;
157    gain_cb_size = 1<<params->gain_bits;
158    gain_cb = params->gain_cb;
159    resp = PUSH(stack, shape_cb_size*subvect_size);
160    E = PUSH(stack, shape_cb_size);
161    Ee = PUSH(stack, shape_cb_size);
162    t = PUSH(stack, nsf);
163    r = PUSH(stack, nsf);
164    e = PUSH(stack, nsf);
165    gains = PUSH(stack, nb_subvect);
166    ind = (int*)PUSH(stack, nb_subvect);
167    
168
169    for (i=0;i<nsf;i++)
170       t[i]=target[i];
171    for (i=0;i<shape_cb_size;i++)
172    {
173       float *res = resp+i*subvect_size;
174       residue_zero(shape_cb+i*subvect_size, awk1, res, subvect_size, p);
175       syn_filt_zero(res, ak, res, subvect_size, p);
176       syn_filt_zero(res, awk2, res, subvect_size,p);
177       E[i]=0;
178       for(j=0;j<subvect_size;j++)
179          E[i]+=res[j]*res[j];
180       Ee[i]=0;
181       for(j=0;j<subvect_size;j++)
182          Ee[i]+=shape_cb[i*subvect_size+j]*shape_cb[i*subvect_size+j];
183       
184    }
185    for (i=0;i<nb_subvect;i++)
186    {
187       int best_index=0;
188       float g, corr, best_gain=0, score, best_score=-1;
189       for (j=0;j<shape_cb_size;j++)
190       {
191          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
192          score=corr*corr/(.001+E[j]);
193          g = corr/(.001+E[j]);
194          if (score>best_score)
195          {
196             best_index=j;
197             best_score=score;
198             best_gain=corr/(.001+E[j]);
199          }
200       }
201       frame_bits_pack(bits,best_index,params->shape_bits);
202       if (best_gain>0)
203          frame_bits_pack(bits,0,1);
204       else
205           frame_bits_pack(bits,1,1);        
206       ind[i]=best_index;
207       gains[i]=best_gain*Ee[ind[i]];
208
209       for (j=0;j<nsf;j++)
210          e[j]=0;
211       for (j=0;j<subvect_size;j++)
212          e[subvect_size*i+j]=best_gain*shape_cb[best_index*subvect_size+j];
213       residue_zero(e, awk1, r, nsf, p);
214       syn_filt_zero(r, ak, r, nsf, p);
215       syn_filt_zero(r, awk2, r, nsf,p);
216       for (j=0;j<nsf;j++)
217          t[j]-=r[j];
218    }
219
220    {
221       int best_vq_index=0, max_index;
222       float max_gain=0, log_max, min_dist=0, *sign;
223
224       if (gain_cb) /*If no gain codebook, do not quantize (for testing/debugging) */
225       {
226          sign = PUSH(stack, nb_subvect);
227          for (i=0;i<nb_subvect;i++)
228          {
229             if (gains[i]<0)
230             {
231                gains[i]=-gains[i];
232                sign[i]=-1;
233             } else {
234                sign[i]=1;
235             }
236          }
237          for (i=0;i<nb_subvect;i++)
238             if (gains[i]>max_gain)
239                max_gain=gains[i];
240          log_max=log(max_gain+1);
241          max_index = (int)(floor(.5+log_max-3));
242          if (max_index>7)
243             max_index=7;
244          if (max_index<0)
245             max_index=0;
246          max_gain=1/exp(max_index+3.0);
247          for (i=0;i<nb_subvect;i++)
248             gains[i]*=max_gain;
249          frame_bits_pack(bits,max_index,3);
250
251          /*Vector quantize gains[i]*/
252          if (nb_subvect<=5)
253          {
254          best_vq_index = vq_index(gains, gain_cb, nb_subvect, gain_cb_size);
255          frame_bits_pack(bits,best_vq_index,params->gain_bits);
256          printf ("best_gains_vq_index %d %f %d\n", best_vq_index, min_dist, max_index);
257          for (i=0;i<nb_subvect;i++)
258             gains[i]= sign[i]*gain_cb[best_vq_index*nb_subvect+i]/max_gain/(Ee[ind[i]]+.001);
259          } else
260          {
261             float tmp[5];
262             int best_vq_index2;
263          best_vq_index = vq_index(gains, gain_cb, nb_subvect/2, gain_cb_size);
264          for (i=0;i<5;i++)
265             tmp[i]=gains[i]-gain_cb[best_vq_index*nb_subvect/2+i];
266          best_vq_index2 = vq_index(tmp, exc_gains_wb2_table, nb_subvect/2, 256);
267
268          frame_bits_pack(bits,best_vq_index,params->gain_bits);
269          printf ("best_gains_vq_index %d %f %d\n", best_vq_index, min_dist, max_index);
270          for (i=0;i<nb_subvect/2;i++)
271             gains[i]= sign[i]*(gain_cb[best_vq_index*nb_subvect/2+i]+exc_gains_wb2_table[best_vq_index2*nb_subvect/2+i])/max_gain/(Ee[ind[i]]+.001);
272
273
274          best_vq_index = vq_index(gains+5, gain_cb, nb_subvect/2, gain_cb_size);
275          frame_bits_pack(bits,best_vq_index,params->gain_bits);
276          for (i=0;i<5;i++)
277             tmp[i]=gains[i+5]-gain_cb[best_vq_index*nb_subvect/2+i];
278          best_vq_index2 = vq_index(tmp, exc_gains_wb2_table, nb_subvect/2, 256);
279
280          printf ("best_gains_vq_index %d %f %d\n", best_vq_index, min_dist, max_index);
281          for (i=0;i<nb_subvect/2;i++)
282             gains[i+5]= sign[i+5]*(gain_cb[best_vq_index*nb_subvect/2+i]+exc_gains_wb2_table[best_vq_index2*nb_subvect/2+i])/max_gain/(Ee[ind[i+5]]+.001);
283          }
284
285     
286
287          POP(stack);
288       } else {
289          printf ("exc: ");
290          for (i=0;i<nb_subvect;i++)
291             printf ("%f ", gains[i]);
292          printf ("\n");
293          for (i=0;i<nb_subvect;i++)
294             gains[i]= gains[i]/(Ee[ind[i]]+.001);
295       }
296
297       for (i=0;i<nb_subvect;i++)
298          for (j=0;j<subvect_size;j++)
299             exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
300
301    }
302
303    /*TODO: Perform joint optimization of gains*/
304    
305    for (i=0;i<nsf;i++)
306       target[i]=t[i];
307
308    POP(stack);
309    POP(stack);
310    POP(stack);
311    POP(stack);
312    POP(stack);
313    POP(stack);
314    POP(stack);
315    POP(stack);
316 }
317
318
319 void split_cb_unquant(
320 float *exc,
321 void *par,                      /* non-overlapping codebook */
322 int   nsf,                      /* number of samples in subframe */
323 FrameBits *bits,
324 float *stack
325 )
326 {
327    int i,j;
328    int *ind;
329    float *gains;
330    float *sign;
331    int max_gain_ind, vq_gain_ind;
332    float max_gain, *Ee;
333    float *shape_cb, *gain_cb;
334    int shape_cb_size, gain_cb_size, subvect_size, nb_subvect;
335    split_cb_params *params;
336
337    params = (split_cb_params *) par;
338    subvect_size = params->subvect_size;
339    nb_subvect = params->nb_subvect;
340    shape_cb_size = 1<<params->shape_bits;
341    shape_cb = params->shape_cb;
342    gain_cb_size = 1<<params->gain_bits;
343    gain_cb = params->gain_cb;
344    
345    ind = (int*)PUSH(stack, nb_subvect);
346    gains = PUSH(stack, nb_subvect);
347    sign = PUSH(stack, nb_subvect);
348    Ee=PUSH(stack, nb_subvect);
349
350    for (i=0;i<nb_subvect;i++)
351    {
352       ind[i] = frame_bits_unpack_unsigned(bits, params->shape_bits);
353       if (frame_bits_unpack_unsigned(bits, 1))
354          sign[i]=-1;
355       else
356          sign[i]=1;
357       Ee[i]=.001;
358       for (j=0;j<subvect_size;j++)
359          Ee[i]+=shape_cb[ind[i]*subvect_size+j]*shape_cb[ind[i]*subvect_size+j];
360    }
361    max_gain_ind = frame_bits_unpack_unsigned(bits, 3);
362    vq_gain_ind = frame_bits_unpack_unsigned(bits, params->gain_bits);
363    printf ("unquant gains ind: %d %d\n", max_gain_ind, vq_gain_ind);
364
365    max_gain=exp(max_gain_ind+3.0);
366    for (i=0;i<nb_subvect;i++)
367       gains[i] = sign[i]*gain_cb[vq_gain_ind*nb_subvect+i]*max_gain/Ee[i];
368    
369    printf ("unquant gains: ");
370    for (i=0;i<nb_subvect;i++)
371       printf ("%f ", gains[i]);
372    printf ("\n");
373
374    for (i=0;i<nb_subvect;i++)
375       for (j=0;j<subvect_size;j++)
376          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
377    
378    POP(stack);
379    POP(stack);
380    POP(stack);
381    POP(stack);
382 }