More decoder stuff
[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       for (i=0;i<5;i++)
285          for (j=0;j<8;j++)
286             exc[8*i+j]+=gains[i]*codebook[ind[i]][j];
287    }
288    /*for (i=0;i<5;i++)
289       printf ("%f ", gains[i]);
290       printf ("cbgains: \n");*/
291    /*TODO: Perform joint optimization of gains and quantization with prediction*/
292    
293    for (i=0;i<40;i++)
294       target[i]=t[i];
295 }
296
297 void split_cb_unquant(
298 float *exc,
299 float codebook[][8],            /* non-overlapping codebook */
300 int   nsf,                      /* number of samples in subframe */
301 FrameBits *bits
302 )
303 {
304    int i;
305    int ind[5];
306    float gains[5];
307    float sign[5];
308    for (i=0;i<5;i++)
309    {
310       ind[i] = frame_bits_unpack_unsigned(bits, 7);
311       if (frame_bits_unpack_unsigned(bits, 1))
312          sign[i]=-1;
313       else
314          sign[i]=1;
315    }
316    frame_bits_unpack_unsigned(bits, 11);
317 }