Big code cleanup, some minor bug fixed too
[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  void overlap_cb_search()                                                             
57                                                                               
58  Searches a gain/shape codebook consisting of overlapping entries for the    
59  closest vector to the target.  Gives identical results to search() above   
60  buts uses fast end correction algorithm for the synthesis of response       
61  vectors.                                                                     
62                                                                              
63 \*---------------------------------------------------------------------------*/
64
65 float overlap_cb_search(
66 float target[],                 /* target vector */
67 float ak[],                     /* LPCs for this subframe */
68 float awk1[],                   /* Weighted LPCs for this subframe */
69 float awk2[],                   /* Weighted LPCs for this subframe */
70 float codebook[],               /* overlapping codebook */
71 int   entries,                  /* number of overlapping entries to search */
72 float *gain,                    /* gain of optimum entry */
73 int   *index,                   /* index of optimum entry */
74 int   p,                        /* number of LPC coeffs */
75 int   nsf                       /* number of samples in subframe */
76 )
77 {
78   float *resp;                  /* zero state response to current entry */
79   float *h;                     /* impulse response of synthesis filter */
80   float *impulse;               /* excitation vector containing one impulse */
81   float d,e,g,score;            /* codebook searching variables */
82   float bscore;                 /* score of "best" vector so far */
83   int i,k;                      /* loop variables */
84
85   /* Initialise */
86   
87   resp = (float*)malloc(sizeof(float)*nsf);
88   h = (float*)malloc(sizeof(float)*nsf);
89   impulse = (float*)malloc(sizeof(float)*nsf);
90
91   for(i=0; i<nsf; i++)
92     impulse[i] = 0.0;
93    
94   *gain = 0.0;
95   *index = 0;
96   bscore = 0.0;
97   impulse[0] = 1.0;
98
99   /* Calculate impulse response of  A(z/g1) / ( A(z)*(z/g2) ) */
100   residue_zero(impulse, awk1, h, nsf, p);
101   syn_filt_zero(h, ak, h, nsf, p);
102   syn_filt_zero(h, awk2, h, nsf,p);
103   
104   /* Calculate codebook zero-response */
105   residue_zero(&codebook[entries-1],awk1,resp,nsf,p);
106   syn_filt_zero(resp,ak,resp,nsf,p);
107   syn_filt_zero(resp,awk2,resp,nsf,p);
108     
109   /* Search codebook backwards using end correction for synthesis */
110   
111   for(k=entries-1; k>=0; k--) {
112
113     d = 0.0; e = 0.0;
114     for(i=0; i<nsf; i++) {
115       d += target[i]*resp[i];
116       e += resp[i]*resp[i];
117     }
118     g = d/(e+.1);
119     score = g*d;
120     /*printf ("score: %f %f %f %f\n", target[0],d,e,score);*/
121     if (score >= bscore) {
122       bscore = score;
123       *gain = g;
124       *index = k;
125     }
126     
127     /* Synthesise next entry */
128     
129     if (k) {
130       for(i=nsf-1; i>=1; i--)
131         resp[i] = resp[i-1] + codebook[k-1]*h[i];
132       resp[0] = codebook[k-1]*h[0];
133     }
134   }
135
136   free(resp);
137   free(h);
138   free(impulse);
139   return bscore;
140 }
141
142
143
144 void split_cb_search(
145 float target[],                 /* target vector */
146 float ak[],                     /* LPCs for this subframe */
147 float awk1[],                   /* Weighted LPCs for this subframe */
148 float awk2[],                   /* Weighted LPCs for this subframe */
149 void *par,                      /* Codebook/search parameters*/
150 int   p,                        /* number of LPC coeffs */
151 int   nsf,                      /* number of samples in subframe */
152 float *exc,
153 FrameBits *bits,
154 float *stack
155 )
156 {
157    int i,j, id;
158    float *resp, *E, q;
159    float *t, *r, *e, *tresp;
160    float *gains;
161    int *ind;
162    float *shape_cb;
163    int shape_cb_size, subvect_size, nb_subvect;
164    float exc_energy=0;
165    split_cb_params *params;
166
167    params = (split_cb_params *) par;
168    subvect_size = params->subvect_size;
169    nb_subvect = params->nb_subvect;
170    shape_cb_size = 1<<params->shape_bits;
171    shape_cb = params->shape_cb;
172    resp = PUSH(stack, shape_cb_size*subvect_size);
173    tresp = PUSH(stack, shape_cb_size*nsf);
174    E = PUSH(stack, shape_cb_size);
175    t = PUSH(stack, nsf);
176    r = PUSH(stack, nsf);
177    e = PUSH(stack, nsf);
178    gains = PUSH(stack, nb_subvect);
179    ind = (int*)PUSH(stack, nb_subvect);
180
181    /* Compute energy of the "real excitation" */
182    syn_filt_zero(target, awk1, e, nsf, p);
183    residue_zero(e, ak, e, nsf, p);
184    residue_zero(e, awk2, e, nsf, p);
185    for (i=0;i<nsf;i++)
186       exc_energy += e[i]*e[i];
187    exc_energy=sqrt(exc_energy/nb_subvect);
188
189    /* Quantize global ("average") gain */
190    q=log(exc_energy+.1);
191    q=floor(.5+2*(q-2));
192    if (q<0)
193       q=0;
194    if (q>15)
195       q=15;
196    id = (int)q;
197    frame_bits_pack(bits, id, 4);
198    exc_energy=exp(.5*q+2);
199
200
201    for (i=0;i<nsf;i++)
202       t[i]=target[i];
203
204    /* Pre-compute codewords response and energy */
205    for (i=0;i<shape_cb_size;i++)
206    {
207       float *res = resp+i*subvect_size;
208
209       /* Compute codeword response */
210       residue_zero(shape_cb+i*subvect_size, awk1, res, subvect_size, p);
211       syn_filt_zero(res, ak, res, subvect_size, p);
212       syn_filt_zero(res, awk2, res, subvect_size,p);
213
214       /* Compute energy of codeword response */
215       E[i]=0;
216       for(j=0;j<subvect_size;j++)
217          E[i]+=res[j]*res[j];
218       
219    }
220
221    for (i=0;i<nb_subvect;i++)
222    {
223       int best_index=0;
224       float g, corr, best_gain=0, score, best_score=-1;
225       /* Find best codeword for current sub-vector */
226       for (j=0;j<shape_cb_size;j++)
227       {
228          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
229          score=corr*corr/(.001+E[j]);
230          g = corr/(.001+E[j]);
231          if (score>best_score)
232          {
233             best_index=j;
234             best_score=score;
235             best_gain=corr/(.001+E[j]);
236          }
237       }
238       frame_bits_pack(bits,best_index,params->shape_bits);
239       
240       /* Quantize gain */
241       {
242          int s=0, best_id;
243          best_gain /= .01+exc_energy;
244          if (best_gain<0)
245          {
246             best_gain=-best_gain;
247             s=1;
248          }
249
250          /* Find gain index (it's a scalar but we use the VQ code anyway)*/
251          best_id = vq_index(&best_gain, scal_gains4, 1, 8);
252
253          best_gain=scal_gains4[best_id];
254          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
255          if (s)
256             best_gain=-best_gain;
257          best_gain *= exc_energy;
258          frame_bits_pack(bits,s,1);
259          frame_bits_pack(bits,best_id,3);
260       }
261       ind[i]=best_index;
262       gains[i]=best_gain;
263
264       for (j=0;j<nsf;j++)
265          e[j]=0;
266       for (j=0;j<subvect_size;j++)
267          e[subvect_size*i+j]=best_gain*shape_cb[best_index*subvect_size+j];
268       residue_zero(e, awk1, r, nsf, p);
269       syn_filt_zero(r, ak, r, nsf, p);
270       syn_filt_zero(r, awk2, r, nsf,p);
271       for (j=0;j<nsf;j++)
272          tresp[i*nsf+j]=r[j];
273       for (j=0;j<nsf;j++)
274          t[j]-=r[j];
275    }
276    
277    /* Put everything back together */
278    for (i=0;i<nb_subvect;i++)
279       for (j=0;j<subvect_size;j++)
280          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
281
282    /* Update excitation */
283    for (j=0;j<nsf;j++)
284       exc[j]+=e[j];
285    
286    /* Update target */
287    residue_zero(e, awk1, r, nsf, p);
288    syn_filt_zero(r, ak, r, nsf, p);
289    syn_filt_zero(r, awk2, r, nsf,p);
290    for (j=0;j<nsf;j++)
291       target[j]-=r[j];
292
293    
294
295
296    POP(stack);
297    POP(stack);
298    POP(stack);
299    POP(stack);
300    POP(stack);
301    POP(stack);
302    POP(stack);
303 }
304
305
306
307
308 void split_cb_unquant(
309 float *exc,
310 void *par,                      /* non-overlapping codebook */
311 int   nsf,                      /* number of samples in subframe */
312 FrameBits *bits,
313 float *stack
314 )
315 {
316    int i,j;
317    int *ind;
318    float *gains;
319    float *sign;
320    float *shape_cb, exc_energy;
321    int shape_cb_size, subvect_size, nb_subvect;
322    split_cb_params *params;
323
324    params = (split_cb_params *) par;
325    subvect_size = params->subvect_size;
326    nb_subvect = params->nb_subvect;
327    shape_cb_size = 1<<params->shape_bits;
328    shape_cb = params->shape_cb;
329    
330    ind = (int*)PUSH(stack, nb_subvect);
331    gains = PUSH(stack, nb_subvect);
332    sign = PUSH(stack, nb_subvect);
333
334    /* Decode global (average) gain */
335    {
336       int id;
337       id = frame_bits_unpack_unsigned(bits, 4);
338       exc_energy=exp(.5*id+2);
339    }
340
341    /* Decode codewords and gains */
342    for (i=0;i<nb_subvect;i++)
343    {
344       int gain_id;
345       ind[i] = frame_bits_unpack_unsigned(bits, params->shape_bits);
346       if (frame_bits_unpack_unsigned(bits, 1))
347          sign[i]=-1;
348       else
349          sign[i]=1;
350       
351       gain_id = frame_bits_unpack_unsigned(bits, 3);
352       gains[i]=scal_gains4[gain_id];
353       gains[i] *= sign[i];
354       gains[i] *= exc_energy;
355
356
357    }
358
359    /* Compute decoded excitation */
360    for (i=0;i<nb_subvect;i++)
361       for (j=0;j<subvect_size;j++)
362          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
363
364    POP(stack);
365    POP(stack);
366    POP(stack);
367 }