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