We're going wideband...
[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
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
131
132 void split_cb_search(
133 float target[],                 /* target vector */
134 float ak[],                     /* LPCs for this subframe */
135 float awk1[],                   /* Weighted LPCs for this subframe */
136 float awk2[],                   /* Weighted LPCs for this subframe */
137 void *par,                      /* Codebook/search parameters*/
138 int   p,                        /* number of LPC coeffs */
139 int   nsf,                      /* number of samples in subframe */
140 float *exc,
141 FrameBits *bits,
142 float *stack
143 )
144 {
145    int i,j;
146    float *resp, *E, *Ee;
147    float *t, *r, *e;
148    float *gains;
149    int *ind;
150    float *shape_cb, *gain_cb;
151    int shape_cb_size, gain_cb_size, subvect_size, nb_subvect;
152    split_cb_params *params;
153
154    params = (split_cb_params *) par;
155    subvect_size = params->subvect_size;
156    nb_subvect = params->nb_subvect;
157    shape_cb_size = 1<<params->shape_bits;
158    shape_cb = params->shape_cb;
159    gain_cb_size = 1<<params->gain_bits;
160    gain_cb = params->gain_cb;
161    resp = PUSH(stack, shape_cb_size*subvect_size);
162    E = PUSH(stack, shape_cb_size);
163    Ee = PUSH(stack, shape_cb_size);
164    t = PUSH(stack, nsf);
165    r = PUSH(stack, nsf);
166    e = PUSH(stack, nsf);
167    gains = PUSH(stack, nb_subvect);
168    ind = (int*)PUSH(stack, nb_subvect);
169    
170
171    for (i=0;i<nsf;i++)
172       t[i]=target[i];
173    for (i=0;i<shape_cb_size;i++)
174    {
175       float *res = resp+i*subvect_size;
176       residue_zero(shape_cb+i*subvect_size, awk1, res, subvect_size, p);
177       syn_filt_zero(res, ak, res, subvect_size, p);
178       syn_filt_zero(res, awk2, res, subvect_size,p);
179       E[i]=0;
180       for(j=0;j<subvect_size;j++)
181          E[i]+=res[j]*res[j];
182       Ee[i]=0;
183       for(j=0;j<subvect_size;j++)
184          Ee[i]+=shape_cb[i*subvect_size+j]*shape_cb[i*subvect_size+j];
185       
186    }
187    for (i=0;i<nb_subvect;i++)
188    {
189       int best_index=0;
190       float g, corr, best_gain=0, score, best_score=-1;
191       for (j=0;j<shape_cb_size;j++)
192       {
193          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
194          score=corr*corr/(.001+E[j]);
195          g = corr/(.001+E[j]);
196          if (score>best_score)
197          {
198             best_index=j;
199             best_score=score;
200             best_gain=corr/(.001+E[j]);
201          }
202       }
203       frame_bits_pack(bits,best_index,params->shape_bits);
204       if (best_gain>0)
205          frame_bits_pack(bits,0,1);
206       else
207           frame_bits_pack(bits,1,1);        
208       ind[i]=best_index;
209       gains[i]=best_gain*Ee[ind[i]];
210
211       for (j=0;j<nsf;j++)
212          e[j]=0;
213       for (j=0;j<subvect_size;j++)
214          e[subvect_size*i+j]=best_gain*shape_cb[best_index*subvect_size+j];
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       for (j=0;j<nsf;j++)
219          t[j]-=r[j];
220    }
221
222    {
223       int best_vq_index=0, max_index;
224       float max_gain=0, log_max, min_dist=0, *sign;
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       best_vq_index = vq_index(gains, gain_cb, nb_subvect, gain_cb_size);
253       frame_bits_pack(bits,best_vq_index,params->gain_bits);
254
255       printf ("best_gains_vq_index %d %f %d\n", best_vq_index, min_dist, max_index);
256
257 #if 1 /* If 0, the gains are not quantized */
258       for (i=0;i<nb_subvect;i++)
259          gains[i]= sign[i]*gain_cb[best_vq_index*nb_subvect+i]/max_gain/(Ee[ind[i]]+.001);
260 #else 
261       for (i=0;i<nb_subvect;i++)
262          gains[i]= sign[i]*gains[i]/max_gain/(Ee[ind[i]]+.001);
263 #endif  
264     
265       for (i=0;i<nb_subvect;i++)
266          for (j=0;j<subvect_size;j++)
267             exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
268
269       POP(stack);
270    }
271
272    /*TODO: Perform joint optimization of gains*/
273    
274    for (i=0;i<nsf;i++)
275       target[i]=t[i];
276
277    POP(stack);
278    POP(stack);
279    POP(stack);
280    POP(stack);
281    POP(stack);
282    POP(stack);
283    POP(stack);
284    POP(stack);
285 }
286
287 void split_cb_unquant(
288 float *exc,
289 void *par,                      /* non-overlapping codebook */
290 int   nsf,                      /* number of samples in subframe */
291 FrameBits *bits,
292 float *stack
293 )
294 {
295    int i,j;
296    int *ind;
297    float *gains;
298    float *sign;
299    int max_gain_ind, vq_gain_ind;
300    float max_gain, *Ee;
301    float *shape_cb, *gain_cb;
302    int shape_cb_size, gain_cb_size, subvect_size, nb_subvect;
303    split_cb_params *params;
304
305    params = (split_cb_params *) par;
306    subvect_size = params->subvect_size;
307    nb_subvect = params->nb_subvect;
308    shape_cb_size = 1<<params->shape_bits;
309    shape_cb = params->shape_cb;
310    gain_cb_size = 1<<params->gain_bits;
311    gain_cb = params->gain_cb;
312    
313    ind = (int*)PUSH(stack, nb_subvect);
314    gains = PUSH(stack, nb_subvect);
315    sign = PUSH(stack, nb_subvect);
316    Ee=PUSH(stack, nb_subvect);
317
318    for (i=0;i<nb_subvect;i++)
319    {
320       ind[i] = frame_bits_unpack_unsigned(bits, params->shape_bits);
321       if (frame_bits_unpack_unsigned(bits, 1))
322          sign[i]=-1;
323       else
324          sign[i]=1;
325       Ee[i]=.001;
326       for (j=0;j<subvect_size;j++)
327          Ee[i]+=shape_cb[ind[i]*subvect_size+j]*shape_cb[ind[i]*subvect_size+j];
328    }
329    max_gain_ind = frame_bits_unpack_unsigned(bits, 3);
330    vq_gain_ind = frame_bits_unpack_unsigned(bits, params->gain_bits);
331    printf ("unquant gains ind: %d %d\n", max_gain_ind, vq_gain_ind);
332
333    max_gain=exp(max_gain_ind+3.0);
334    for (i=0;i<nb_subvect;i++)
335       gains[i] = sign[i]*gain_cb[vq_gain_ind*nb_subvect+i]*max_gain/Ee[i];
336    
337    printf ("unquant gains: ");
338    for (i=0;i<nb_subvect;i++)
339       printf ("%f ", gains[i]);
340    printf ("\n");
341
342    for (i=0;i<nb_subvect;i++)
343       for (j=0;j<subvect_size;j++)
344          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
345    
346    POP(stack);
347    POP(stack);
348    POP(stack);
349    POP(stack);
350 }