bc0d92513c43141459cab6de0b5c2dafd68d2984
[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 #define EXC_CB_SIZE 128
41 #define min(a,b) ((a) < (b) ? (a) : (b))
42
43 /*---------------------------------------------------------------------------*\
44                                                                              
45  void overlap_cb_search()                                                             
46                                                                               
47  Searches a gain/shape codebook consisting of overlapping entries for the    
48  closest vector to the target.  Gives identical results to search() above   
49  buts uses fast end correction algorithm for the synthesis of response       
50  vectors.                                                                     
51                                                                              
52 \*---------------------------------------------------------------------------*/
53
54 float overlap_cb_search(
55 float target[],                 /* target vector */
56 float ak[],                     /* LPCs for this subframe */
57 float awk1[],                   /* Weighted LPCs for this subframe */
58 float awk2[],                   /* Weighted LPCs for this subframe */
59 float codebook[],               /* overlapping codebook */
60 int   entries,                  /* number of overlapping entries to search */
61 float *gain,                    /* gain of optimum entry */
62 int   *index,                   /* index of optimum entry */
63 int   p,                        /* number of LPC coeffs */
64 int   nsf                       /* number of samples in subframe */
65 )
66 {
67   float *resp;                  /* zero state response to current entry */
68   float *h;                     /* impulse response of synthesis filter */
69   float *impulse;               /* excitation vector containing one impulse */
70   float d,e,g,score;            /* codebook searching variables */
71   float bscore;                 /* score of "best" vector so far */
72   int i,k;                      /* loop variables */
73
74   /* Initialise */
75   
76   resp = (float*)malloc(sizeof(float)*nsf);
77   h = (float*)malloc(sizeof(float)*nsf);
78   impulse = (float*)malloc(sizeof(float)*nsf);
79
80   for(i=0; i<nsf; i++)
81     impulse[i] = 0.0;
82    
83   *gain = 0.0;
84   *index = 0;
85   bscore = 0.0;
86   impulse[0] = 1.0;
87
88   /* Calculate impulse response of  A(z/g2) / ( A(z)*(z/g1) ) */
89   residue_zero(impulse, awk1, h, nsf, p);
90   syn_filt_zero(h, ak, h, nsf, p);
91   syn_filt_zero(h, awk2, h, nsf,p);
92   
93   /* Calculate codebook zero-response */
94   residue_zero(&codebook[entries-1],awk1,resp,nsf,p);
95   syn_filt_zero(resp,ak,resp,nsf,p);
96   syn_filt_zero(resp,awk2,resp,nsf,p);
97     
98   /* Search codebook backwards using end correction for synthesis */
99   
100   for(k=entries-1; k>=0; k--) {
101
102     d = 0.0; e = 0.0;
103     for(i=0; i<nsf; i++) {
104       d += target[i]*resp[i];
105       e += resp[i]*resp[i];
106     }
107     g = d/(e+.1);
108     score = g*d;
109     /*printf ("score: %f %f %f %f\n", target[0],d,e,score);*/
110     if (score >= bscore) {
111       bscore = score;
112       *gain = g;
113       *index = k;
114     }
115     
116     /* Synthesise next entry */
117     
118     if (k) {
119       for(i=nsf-1; i>=1; i--)
120         resp[i] = resp[i-1] + codebook[k-1]*h[i];
121       resp[0] = codebook[k-1]*h[0];
122     }
123   }
124
125   free(resp);
126   free(h);
127   free(impulse);
128   return bscore;
129 }
130
131
132
133
134 void split_cb_search(
135 float target[],                 /* target vector */
136 float ak[],                     /* LPCs for this subframe */
137 float awk1[],                   /* Weighted LPCs for this subframe */
138 float awk2[],                   /* Weighted LPCs for this subframe */
139 void *par,                      /* Codebook/search parameters*/
140 int   p,                        /* number of LPC coeffs */
141 int   nsf,                      /* number of samples in subframe */
142 float *exc,
143 FrameBits *bits,
144 float *stack
145 )
146 {
147    int i,j;
148    float *resp, *E, *Ee;
149    float *t, *r, *e;
150    float *gains;
151    int *ind;
152    float *shape_cb, *gain_cb;
153    int shape_cb_size, gain_cb_size, subvect_size, nb_subvect;
154    split_cb_params *params;
155
156    params = (split_cb_params *) par;
157    subvect_size = params->subvect_size;
158    nb_subvect = params->nb_subvect;
159    shape_cb_size = 1<<params->shape_bits;
160    shape_cb = params->shape_cb;
161    gain_cb_size = 1<<params->gain_bits;
162    gain_cb = params->gain_cb;
163    resp = PUSH(stack, shape_cb_size*subvect_size);
164    E = PUSH(stack, shape_cb_size);
165    Ee = PUSH(stack, shape_cb_size);
166    t = PUSH(stack, nsf);
167    r = PUSH(stack, nsf);
168    e = PUSH(stack, nsf);
169    gains = PUSH(stack, nb_subvect);
170    ind = (int*)PUSH(stack, nb_subvect);
171    
172
173    for (i=0;i<nsf;i++)
174       t[i]=target[i];
175    for (i=0;i<shape_cb_size;i++)
176    {
177       float *res = resp+i*subvect_size;
178       residue_zero(shape_cb+i*subvect_size, awk1, res, subvect_size, p);
179       syn_filt_zero(res, ak, res, subvect_size, p);
180       syn_filt_zero(res, awk2, res, subvect_size,p);
181       E[i]=0;
182       for(j=0;j<subvect_size;j++)
183          E[i]+=res[j]*res[j];
184       Ee[i]=0;
185       for(j=0;j<subvect_size;j++)
186          Ee[i]+=shape_cb[i*subvect_size+j]*shape_cb[i*subvect_size+j];
187       
188    }
189    for (i=0;i<nb_subvect;i++)
190    {
191       int best_index=0;
192       float g, corr, best_gain=0, score, best_score=-1;
193       for (j=0;j<shape_cb_size;j++)
194       {
195          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
196          score=corr*corr/(.001+E[j]);
197          g = corr/(.001+E[j]);
198          if (score>best_score)
199          {
200             best_index=j;
201             best_score=score;
202             best_gain=corr/(.001+E[j]);
203          }
204       }
205       frame_bits_pack(bits,best_index,params->shape_bits);
206       if (best_gain>0)
207          frame_bits_pack(bits,0,1);
208       else
209           frame_bits_pack(bits,1,1);        
210       ind[i]=best_index;
211       gains[i]=best_gain*Ee[ind[i]];
212
213       for (j=0;j<nsf;j++)
214          e[j]=0;
215       for (j=0;j<subvect_size;j++)
216          e[subvect_size*i+j]=best_gain*shape_cb[best_index*subvect_size+j];
217       residue_zero(e, awk1, r, nsf, p);
218       syn_filt_zero(r, ak, r, nsf, p);
219       syn_filt_zero(r, awk2, r, nsf,p);
220       for (j=0;j<nsf;j++)
221          t[j]-=r[j];
222    }
223
224    {
225       int best_vq_index=0, max_index;
226       float max_gain=0, log_max, min_dist=0, *sign;
227
228       sign = PUSH(stack, nb_subvect);
229       for (i=0;i<nb_subvect;i++)
230       {
231          if (gains[i]<0)
232          {
233             gains[i]=-gains[i];
234             sign[i]=-1;
235          } else {
236             sign[i]=1;
237          }
238       }
239       for (i=0;i<nb_subvect;i++)
240          if (gains[i]>max_gain)
241             max_gain=gains[i];
242       log_max=log(max_gain+1);
243       max_index = (int)(floor(.5+log_max-3));
244       if (max_index>7)
245          max_index=7;
246       if (max_index<0)
247          max_index=0;
248       max_gain=1/exp(max_index+3.0);
249       for (i=0;i<nb_subvect;i++)
250         gains[i]*=max_gain;
251       frame_bits_pack(bits,max_index,3);
252
253       /*Vector quantize gains[i]*/
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
257       printf ("best_gains_vq_index %d %f %d\n", best_vq_index, min_dist, max_index);
258
259 #if 1 /* If 0, the gains are not quantized */
260       for (i=0;i<nb_subvect;i++)
261          gains[i]= sign[i]*gain_cb[best_vq_index*nb_subvect+i]/max_gain/(Ee[ind[i]]+.001);
262 #else 
263       for (i=0;i<nb_subvect;i++)
264          gains[i]= sign[i]*gains[i]/max_gain/(Ee[ind[i]]+.001);
265 #endif  
266     
267       for (i=0;i<nb_subvect;i++)
268          for (j=0;j<subvect_size;j++)
269             exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
270
271       POP(stack);
272    }
273
274    /*TODO: Perform joint optimization of gains*/
275    
276    for (i=0;i<nsf;i++)
277       target[i]=t[i];
278
279    POP(stack);
280    POP(stack);
281    POP(stack);
282    POP(stack);
283    POP(stack);
284    POP(stack);
285    POP(stack);
286    POP(stack);
287 }
288
289 void split_cb_unquant(
290 float *exc,
291 void *par,                      /* non-overlapping codebook */
292 int   nsf,                      /* number of samples in subframe */
293 FrameBits *bits,
294 float *stack
295 )
296 {
297    int i,j;
298    int *ind;
299    float *gains;
300    float *sign;
301    int max_gain_ind, vq_gain_ind;
302    float max_gain, *Ee;
303    float *shape_cb, *gain_cb;
304    int shape_cb_size, gain_cb_size, subvect_size, nb_subvect;
305    split_cb_params *params;
306
307    params = (split_cb_params *) par;
308    subvect_size = params->subvect_size;
309    nb_subvect = params->nb_subvect;
310    shape_cb_size = 1<<params->shape_bits;
311    shape_cb = params->shape_cb;
312    gain_cb_size = 1<<params->gain_bits;
313    gain_cb = params->gain_cb;
314    
315    ind = (int*)PUSH(stack, nb_subvect);
316    gains = PUSH(stack, nb_subvect);
317    sign = PUSH(stack, nb_subvect);
318    Ee=PUSH(stack, nb_subvect);
319
320    for (i=0;i<nb_subvect;i++)
321    {
322       ind[i] = frame_bits_unpack_unsigned(bits, params->shape_bits);
323       if (frame_bits_unpack_unsigned(bits, 1))
324          sign[i]=-1;
325       else
326          sign[i]=1;
327       Ee[i]=.001;
328       for (j=0;j<subvect_size;j++)
329          Ee[i]+=shape_cb[ind[i]*subvect_size+j]*shape_cb[ind[i]*subvect_size+j];
330    }
331    max_gain_ind = frame_bits_unpack_unsigned(bits, 3);
332    vq_gain_ind = frame_bits_unpack_unsigned(bits, params->gain_bits);
333    printf ("unquant gains ind: %d %d\n", max_gain_ind, vq_gain_ind);
334
335    max_gain=exp(max_gain_ind+3.0);
336    for (i=0;i<nb_subvect;i++)
337       gains[i] = sign[i]*gain_cb[vq_gain_ind*nb_subvect+i]*max_gain/Ee[i];
338    
339    printf ("unquant gains: ");
340    for (i=0;i<nb_subvect;i++)
341       printf ("%f ", gains[i]);
342    printf ("\n");
343
344    for (i=0;i<nb_subvect;i++)
345       for (j=0;j<subvect_size;j++)
346          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
347    
348    POP(stack);
349    POP(stack);
350    POP(stack);
351    POP(stack);
352 }