2b4db2ca29f88c264099be3a529d8a4384fbcf7f
[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       if (gain_cb) /*If no gain codebok, do not quantize (for testing/debugging) */
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          for (i=0;i<nb_subvect;i++)
260             gains[i]= sign[i]*gain_cb[best_vq_index*nb_subvect+i]/max_gain/(Ee[ind[i]]+.001);
261     
262
263          POP(stack);
264       } else {
265
266          for (i=0;i<nb_subvect;i++)
267             gains[i]= gains[i]/(Ee[ind[i]]+.001);
268       }
269
270       for (i=0;i<nb_subvect;i++)
271          for (j=0;j<subvect_size;j++)
272             exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
273
274    }
275
276    /*TODO: Perform joint optimization of gains*/
277    
278    for (i=0;i<nsf;i++)
279       target[i]=t[i];
280
281    POP(stack);
282    POP(stack);
283    POP(stack);
284    POP(stack);
285    POP(stack);
286    POP(stack);
287    POP(stack);
288    POP(stack);
289 }
290
291 void split_cb_unquant(
292 float *exc,
293 void *par,                      /* non-overlapping codebook */
294 int   nsf,                      /* number of samples in subframe */
295 FrameBits *bits,
296 float *stack
297 )
298 {
299    int i,j;
300    int *ind;
301    float *gains;
302    float *sign;
303    int max_gain_ind, vq_gain_ind;
304    float max_gain, *Ee;
305    float *shape_cb, *gain_cb;
306    int shape_cb_size, gain_cb_size, subvect_size, nb_subvect;
307    split_cb_params *params;
308
309    params = (split_cb_params *) par;
310    subvect_size = params->subvect_size;
311    nb_subvect = params->nb_subvect;
312    shape_cb_size = 1<<params->shape_bits;
313    shape_cb = params->shape_cb;
314    gain_cb_size = 1<<params->gain_bits;
315    gain_cb = params->gain_cb;
316    
317    ind = (int*)PUSH(stack, nb_subvect);
318    gains = PUSH(stack, nb_subvect);
319    sign = PUSH(stack, nb_subvect);
320    Ee=PUSH(stack, nb_subvect);
321
322    for (i=0;i<nb_subvect;i++)
323    {
324       ind[i] = frame_bits_unpack_unsigned(bits, params->shape_bits);
325       if (frame_bits_unpack_unsigned(bits, 1))
326          sign[i]=-1;
327       else
328          sign[i]=1;
329       Ee[i]=.001;
330       for (j=0;j<subvect_size;j++)
331          Ee[i]+=shape_cb[ind[i]*subvect_size+j]*shape_cb[ind[i]*subvect_size+j];
332    }
333    max_gain_ind = frame_bits_unpack_unsigned(bits, 3);
334    vq_gain_ind = frame_bits_unpack_unsigned(bits, params->gain_bits);
335    printf ("unquant gains ind: %d %d\n", max_gain_ind, vq_gain_ind);
336
337    max_gain=exp(max_gain_ind+3.0);
338    for (i=0;i<nb_subvect;i++)
339       gains[i] = sign[i]*gain_cb[vq_gain_ind*nb_subvect+i]*max_gain/Ee[i];
340    
341    printf ("unquant gains: ");
342    for (i=0;i<nb_subvect;i++)
343       printf ("%f ", gains[i]);
344    printf ("\n");
345
346    for (i=0;i<nb_subvect;i++)
347       for (j=0;j<subvect_size;j++)
348          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
349    
350    POP(stack);
351    POP(stack);
352    POP(stack);
353    POP(stack);
354 }