OK, back to normal (narrowband codec works)
[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 #if 1
224       int best_vq_index=0, max_index;
225       float max_gain=0, log_max, min_dist=0, *sign;
226
227       sign = PUSH(stack, nb_subvect);
228       for (i=0;i<nb_subvect;i++)
229       {
230          if (gains[i]<0)
231          {
232             gains[i]=-gains[i];
233             sign[i]=-1;
234          } else {
235             sign[i]=1;
236          }
237       }
238       for (i=0;i<nb_subvect;i++)
239          if (gains[i]>max_gain)
240             max_gain=gains[i];
241       log_max=log(max_gain+1);
242       max_index = (int)(floor(.5+log_max-3));
243       if (max_index>7)
244          max_index=7;
245       if (max_index<0)
246          max_index=0;
247       max_gain=1/exp(max_index+3.0);
248       for (i=0;i<nb_subvect;i++)
249         gains[i]*=max_gain;
250       frame_bits_pack(bits,max_index,3);
251
252       /*Vector quantize gains[i]*/
253       best_vq_index = vq_index(gains, gain_cb, nb_subvect, gain_cb_size);
254       frame_bits_pack(bits,best_vq_index,params->gain_bits);
255
256       printf ("best_gains_vq_index %d %f %d\n", best_vq_index, min_dist, max_index);
257
258 #if 1 /* If 0, the gains are not quantized */
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 #else 
262       for (i=0;i<nb_subvect;i++)
263          gains[i]= sign[i]*gains[i]/max_gain/(Ee[ind[i]]+.001);
264 #endif  
265     
266
267       POP(stack);
268 #else
269       for (i=0;i<nb_subvect;i++)
270          gains[i]= gains[i]/(Ee[ind[i]]+.001);
271
272 #endif
273       for (i=0;i<nb_subvect;i++)
274          for (j=0;j<subvect_size;j++)
275             exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
276
277    }
278
279    /*TODO: Perform joint optimization of gains*/
280    
281    for (i=0;i<nsf;i++)
282       target[i]=t[i];
283
284    POP(stack);
285    POP(stack);
286    POP(stack);
287    POP(stack);
288    POP(stack);
289    POP(stack);
290    POP(stack);
291    POP(stack);
292 }
293
294 void split_cb_unquant(
295 float *exc,
296 void *par,                      /* non-overlapping codebook */
297 int   nsf,                      /* number of samples in subframe */
298 FrameBits *bits,
299 float *stack
300 )
301 {
302    int i,j;
303    int *ind;
304    float *gains;
305    float *sign;
306    int max_gain_ind, vq_gain_ind;
307    float max_gain, *Ee;
308    float *shape_cb, *gain_cb;
309    int shape_cb_size, gain_cb_size, subvect_size, nb_subvect;
310    split_cb_params *params;
311
312    params = (split_cb_params *) par;
313    subvect_size = params->subvect_size;
314    nb_subvect = params->nb_subvect;
315    shape_cb_size = 1<<params->shape_bits;
316    shape_cb = params->shape_cb;
317    gain_cb_size = 1<<params->gain_bits;
318    gain_cb = params->gain_cb;
319    
320    ind = (int*)PUSH(stack, nb_subvect);
321    gains = PUSH(stack, nb_subvect);
322    sign = PUSH(stack, nb_subvect);
323    Ee=PUSH(stack, nb_subvect);
324
325    for (i=0;i<nb_subvect;i++)
326    {
327       ind[i] = frame_bits_unpack_unsigned(bits, params->shape_bits);
328       if (frame_bits_unpack_unsigned(bits, 1))
329          sign[i]=-1;
330       else
331          sign[i]=1;
332       Ee[i]=.001;
333       for (j=0;j<subvect_size;j++)
334          Ee[i]+=shape_cb[ind[i]*subvect_size+j]*shape_cb[ind[i]*subvect_size+j];
335    }
336    max_gain_ind = frame_bits_unpack_unsigned(bits, 3);
337    vq_gain_ind = frame_bits_unpack_unsigned(bits, params->gain_bits);
338    printf ("unquant gains ind: %d %d\n", max_gain_ind, vq_gain_ind);
339
340    max_gain=exp(max_gain_ind+3.0);
341    for (i=0;i<nb_subvect;i++)
342       gains[i] = sign[i]*gain_cb[vq_gain_ind*nb_subvect+i]*max_gain/Ee[i];
343    
344    printf ("unquant gains: ");
345    for (i=0;i<nb_subvect;i++)
346       printf ("%f ", gains[i]);
347    printf ("\n");
348
349    for (i=0;i<nb_subvect;i++)
350       for (j=0;j<subvect_size;j++)
351          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
352    
353    POP(stack);
354    POP(stack);
355    POP(stack);
356    POP(stack);
357 }