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