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