Working demo...
[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
37 #define min(a,b) ((a) < (b) ? (a) : (b))
38
39 /*---------------------------------------------------------------------------*\
40                                                                              
41  void overlap_cb_search()                                                             
42                                                                               
43  Searches a gain/shape codebook consisting of overlapping entries for the    
44  closest vector to the target.  Gives identical results to search() above   
45  buts uses fast end correction algorithm for the synthesis of response       
46  vectors.                                                                     
47                                                                              
48 \*---------------------------------------------------------------------------*/
49
50 float overlap_cb_search(
51 float target[],                 /* target vector */
52 float ak[],                     /* LPCs for this subframe */
53 float awk1[],                   /* Weighted LPCs for this subframe */
54 float awk2[],                   /* Weighted LPCs for this subframe */
55 float codebook[],               /* overlapping codebook */
56 int   entries,                  /* number of overlapping entries to search */
57 float *gain,                    /* gain of optimum entry */
58 int   *index,                   /* index of optimum entry */
59 int   p,                        /* number of LPC coeffs */
60 int   nsf                       /* number of samples in subframe */
61 )
62 {
63   float *resp;                  /* zero state response to current entry */
64   float *h;                     /* impulse response of synthesis filter */
65   float *impulse;               /* excitation vector containing one impulse */
66   float d,e,g,score;            /* codebook searching variables */
67   float bscore;                 /* score of "best" vector so far */
68   int i,k;                      /* loop variables */
69
70   /* Initialise */
71   
72   resp = (float*)malloc(sizeof(float)*nsf);
73   h = (float*)malloc(sizeof(float)*nsf);
74   impulse = (float*)malloc(sizeof(float)*nsf);
75
76   for(i=0; i<nsf; i++)
77     impulse[i] = 0.0;
78    
79   *gain = 0.0;
80   *index = 0;
81   bscore = 0.0;
82   impulse[0] = 1.0;
83
84   /* Calculate impulse response of  A(z/g2) / ( A(z)*(z/g1) ) */
85   residue_zero(impulse, awk1, h, nsf, p);
86   syn_filt_zero(h, ak, h, nsf, p);
87   syn_filt_zero(h, awk2, h, nsf,p);
88   
89   /* Calculate codebook zero-response */
90   residue_zero(&codebook[entries-1],awk1,resp,nsf,p);
91   syn_filt_zero(resp,ak,resp,nsf,p);
92   syn_filt_zero(resp,awk2,resp,nsf,p);
93     
94   /* Search codebook backwards using end correction for synthesis */
95   
96   for(k=entries-1; k>=0; k--) {
97
98     d = 0.0; e = 0.0;
99     for(i=0; i<nsf; i++) {
100       d += target[i]*resp[i];
101       e += resp[i]*resp[i];
102     }
103     g = d/(e+.1);
104     score = g*d;
105     /*printf ("score: %f %f %f %f\n", target[0],d,e,score);*/
106     if (score >= bscore) {
107       bscore = score;
108       *gain = g;
109       *index = k;
110     }
111     
112     /* Synthesise next entry */
113     
114     if (k) {
115       for(i=nsf-1; i>=1; i--)
116         resp[i] = resp[i-1] + codebook[k-1]*h[i];
117       resp[0] = codebook[k-1]*h[0];
118     }
119   }
120
121   free(resp);
122   free(h);
123   free(impulse);
124   return bscore;
125 }
126
127
128
129
130
131 float split_cb_search(
132 float target[],                 /* target vector */
133 float ak[],                     /* LPCs for this subframe */
134 float awk1[],                   /* Weighted LPCs for this subframe */
135 float awk2[],                   /* Weighted LPCs for this subframe */
136 float codebook[][8],            /* overlapping codebook */
137 int   entries,                  /* number of entries to search */
138 float *gain,                    /* gain of optimum entries */
139 int   *index,                   /* index of optimum entries */
140 int   p,                        /* number of LPC coeffs */
141 int   nsf,                      /* number of samples in subframe */
142 float *exc
143 )
144 {
145    int i,j;
146    float resp[64][8], E[64];
147    float t[40], r[40], e[40];
148    float gains[5];
149    for (i=0;i<40;i++)
150       t[i]=target[i];
151    for (i=0;i<64;i++)
152    {
153       residue_zero(codebook[i], awk1, resp[i], 8, p);
154       syn_filt_zero(resp[i], ak, resp[i], 8, p);
155       syn_filt_zero(resp[i], awk2, resp[i], 8,p);
156       E[i]=0;
157       for(j=0;j<8;j++)
158          E[i]+=resp[i][j]*resp[i][j];
159    }
160    for (i=0;i<5;i++)
161    {
162       int best_index;
163       float corr, best_gain, score, best_score=-1;
164       for (j=0;j<64;j++)
165       {
166          corr=xcorr(resp[j],t+8*i,8);
167          score=corr*corr/(.001+E[j]);
168          if (score>best_score)
169          {
170             best_index=j;
171             best_score=score;
172             best_gain=corr/(.001+E[j]);
173          }
174       }
175
176       if (1) { /* Simulating scalar quantization of the gain*/
177          float sign=1;
178          printf("before: %f\n", best_gain);
179          if (best_gain<0)
180             sign=-1;
181          best_gain = abs(best_gain)+.1;
182          best_gain = log(best_gain);
183          if (best_gain>8)
184             best_gain=8;
185          if (best_gain<0)
186             best_gain=0;
187          /*best_gain=.25*rint(4*best_gain);*/
188          best_gain=.25*floor(4*best_gain+.5);
189          best_gain=sign*exp(best_gain);
190          printf("after: %f\n", best_gain);
191       }
192       gains[i]=best_gain;
193
194       printf ("search: %d %f %f %f\n", best_index, best_gain, best_gain*best_gain*E[best_index], best_score);
195       for (j=0;j<40;j++)
196          e[j]=0;
197       for (j=0;j<8;j++)
198          e[8*i+j]=best_gain*codebook[best_index][j];
199       residue_zero(e, awk1, r, 40, p);
200       syn_filt_zero(r, ak, r, 40, p);
201       syn_filt_zero(r, awk2, r, 40,p);
202       for (j=0;j<40;j++)
203          t[j]-=r[j];
204
205       /*FIXME: Should move that out of the loop if we are to vector-quantize the gains*/
206       for (j=0;j<40;j++)
207          exc[j]+=e[j];
208    }
209
210    for (i=0;i<5;i++)
211       printf ("%f ", gains[i]);
212    printf ("cbgains: \n");
213    /*TODO: Perform joint optimization of gains and quantization with prediction*/
214    
215    for (i=0;i<40;i++)
216       target[i]=t[i];
217 }
218