47d96410ee55ddbf5d45da3b44d9d3705694fcdf
[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
38 #define EXC_CB_SIZE 128
39 #define min(a,b) ((a) < (b) ? (a) : (b))
40 extern float exc_gains_table[128][5];
41
42 /*---------------------------------------------------------------------------*\
43                                                                              
44  void overlap_cb_search()                                                             
45                                                                               
46  Searches a gain/shape codebook consisting of overlapping entries for the    
47  closest vector to the target.  Gives identical results to search() above   
48  buts uses fast end correction algorithm for the synthesis of response       
49  vectors.                                                                     
50                                                                              
51 \*---------------------------------------------------------------------------*/
52
53 float overlap_cb_search(
54 float target[],                 /* target vector */
55 float ak[],                     /* LPCs for this subframe */
56 float awk1[],                   /* Weighted LPCs for this subframe */
57 float awk2[],                   /* Weighted LPCs for this subframe */
58 float codebook[],               /* overlapping codebook */
59 int   entries,                  /* number of overlapping entries to search */
60 float *gain,                    /* gain of optimum entry */
61 int   *index,                   /* index of optimum entry */
62 int   p,                        /* number of LPC coeffs */
63 int   nsf                       /* number of samples in subframe */
64 )
65 {
66   float *resp;                  /* zero state response to current entry */
67   float *h;                     /* impulse response of synthesis filter */
68   float *impulse;               /* excitation vector containing one impulse */
69   float d,e,g,score;            /* codebook searching variables */
70   float bscore;                 /* score of "best" vector so far */
71   int i,k;                      /* loop variables */
72
73   /* Initialise */
74   
75   resp = (float*)malloc(sizeof(float)*nsf);
76   h = (float*)malloc(sizeof(float)*nsf);
77   impulse = (float*)malloc(sizeof(float)*nsf);
78
79   for(i=0; i<nsf; i++)
80     impulse[i] = 0.0;
81    
82   *gain = 0.0;
83   *index = 0;
84   bscore = 0.0;
85   impulse[0] = 1.0;
86
87   /* Calculate impulse response of  A(z/g2) / ( A(z)*(z/g1) ) */
88   residue_zero(impulse, awk1, h, nsf, p);
89   syn_filt_zero(h, ak, h, nsf, p);
90   syn_filt_zero(h, awk2, h, nsf,p);
91   
92   /* Calculate codebook zero-response */
93   residue_zero(&codebook[entries-1],awk1,resp,nsf,p);
94   syn_filt_zero(resp,ak,resp,nsf,p);
95   syn_filt_zero(resp,awk2,resp,nsf,p);
96     
97   /* Search codebook backwards using end correction for synthesis */
98   
99   for(k=entries-1; k>=0; k--) {
100
101     d = 0.0; e = 0.0;
102     for(i=0; i<nsf; i++) {
103       d += target[i]*resp[i];
104       e += resp[i]*resp[i];
105     }
106     g = d/(e+.1);
107     score = g*d;
108     /*printf ("score: %f %f %f %f\n", target[0],d,e,score);*/
109     if (score >= bscore) {
110       bscore = score;
111       *gain = g;
112       *index = k;
113     }
114     
115     /* Synthesise next entry */
116     
117     if (k) {
118       for(i=nsf-1; i>=1; i--)
119         resp[i] = resp[i-1] + codebook[k-1]*h[i];
120       resp[0] = codebook[k-1]*h[0];
121     }
122   }
123
124   free(resp);
125   free(h);
126   free(impulse);
127   return bscore;
128 }
129
130
131
132
133
134 void split_cb_search(
135 float target[],                 /* target vector */
136 float ak[],                     /* LPCs for this subframe */
137 float awk1[],                   /* Weighted LPCs for this subframe */
138 float awk2[],                   /* Weighted LPCs for this subframe */
139 float codebook[][8],            /* non-overlapping codebook */
140 int   entries,                  /* number of entries to search */
141 float *gain,                    /* gain of optimum entries */
142 int   *index,                   /* index of optimum entries */
143 int   p,                        /* number of LPC coeffs */
144 int   nsf,                      /* number of samples in subframe */
145 float *exc,
146 FrameBits *bits
147 )
148 {
149    int i,j;
150    float resp[EXC_CB_SIZE][8], E[EXC_CB_SIZE], Ee[EXC_CB_SIZE];
151    float t[40], r[40], e[40];
152    float gains[5];
153    int ind[5];
154    for (i=0;i<40;i++)
155       t[i]=target[i];
156    for (i=0;i<EXC_CB_SIZE;i++)
157    {
158       residue_zero(codebook[i], awk1, resp[i], 8, p);
159       syn_filt_zero(resp[i], ak, resp[i], 8, p);
160       syn_filt_zero(resp[i], awk2, resp[i], 8,p);
161       E[i]=0;
162       for(j=0;j<8;j++)
163          E[i]+=resp[i][j]*resp[i][j];
164       Ee[i]=0;
165       for(j=0;j<8;j++)
166          Ee[i]+=codebook[i][j]*codebook[i][j];
167       
168    }
169    for (i=0;i<5;i++)
170    {
171       int best_index=0;
172       float g, corr, best_gain=0, score, best_score=-1;
173       for (j=0;j<EXC_CB_SIZE;j++)
174       {
175          corr=xcorr(resp[j],t+8*i,8);
176          score=corr*corr/(.001+E[j]);
177          g = corr/(.001+E[j]);
178          if (score>best_score)
179          {
180             best_index=j;
181             best_score=score;
182             best_gain=corr/(.001+E[j]);
183          }
184       }
185       frame_bits_pack(bits,best_index,7);
186       if (best_gain>0)
187          frame_bits_pack(bits,0,1);
188       else
189           frame_bits_pack(bits,1,1);        
190       ind[i]=best_index;
191       gains[i]=best_gain*Ee[ind[i]];
192       if (0) { /* Simulating scalar quantization of the gain*/
193          float sign=1;
194          printf("before: %f\n", best_gain);
195          if (best_gain<0)
196          {
197             sign=-1;
198          }
199          best_gain = fabs(best_gain)+.1;
200          best_gain = log(best_gain);
201          if (best_gain>8)
202             best_gain=8;
203          if (best_gain<0)
204             best_gain=0;
205          /*best_gain=.125*floor(8*best_gain+.5);*/
206          best_gain=.25*floor(4*best_gain+.5);
207          best_gain=sign*exp(best_gain);
208          if (fabs(best_gain)<1.01)
209             best_gain=0;
210          printf("after: %f\n", best_gain);
211       }
212
213       printf ("search: %d %f %f %f\n", best_index, best_gain, best_gain*best_gain*E[best_index], best_score);
214       for (j=0;j<40;j++)
215          e[j]=0;
216       for (j=0;j<8;j++)
217          e[8*i+j]=best_gain*codebook[best_index][j];
218       residue_zero(e, awk1, r, 40, p);
219       syn_filt_zero(r, ak, r, 40, p);
220       syn_filt_zero(r, awk2, r, 40,p);
221       for (j=0;j<40;j++)
222          t[j]-=r[j];
223
224       /*FIXME: Should move that out of the loop if we are to vector-quantize the gains*/
225       /*for (j=0;j<40;j++)
226         exc[j]+=e[j];*/
227    }
228
229    {
230       int best_vq_index=0, max_index;
231       float max_gain=0, log_max, min_dist=0, sign[5];
232
233       for (i=0;i<5;i++)
234       {
235          if (gains[i]<0)
236          {
237             gains[i]=-gains[i];
238             sign[i]=-1;
239          } else {
240             sign[i]=1;
241          }
242       }
243       for (i=0;i<5;i++)
244          if (gains[i]>max_gain)
245             max_gain=gains[i];
246       log_max=log(max_gain+1);
247       max_index = (int)(floor(.5+log_max-3));
248       if (max_index>7)
249          max_index=7;
250       if (max_index<0)
251          max_index=0;
252       max_gain=1/exp(max_index+3.0);
253       for (i=0;i<5;i++)
254         gains[i]*=max_gain;
255       frame_bits_pack(bits,max_index,3);
256
257       if (max_index>2)
258       {
259       for (i=0;i<5;i++)
260          printf ("%f ", gains[i]);
261       printf ("cbgains: \n");
262       }
263       /*Vector quantize gains[i]*/
264       for (i=0;i<256;i++)
265       {
266          float dist=0;
267          for (j=0;j<5;j++)
268             dist += (exc_gains_table[i][j]-gains[j])*(exc_gains_table[i][j]-gains[j]);
269          if (i==0 || dist<min_dist)
270          {
271             min_dist=dist;
272             best_vq_index=i;
273          }
274       }
275       frame_bits_pack(bits,best_vq_index,8);
276       printf ("best_gains_vq_index %d %f %d\n", best_vq_index, min_dist, max_index);
277 #if 1
278       for (i=0;i<5;i++)
279          gains[i]= sign[i]*exc_gains_table[best_vq_index][i]/max_gain/(Ee[ind[i]]+.001);
280 #else 
281       for (i=0;i<5;i++)
282          gains[i]= sign[i]*gains[i]/max_gain/(Ee[ind[i]]+.001);
283 #endif  
284
285    printf ("unquant gains: ");
286    for (i=0;i<5;i++)
287       printf ("%f ", gains[i]);
288    printf ("\n");
289     
290       for (i=0;i<5;i++)
291          for (j=0;j<8;j++)
292             exc[8*i+j]+=gains[i]*codebook[ind[i]][j];
293    }
294    /*for (i=0;i<5;i++)
295       printf ("%f ", gains[i]);
296       printf ("cbgains: \n");*/
297    /*TODO: Perform joint optimization of gains and quantization with prediction*/
298    
299    for (i=0;i<40;i++)
300       target[i]=t[i];
301 }
302
303 void split_cb_unquant(
304 float *exc,
305 float codebook[][8],            /* non-overlapping codebook */
306 int   nsf,                      /* number of samples in subframe */
307 FrameBits *bits
308 )
309 {
310    int i,j;
311    int ind[5];
312    float gains[5];
313    float sign[5];
314    int max_gain_ind, vq_gain_ind;
315    float max_gain, Ee[5];
316    for (i=0;i<5;i++)
317    {
318       ind[i] = frame_bits_unpack_unsigned(bits, 7);
319       if (frame_bits_unpack_unsigned(bits, 1))
320          sign[i]=-1;
321       else
322          sign[i]=1;
323       Ee[i]=.001;
324       for (j=0;j<8;j++)
325          Ee[i]+=codebook[ind[i]][j]*codebook[ind[i]][j];
326    }
327    max_gain_ind = frame_bits_unpack_unsigned(bits, 3);
328    vq_gain_ind = frame_bits_unpack_unsigned(bits, 8);
329    printf ("unquant gains ind: %d %d\n", max_gain_ind, vq_gain_ind);
330
331    max_gain=exp(max_gain_ind+3.0);
332    for (i=0;i<5;i++)
333       gains[i] = sign[i]*exc_gains_table[vq_gain_ind][i]*max_gain/Ee[i];
334    
335    printf ("unquant gains: ");
336    for (i=0;i<5;i++)
337       printf ("%f ", gains[i]);
338    printf ("\n");
339
340    for (i=0;i<5;i++)
341       for (j=0;j<8;j++)
342          exc[8*i+j]+=gains[i]*codebook[ind[i]][j];
343    
344 }