887124cba03972d2c290cd287605a7d6a494ab03
[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;
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    E = PUSH(stack, shape_cb_size);
174    t = PUSH(stack, nsf);
175    r = PUSH(stack, nsf);
176    e = PUSH(stack, nsf);
177    gains = PUSH(stack, nb_subvect);
178    ind = (int*)PUSH(stack, nb_subvect);
179
180    /* Compute energy of the "real excitation" */
181    syn_filt_zero(target, awk1, e, nsf, p);
182    residue_zero(e, ak, e, nsf, p);
183    residue_zero(e, awk2, e, nsf, p);
184    for (i=0;i<nsf;i++)
185       exc_energy += e[i]*e[i];
186    exc_energy=sqrt(exc_energy/nb_subvect);
187
188    /* Quantize global ("average") gain */
189    q=log(exc_energy+.1);
190    q=floor(.5+2*(q-2));
191    if (q<0)
192       q=0;
193    if (q>15)
194       q=15;
195    id = (int)q;
196    frame_bits_pack(bits, id, 4);
197    exc_energy=exp(.5*q+2);
198
199
200    for (i=0;i<nsf;i++)
201       t[i]=target[i];
202
203    e[0]=1;
204    for (i=1;i<nsf;i++)
205       e[i]=0;
206    residue_zero(e, awk1, r, nsf, p);
207    syn_filt_zero(r, ak, r, nsf, p);
208    syn_filt_zero(r, awk2, r, nsf,p);
209    
210    /* Pre-compute codewords response and energy */
211    for (i=0;i<shape_cb_size;i++)
212    {
213       float *res = resp+i*subvect_size;
214
215       /* Compute codeword response */
216 #if 0
217       residue_zero(shape_cb+i*subvect_size, awk1, res, subvect_size, p);
218       syn_filt_zero(res, ak, res, subvect_size, p);
219       syn_filt_zero(res, awk2, res, subvect_size,p);
220 #else
221       int k;
222       for(j=0;j<subvect_size;j++)
223          res[j]=0;
224       for(j=0;j<subvect_size;j++)
225       {
226          for (k=j;k<subvect_size;k++)
227             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
228       }
229 #endif
230       /* Compute energy of codeword response */
231       E[i]=0;
232       for(j=0;j<subvect_size;j++)
233          E[i]+=res[j]*res[j];
234       
235    }
236
237    for (i=0;i<nb_subvect;i++)
238    {
239       int best_index=0;
240       float g, corr, best_gain=0, score, best_score=-1;
241       /* Find best codeword for current sub-vector */
242       for (j=0;j<shape_cb_size;j++)
243       {
244          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
245          score=corr*corr/(.001+E[j]);
246          g = corr/(.001+E[j]);
247          if (score>best_score)
248          {
249             best_index=j;
250             best_score=score;
251             best_gain=corr/(.001+E[j]);
252          }
253       }
254       frame_bits_pack(bits,best_index,params->shape_bits);
255       
256       /* Quantize gain */
257       {
258          int s=0, best_id;
259          best_gain /= .01+exc_energy;
260          if (best_gain<0)
261          {
262             best_gain=-best_gain;
263             s=1;
264          }
265
266          /* Find gain index (it's a scalar but we use the VQ code anyway)*/
267          best_id = vq_index(&best_gain, scal_gains4, 1, 8);
268
269          best_gain=scal_gains4[best_id];
270          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
271          if (s)
272             best_gain=-best_gain;
273          best_gain *= exc_energy;
274          frame_bits_pack(bits,s,1);
275          frame_bits_pack(bits,best_id,3);
276       }
277       ind[i]=best_index;
278       gains[i]=best_gain;
279 #if 0
280       for (j=0;j<nsf;j++)
281          e[j]=0;
282       for (j=0;j<subvect_size;j++)
283          e[subvect_size*i+j]=best_gain*shape_cb[best_index*subvect_size+j];
284       residue_zero(e, awk1, r, nsf, p);
285       syn_filt_zero(r, ak, r, nsf, p);
286       syn_filt_zero(r, awk2, r, nsf,p);
287       for (j=0;j<nsf;j++)
288          t[j]-=r[j];
289 #else
290       {
291          int k,m;
292          float g;
293          for (j=0;j<subvect_size;j++)
294          {
295             g=best_gain*shape_cb[best_index*subvect_size+j];
296             for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
297                t[k] -= g*r[m];
298          }
299       }
300 #endif
301    }
302    
303    /* Put everything back together */
304    for (i=0;i<nb_subvect;i++)
305       for (j=0;j<subvect_size;j++)
306          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
307
308    /* Update excitation */
309    for (j=0;j<nsf;j++)
310       exc[j]+=e[j];
311    
312    /* Update target */
313    residue_zero(e, awk1, r, nsf, p);
314    syn_filt_zero(r, ak, r, nsf, p);
315    syn_filt_zero(r, awk2, r, nsf,p);
316    for (j=0;j<nsf;j++)
317       target[j]-=r[j];
318
319    
320
321
322    POP(stack);
323    POP(stack);
324    POP(stack);
325    POP(stack);
326    POP(stack);
327    POP(stack);
328 }
329
330
331
332
333 void split_cb_unquant(
334 float *exc,
335 void *par,                      /* non-overlapping codebook */
336 int   nsf,                      /* number of samples in subframe */
337 FrameBits *bits,
338 float *stack
339 )
340 {
341    int i,j;
342    int *ind;
343    float *gains;
344    float *sign;
345    float *shape_cb, exc_energy;
346    int shape_cb_size, subvect_size, nb_subvect;
347    split_cb_params *params;
348
349    params = (split_cb_params *) par;
350    subvect_size = params->subvect_size;
351    nb_subvect = params->nb_subvect;
352    shape_cb_size = 1<<params->shape_bits;
353    shape_cb = params->shape_cb;
354    
355    ind = (int*)PUSH(stack, nb_subvect);
356    gains = PUSH(stack, nb_subvect);
357    sign = PUSH(stack, nb_subvect);
358
359    /* Decode global (average) gain */
360    {
361       int id;
362       id = frame_bits_unpack_unsigned(bits, 4);
363       exc_energy=exp(.5*id+2);
364    }
365
366    /* Decode codewords and gains */
367    for (i=0;i<nb_subvect;i++)
368    {
369       int gain_id;
370       ind[i] = frame_bits_unpack_unsigned(bits, params->shape_bits);
371       if (frame_bits_unpack_unsigned(bits, 1))
372          sign[i]=-1;
373       else
374          sign[i]=1;
375       
376       gain_id = frame_bits_unpack_unsigned(bits, 3);
377       gains[i]=scal_gains4[gain_id];
378       gains[i] *= sign[i];
379       gains[i] *= exc_energy;
380
381
382    }
383
384    /* Compute decoded excitation */
385    for (i=0;i<nb_subvect;i++)
386       for (j=0;j<subvect_size;j++)
387          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
388
389    POP(stack);
390    POP(stack);
391    POP(stack);
392 }