SB-CELP decoder (continued)
[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       int id;
191       q=log(exc_energy+.1);
192       q=floor(.5+2*(q-2));
193       if (q<0)
194          q=0;
195       if (q>15)
196          q=15;
197       id = (int)q;
198       frame_bits_pack(bits, id, 4);
199       exc_energy=exp(.5*q+2);
200    }
201
202    for (i=0;i<nsf;i++)
203       t[i]=target[i];
204    for (i=0;i<shape_cb_size;i++)
205    {
206       float *res = resp+i*subvect_size;
207       residue_zero(shape_cb+i*subvect_size, awk1, res, subvect_size, p);
208       syn_filt_zero(res, ak, res, subvect_size, p);
209       syn_filt_zero(res, awk2, res, subvect_size,p);
210       E[i]=0;
211       for(j=0;j<subvect_size;j++)
212          E[i]+=res[j]*res[j];
213       Ee[i]=0;
214       for(j=0;j<subvect_size;j++)
215          Ee[i]+=shape_cb[i*subvect_size+j]*shape_cb[i*subvect_size+j];
216       
217    }
218
219    for (i=0;i<nb_subvect;i++)
220    {
221       int best_index=0;
222       float g, corr, best_gain=0, score, best_score=-1;
223       for (j=0;j<shape_cb_size;j++)
224       {
225          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
226          score=corr*corr/(.001+E[j]);
227          g = corr/(.001+E[j]);
228          if (score>best_score)
229          {
230             best_index=j;
231             best_score=score;
232             best_gain=corr/(.001+E[j]);
233          }
234       }
235       frame_bits_pack(bits,best_index,params->shape_bits);
236       {
237          int s=0, best_id, j;
238          float best_dist;
239          best_gain /= .01+exc_energy;
240          if (best_gain<0)
241          {
242             best_gain=-best_gain;
243             s=1;
244          }
245          best_dist=(best_gain-scal_gains4[0])*(best_gain-scal_gains4[0]);
246          best_id=0;
247          for (j=1;j<8;j++)
248          {
249             float dist;
250             dist=(best_gain-scal_gains4[j])*(best_gain-scal_gains4[j]);
251             if (dist<best_dist)
252             {
253                best_id=j;
254                best_dist=dist;
255             }
256          }
257          best_gain=scal_gains4[best_id];
258          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
259          if (s)
260             best_gain=-best_gain;
261          best_gain *= exc_energy;
262          frame_bits_pack(bits,s,1);
263          frame_bits_pack(bits,best_id,3);
264       }
265       ind[i]=best_index;
266       gains[i]=best_gain;
267
268       for (j=0;j<nsf;j++)
269          e[j]=0;
270       for (j=0;j<subvect_size;j++)
271          e[subvect_size*i+j]=best_gain*shape_cb[best_index*subvect_size+j];
272       residue_zero(e, awk1, r, nsf, p);
273       syn_filt_zero(r, ak, r, nsf, p);
274       syn_filt_zero(r, awk2, r, nsf,p);
275       for (j=0;j<nsf;j++)
276          tresp[i*nsf+j]=r[j];
277       for (j=0;j<nsf;j++)
278          t[j]-=r[j];
279    }
280    
281    for (i=0;i<nb_subvect;i++)
282       for (j=0;j<subvect_size;j++)
283          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
284
285    for (j=0;j<nsf;j++)
286       exc[j]+=e[j];
287    residue_zero(e, awk1, r, nsf, p);
288    syn_filt_zero(r, ak, r, nsf, p);
289    syn_filt_zero(r, awk2, r, nsf,p);
290    for (j=0;j<nsf;j++)
291       target[j]-=r[j];
292
293    
294
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
307
308
309 void split_cb_unquant(
310 float *exc,
311 void *par,                      /* non-overlapping codebook */
312 int   nsf,                      /* number of samples in subframe */
313 FrameBits *bits,
314 float *stack
315 )
316 {
317    int i,j;
318    int *ind;
319    float *gains;
320    float *sign;
321    float *shape_cb, exc_energy;
322    int shape_cb_size, subvect_size, nb_subvect;
323    split_cb_params *params;
324
325    params = (split_cb_params *) par;
326    subvect_size = params->subvect_size;
327    nb_subvect = params->nb_subvect;
328    shape_cb_size = 1<<params->shape_bits;
329    shape_cb = params->shape_cb;
330    
331    ind = (int*)PUSH(stack, nb_subvect);
332    gains = PUSH(stack, nb_subvect);
333    sign = PUSH(stack, nb_subvect);
334
335    /* Decode global (average) gain */
336    {
337       int id;
338       id = frame_bits_unpack_unsigned(bits, 4);
339       exc_energy=exp(.5*id+2);
340    }
341
342    for (i=0;i<nb_subvect;i++)
343    {
344       int gain_id;
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       
351       gain_id = frame_bits_unpack_unsigned(bits, 3);
352       gains[i]=scal_gains4[gain_id];
353       gains[i] *= sign[i];
354       gains[i] *= exc_energy;
355    }
356
357    for (i=0;i<nb_subvect;i++)
358       for (j=0;j<subvect_size;j++)
359          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
360    
361    POP(stack);
362    POP(stack);
363    POP(stack);
364 }