Compressing the innovation along the pitch direction
[opus.git] / libcelt / vq.c
1 /* (C) 2007 Jean-Marc Valin, CSIRO
2 */
3 /*
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7    
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10    
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14    
15    - Neither the name of the Xiph.org Foundation nor the names of its
16    contributors may be used to endorse or promote products derived from
17    this software without specific prior written permission.
18    
19    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
22    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
23    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
24    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
25    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
26    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
27    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
28    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
29    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30 */
31
32 #include <math.h>
33 #include <stdlib.h>
34
35 /* Algebraic pulse-base quantiser. The signal x is replaced by the sum of the pitch 
36    a combination of pulses such that its norm is still equal to 1 */
37 void alg_quant(float *x, int N, int K, float *p)
38 {
39    float y[N];
40    int i,j;
41    float xy = 0;
42    float yy = 0;
43    float yp = 0;
44    float Rpp=0;
45    float gain=0;
46    for (j=0;j<N;j++)
47       Rpp += p[j]*p[j];
48    for (i=0;i<N;i++)
49       y[i] = 0;
50       
51    for (i=0;i<K;i++)
52    {
53       int best_id=0;
54       float max_val=-1e10;
55       float best_xy=0, best_yy=0, best_yp = 0;
56       for (j=0;j<N;j++)
57       {
58          float tmp_xy, tmp_yy, tmp_yp;
59          float score;
60          float g;
61          tmp_xy = xy + fabs(x[j]);
62          tmp_yy = yy + 2*fabs(y[j]) + 1;
63          if (x[j]>0)
64             tmp_yp = yp + p[j];
65          else
66             tmp_yp = yp - p[j];
67          g = (sqrt(tmp_yp*tmp_yp + tmp_yy - tmp_yy*Rpp) - tmp_yp)/tmp_yy;
68          score = 2*g*tmp_xy - g*g*tmp_yy;
69          if (score>max_val)
70          {
71             max_val = score;
72             best_id = j;
73             best_xy = tmp_xy;
74             best_yy = tmp_yy;
75             best_yp = tmp_yp;
76             gain = g;
77          }
78       }
79
80       xy = best_xy;
81       yy = best_yy;
82       yp = best_yp;
83       if (x[best_id]>0)
84          y[best_id] += 1;
85       else
86          y[best_id] -= 1;
87    }
88    
89    for (i=0;i<N;i++)
90       x[i] = p[i]+gain*y[i];
91    
92 }
93
94 /* Improved algebraic pulse-base quantiser. The signal x is replaced by the sum of the pitch 
95    a combination of pulses such that its norm is still equal to 1. The only difference with 
96    the quantiser above is that the search is more complete. */
97 void alg_quant2(float *x, int N, int K, float *p)
98 {
99    int L = 5;
100    //float tata[200];
101    float y[L][N];
102    int iy[L][N];
103    //float tata2[200];
104    float ny[L][N];
105    int iny[L][N];
106    int i, j, m;
107    float xy[L], nxy[L];
108    float yy[L], nyy[L];
109    float yp[L], nyp[L];
110    float best_scores[L];
111    float Rpp=0, Rxp=0;
112    float gain[L];
113    int maxL = 1;
114    float alpha = .9;
115    
116    for (j=0;j<N;j++)
117       Rpp += p[j]*p[j];
118    for (j=0;j<N;j++)
119       Rxp += x[j]*p[j];
120    for (m=0;m<L;m++)
121       for (i=0;i<N;i++)
122          y[m][i] = 0;
123       
124    for (m=0;m<L;m++)
125       for (i=0;i<N;i++)
126          ny[m][i] = 0;
127
128    for (m=0;m<L;m++)
129       for (i=0;i<N;i++)
130          iy[m][i] = iny[m][i] = 0;
131
132    for (m=0;m<L;m++)
133       xy[m] = yy[m] = yp[m] = gain[m] = 0;
134    
135    for (i=0;i<K;i++)
136    {
137       int L2 = L;
138       if (L>maxL)
139       {
140          L2 = maxL;
141          maxL *= N;
142       }
143       for (m=0;m<L;m++)
144          best_scores[m] = -1e10;
145
146       for (m=0;m<L2;m++)
147       {
148          for (j=0;j<N;j++)
149          {
150             int sign;
151             for (sign=-1;sign<=1;sign+=2)
152             {
153                if (iy[m][j]*sign < 0)
154                   continue;
155                //fprintf (stderr, "%d/%d %d/%d %d/%d\n", i, K, m, L2, j, N);
156                float tmp_xy, tmp_yy, tmp_yp;
157                float score;
158                float g;
159                float s = sign;
160                tmp_xy = xy[m] + s*x[j]               - alpha*s*p[j]*Rxp;
161                tmp_yy = yy[m] + 2*s*y[m][j] + 1      +alpha*alpha*p[j]*p[j]*Rpp - 2*alpha*s*p[j]*yp[m] - 2*alpha*p[j]*p[j];
162                tmp_yp = yp[m] + s*p[j]               *(1-alpha*Rpp);
163                g = (sqrt(tmp_yp*tmp_yp + tmp_yy - tmp_yy*Rpp) - tmp_yp)/tmp_yy;
164                score = 2*g*tmp_xy - g*g*tmp_yy;
165
166                if (score>best_scores[L-1])
167                {
168                   int k, n;
169                   int id = L-1;
170                   while (id > 0 && score > best_scores[id-1])
171                      id--;
172                
173                   for (k=L-1;k>id;k--)
174                   {
175                      nxy[k] = nxy[k-1];
176                      nyy[k] = nyy[k-1];
177                      nyp[k] = nyp[k-1];
178                      //fprintf(stderr, "%d %d \n", N, k);
179                      for (n=0;n<N;n++)
180                         ny[k][n] = ny[k-1][n];
181                      for (n=0;n<N;n++)
182                         iny[k][n] = iny[k-1][n];
183                      gain[k] = gain[k-1];
184                      best_scores[k] = best_scores[k-1];
185                   }
186
187                   nxy[id] = tmp_xy;
188                   nyy[id] = tmp_yy;
189                   nyp[id] = tmp_yp;
190                   gain[id] = g;
191                   for (n=0;n<N;n++)
192                      ny[id][n] = y[m][n];
193                   ny[id][j] += s;
194                   for (n=0;n<N;n++)
195                      ny[id][n] -= alpha*s*p[j]*p[n];
196                
197                   for (n=0;n<N;n++)
198                      iny[id][n] = iy[m][n];
199                   if (s>0)
200                      iny[id][j] += 1;
201                   else
202                      iny[id][j] -= 1;
203                   best_scores[id] = score;
204                }
205             }   
206          }
207          
208       }
209       int k,n;
210       for (k=0;k<L;k++)
211       {
212          xy[k] = nxy[k];
213          yy[k] = nyy[k];
214          yp[k] = nyp[k];
215          for (n=0;n<N;n++)
216             y[k][n] = ny[k][n];
217          for (n=0;n<N;n++)
218             iy[k][n] = iny[k][n];
219       }
220
221    }
222    
223    for (i=0;i<N;i++)
224       x[i] = p[i]+gain[0]*y[0][i];
225    if (0) {
226       float E=1e-15;
227       int ABS = 0;
228       for (i=0;i<N;i++)
229          ABS += abs(iy[0][i]);
230       //if (K != ABS)
231       //   printf ("%d %d\n", K, ABS);
232       for (i=0;i<N;i++)
233          E += x[i]*x[i];
234       //printf ("%f\n", E);
235       E = 1/sqrt(E);
236       for (i=0;i<N;i++)
237          x[i] *= E;
238    }
239 }
240
241 /* Just replace the band with noise of unit energy */
242 void noise_quant(float *x, int N, int K, float *p)
243 {
244    int i;
245    float E = 1e-10;
246    for (i=0;i<N;i++)
247    {
248       x[i] = (rand()%1000)/500.-1;
249       E += x[i]*x[i];
250    }
251    E = 1./sqrt(E);
252    for (i=0;i<N;i++)
253    {
254       x[i] *= E;
255    }
256 }
257
258 static const float pg[5] = {1.f, .82f, .75f, 0.7f, 0.6f};
259
260 /* Finds the right offset into Y and copy it */
261 void copy_quant(float *x, int N, int K, float *Y, int B, int N0)
262 {
263    int i,j;
264    int best=0;
265    float best_score=0;
266    float s = 1;
267    float E;
268    for (i=0;i<N0*B-N;i+=B)
269    {
270       int j;
271       float xy=0, yy=0;
272       float score;
273       for (j=0;j<N;j++)
274       {
275          xy += x[j]*Y[i+j];
276          yy += Y[i+j]*Y[i+j];
277       }
278       score = xy*xy/(.001+yy);
279       if (score > best_score)
280       {
281          best_score = score;
282          best = i;
283          if (xy>0)
284             s = 1;
285          else
286             s = -1;
287       }
288    }
289    //printf ("%d %f\n", best, best_score);
290    if (K==0)
291    {
292       E = 1e-10;
293       for (j=0;j<N;j++)
294       {
295          x[j] = s*Y[best+j];
296          E += x[j]*x[j];
297       }
298       E = 1/sqrt(E);
299       for (j=0;j<N;j++)
300          x[j] *= E;
301    } else {
302       float P[N];
303       float pred_gain;
304       if (K>4)
305          pred_gain = .5;
306       else
307          pred_gain = pg[K];
308       E = 1e-10;
309       for (j=0;j<N;j++)
310       {
311          P[j] = s*Y[best+j];
312          E += P[j]*P[j];
313       }
314       E = .8/sqrt(E);
315       for (j=0;j<N;j++)
316          P[j] *= E;
317       alg_quant2(x, N, K, P);
318    }
319 }