e426568222cc2fbb972291cea00016df9e6ac13a
[speexdsp.git] / libspeex / ltp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: ltp.c
3    Lont-Term Prediction functions
4
5    Redistribution and use in source and binary forms, with or without
6    modification, are permitted provided that the following conditions
7    are met:
8    
9    - Redistributions of source code must retain the above copyright
10    notice, this list of conditions and the following disclaimer.
11    
12    - Redistributions in binary form must reproduce the above copyright
13    notice, this list of conditions and the following disclaimer in the
14    documentation and/or other materials provided with the distribution.
15    
16    - Neither the name of the Xiph.org Foundation nor the names of its
17    contributors may be used to endorse or promote products derived from
18    this software without specific prior written permission.
19    
20    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
24    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <math.h>
34 #include <stdio.h>
35 #include "ltp.h"
36 #include "stack_alloc.h"
37 #include "filters.h"
38 #include "speex_bits.h"
39
40 #ifdef _USE_SSE
41 #include "ltp_sse.h"
42 #else
43 static float inner_prod(float *x, float *y, int len)
44 {
45    int i;
46    float sum1=0,sum2=0,sum3=0,sum4=0;
47    for (i=0;i<len;)
48    {
49       sum1 += x[i]*y[i];
50       sum2 += x[i+1]*y[i+1];
51       sum3 += x[i+2]*y[i+2];
52       sum4 += x[i+3]*y[i+3];
53       i+=4;
54    }
55    return sum1+sum2+sum3+sum4;
56 }
57 #endif
58
59 /*Original, non-optimized version*/
60 /*static float inner_prod(float *x, float *y, int len)
61 {
62    int i;
63    float sum=0;
64    for (i=0;i<len;i++)
65       sum += x[i]*y[i];
66    return sum;
67 }
68 */
69
70
71 void open_loop_nbest_pitch(float *sw, int start, int end, int len, int *pitch, float *gain, int N, void *stack)
72 {
73    int i,j,k;
74    /*float corr=0;*/
75    /*float energy;*/
76    float *best_score;
77    float e0;
78    float *corr, *energy, *score;
79
80    best_score = PUSH(stack,N, float);
81    corr = PUSH(stack,end-start+1, float);
82    energy = PUSH(stack,end-start+2, float);
83    score = PUSH(stack,end-start+1, float);
84    for (i=0;i<N;i++)
85    {
86         best_score[i]=-1;
87         gain[i]=0;
88    }
89    energy[0]=inner_prod(sw-start, sw-start, len);
90    e0=inner_prod(sw, sw, len);
91    for (i=start;i<=end;i++)
92    {
93       /* Update energy for next pitch*/
94       energy[i-start+1] = energy[i-start] + sw[-i-1]*sw[-i-1] - sw[-i+len-1]*sw[-i+len-1];
95    }
96    for (i=start;i<=end;i++)
97    {
98       corr[i-start]=0;
99       score[i-start]=0;
100    }
101
102    for (i=start;i<=end;i++)
103    {
104       /* Compute correlation*/
105       corr[i-start]=inner_prod(sw, sw-i, len);
106       score[i-start]=corr[i-start]*corr[i-start]/(energy[i-start]+1);
107    }
108    for (i=start;i<=end;i++)
109    {
110       if (score[i-start]>best_score[N-1])
111       {
112          float g1, g;
113          g1 = corr[i-start]/(energy[i-start]+10);
114          g = sqrt(g1*corr[i-start]/(e0+10));
115          if (g>g1)
116             g=g1;
117          if (g<0)
118             g=0;
119          for (j=0;j<N;j++)
120          {
121             if (score[i-start] > best_score[j])
122             {
123                for (k=N-1;k>j;k--)
124                {
125                   best_score[k]=best_score[k-1];
126                   pitch[k]=pitch[k-1];
127                   gain[k] = gain[k-1];
128                }
129                best_score[j]=score[i-start];
130                pitch[j]=i;
131                gain[j]=g;
132                break;
133             }
134          }
135       }
136    }
137
138 }
139
140
141
142
143 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
144 float pitch_gain_search_3tap(
145 float target[],                 /* Target vector */
146 float ak[],                     /* LPCs for this subframe */
147 float awk1[],                   /* Weighted LPCs #1 for this subframe */
148 float awk2[],                   /* Weighted LPCs #2 for this subframe */
149 float exc[],                    /* Excitation */
150 void *par,
151 int   pitch,                    /* Pitch value */
152 int   p,                        /* Number of LPC coeffs */
153 int   nsf,                      /* Number of samples in subframe */
154 SpeexBits *bits,
155 void *stack,
156 float *exc2,
157 float *r,
158 int  *cdbk_index
159 )
160 {
161    int i,j;
162    float *tmp, *tmp2;
163    float *x[3];
164    float *e[3];
165    float corr[3];
166    float A[3][3];
167    float gain[3];
168    int   gain_cdbk_size;
169    float *gain_cdbk;
170    float err1,err2;
171
172    ltp_params *params;
173    params = (ltp_params*) par;
174    gain_cdbk=params->gain_cdbk;
175    gain_cdbk_size=1<<params->gain_bits;
176    tmp = PUSH(stack, 3*nsf, float);
177    tmp2 = PUSH(stack, 3*nsf, float);
178
179    x[0]=tmp;
180    x[1]=tmp+nsf;
181    x[2]=tmp+2*nsf;
182
183    e[0]=tmp2;
184    e[1]=tmp2+nsf;
185    e[2]=tmp2+2*nsf;
186    
187    for (i=2;i>=0;i--)
188    {
189       int pp=pitch+1-i;
190       for (j=0;j<nsf;j++)
191       {
192          if (j-pp<0)
193             e[i][j]=exc2[j-pp];
194          else if (j-pp-pitch<0)
195             e[i][j]=exc2[j-pp-pitch];
196          else
197             e[i][j]=0;
198       }
199
200       if (i==2)
201          syn_percep_zero(e[i], ak, awk1, awk2, x[i], nsf, p, stack);
202       else {
203          for (j=0;j<nsf-1;j++)
204             x[i][j+1]=x[i+1][j];
205          x[i][0]=0;
206          for (j=0;j<nsf;j++)
207             x[i][j]+=e[i][0]*r[j];
208       }
209    }
210
211    for (i=0;i<3;i++)
212       corr[i]=inner_prod(x[i],target,nsf);
213    
214    for (i=0;i<3;i++)
215       for (j=0;j<=i;j++)
216          A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);
217    
218    {
219       int j;
220       float C[9];
221       float *ptr=gain_cdbk;
222       int best_cdbk=0;
223       float best_sum=0;
224       C[0]=corr[2];
225       C[1]=corr[1];
226       C[2]=corr[0];
227       C[3]=A[1][2];
228       C[4]=A[0][1];
229       C[5]=A[0][2];
230       C[6]=A[2][2];
231       C[7]=A[1][1];
232       C[8]=A[0][0];
233       
234       for (i=0;i<gain_cdbk_size;i++)
235       {
236          float sum=0;
237          ptr = gain_cdbk+12*i;
238          for (j=0;j<9;j++)
239             sum+=C[j]*ptr[j+3];
240          if (0) {
241             float tot = fabs(ptr[1]);
242             if (ptr[0]>0)
243                tot+=ptr[0];
244             if (ptr[2]>0)
245                tot+=ptr[2];
246             if (tot>1)
247                continue;
248          }
249          if (0) {
250             float tot=ptr[0]+ptr[1]+ptr[2];
251             if (tot < 1.1)
252                sum *= 1+.15*tot;
253          }
254          if (sum>best_sum || i==0)
255          {
256             best_sum=sum;
257             best_cdbk=i;
258          }
259       }
260       gain[0] = gain_cdbk[best_cdbk*12];
261       gain[1] = gain_cdbk[best_cdbk*12+1];
262       gain[2] = gain_cdbk[best_cdbk*12+2];
263
264       *cdbk_index=best_cdbk;
265    }
266    /* Calculate gains by matrix inversion... (unquantized) */
267    if (0) {
268       float tmp;
269       float B[3][3];
270       A[0][0]+=1;
271       A[1][1]+=1;
272       A[2][2]+=1;
273       
274       for (i=0;i<3;i++)
275          for (j=0;j<3;j++)
276             B[i][j]=A[i][j];
277
278
279       tmp=A[1][0]/A[0][0];
280       for (i=0;i<3;i++)
281          A[1][i] -= tmp*A[0][i];
282       corr[1] -= tmp*corr[0];
283
284       tmp=A[2][0]/A[0][0];
285       for (i=0;i<3;i++)
286          A[2][i] -= tmp*A[0][i];
287       corr[2] -= tmp*corr[0];
288       
289       tmp=A[2][1]/A[1][1];
290       A[2][2] -= tmp*A[1][2];
291       corr[2] -= tmp*corr[1];
292
293       corr[2] /= A[2][2];
294       corr[1] = (corr[1] - A[1][2]*corr[2])/A[1][1];
295       corr[0] = (corr[0] - A[0][2]*corr[2] - A[0][1]*corr[1])/A[0][0];
296       /*printf ("\n%f %f %f\n", best_corr[0], best_corr[1], best_corr[2]);*/
297
298    
299       /* Put gains in right order */
300       gain[0]=corr[2];gain[1]=corr[1];gain[2]=corr[0];
301
302       {
303          float gain_sum = gain[0]+gain[1]+gain[2];
304          if (fabs(gain_sum)>2.5)
305          {
306             float fact = 2.5/gain_sum;
307             for (i=0;i<3;i++)
308                gain[i]*=fact;
309          }
310       }
311       
312    }
313    
314    for (i=0;i<nsf;i++)
315       exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
316 #ifdef DEBUG
317    printf ("3-tap pitch = %d, gains = [%f %f %f]\n",pitch, gain[0], gain[1], gain[2]);
318 #endif
319    
320    err1=0;
321    err2=0;
322    for (i=0;i<nsf;i++)
323       err1+=target[i]*target[i];
324    for (i=0;i<nsf;i++)
325       err2+=(target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i])
326       * (target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i]);
327 #ifdef DEBUG
328    printf ("prediction gain = %f\n",err1/(err2+1));
329 #endif
330
331    return err2;
332 }
333
334
335 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
336 int pitch_search_3tap(
337 float target[],                 /* Target vector */
338 float *sw,
339 float ak[],                     /* LPCs for this subframe */
340 float awk1[],                   /* Weighted LPCs #1 for this subframe */
341 float awk2[],                   /* Weighted LPCs #2 for this subframe */
342 float exc[],                    /* Excitation */
343 void *par,
344 int   start,                    /* Smallest pitch value allowed */
345 int   end,                      /* Largest pitch value allowed */
346 float pitch_coef,               /* Voicing (pitch) coefficient */
347 int   p,                        /* Number of LPC coeffs */
348 int   nsf,                      /* Number of samples in subframe */
349 SpeexBits *bits,
350 void *stack,
351 float *exc2,
352 float *r,
353 int complexity
354 )
355 {
356    int i,j;
357    int cdbk_index, pitch=0, best_gain_index=0;
358    float *best_exc;
359    int best_pitch=0;
360    float err, best_err=-1;
361    int N=3;
362    ltp_params *params;
363    int *nbest;
364    float *gains;
365
366    N=complexity;
367    if (N>10)
368       N=10;
369
370    nbest=PUSH(stack, N, int);
371    gains = PUSH(stack, N, float);
372    params = (ltp_params*) par;
373
374    if (N==0 || end<start)
375    {
376       speex_bits_pack(bits, 0, params->pitch_bits);
377       speex_bits_pack(bits, 0, params->gain_bits);
378       for (i=0;i<nsf;i++)
379          exc[i]=0;
380       return start;
381    }
382    
383    best_exc=PUSH(stack,nsf, float);
384    
385    if (N>end-start+1)
386       N=end-start+1;
387    open_loop_nbest_pitch(sw, start, end, nsf, nbest, gains, N, stack);
388    for (i=0;i<N;i++)
389    {
390       pitch=nbest[i];
391       for (j=0;j<nsf;j++)
392          exc[j]=0;
393       err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, par, pitch, p, nsf,
394                                  bits, stack, exc2, r, &cdbk_index);
395       if (err<best_err || best_err<0)
396       {
397          for (j=0;j<nsf;j++)
398             best_exc[j]=exc[j];
399          best_err=err;
400          best_pitch=pitch;
401          best_gain_index=cdbk_index;
402       }
403    }
404    
405    /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/
406    speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
407    speex_bits_pack(bits, best_gain_index, params->gain_bits);
408    /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/
409    for (i=0;i<nsf;i++)
410       exc[i]=best_exc[i];
411
412    return pitch;
413 }
414
415
416 void pitch_unquant_3tap(
417 float exc[],                    /* Excitation */
418 int   start,                    /* Smallest pitch value allowed */
419 int   end,                      /* Largest pitch value allowed */
420 float pitch_coef,               /* Voicing (pitch) coefficient */
421 void *par,
422 int   nsf,                      /* Number of samples in subframe */
423 int *pitch_val,
424 float *gain_val,
425 SpeexBits *bits,
426 void *stack,
427 int lost)
428 {
429    int i;
430    int pitch;
431    int gain_index;
432    float gain[3];
433    float *gain_cdbk;
434    ltp_params *params;
435    params = (ltp_params*) par;
436    gain_cdbk=params->gain_cdbk;
437
438    pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
439    pitch += start;
440    gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
441    /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
442    gain[0] = gain_cdbk[gain_index*12];
443    gain[1] = gain_cdbk[gain_index*12+1];
444    gain[2] = gain_cdbk[gain_index*12+2];
445    if (lost)
446    {
447       float gain_sum;
448       gain_sum = fabs(gain[0])+fabs(gain[1])+fabs(gain[2]);
449       if (gain_sum>.95)
450       {
451          float fact = .95/gain_sum;
452          for (i=0;i<3;i++)
453             gain[i]*=fact;
454       }
455    }
456
457    *pitch_val = pitch;
458    /**gain_val = gain[0]+gain[1]+gain[2];*/
459    gain_val[0]=gain[0];
460    gain_val[1]=gain[1];
461    gain_val[2]=gain[2];
462
463 #ifdef DEBUG
464    printf ("unquantized pitch: %d %f %f %f\n", pitch, gain[0], gain[1], gain[2]);
465 #endif
466    for(i=0;i<nsf;i++)
467       exc[i]=0;
468
469    {
470       float *e[3];
471       float *tmp2;
472       tmp2=PUSH(stack, 3*nsf, float);
473       e[0]=tmp2;
474       e[1]=tmp2+nsf;
475       e[2]=tmp2+2*nsf;
476       
477       for (i=0;i<3;i++)
478       {
479          int j;
480          int pp=pitch+1-i;
481 #if 0
482          for (j=0;j<nsf;j++)
483          {
484             if (j-pp<0)
485                e[i][j]=exc[j-pp];
486             else if (j-pp-pitch<0)
487                e[i][j]=exc[j-pp-pitch];
488             else
489                e[i][j]=0;
490          }
491 #else
492          {
493             int tmp1, tmp2;
494             tmp1=nsf;
495             if (tmp1>pp)
496                tmp1=pp;
497             for (j=0;j<tmp1;j++)
498                e[i][j]=exc[j-pp];
499             tmp2=nsf;
500             if (tmp2>pp+pitch)
501                tmp2=pp+pitch;
502             for (j=tmp1;j<tmp2;j++)
503                e[i][j]=exc[j-pp-pitch];
504             for (j=tmp2;j<nsf;j++)
505                e[i][j]=0;
506          }
507 #endif
508       }
509       for (i=0;i<nsf;i++)
510            exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
511    }
512 }
513
514
515 /** Forced pitch delay and gain */
516 int forced_pitch_quant(
517 float target[],                 /* Target vector */
518 float *sw,
519 float ak[],                     /* LPCs for this subframe */
520 float awk1[],                   /* Weighted LPCs #1 for this subframe */
521 float awk2[],                   /* Weighted LPCs #2 for this subframe */
522 float exc[],                    /* Excitation */
523 void *par,
524 int   start,                    /* Smallest pitch value allowed */
525 int   end,                      /* Largest pitch value allowed */
526 float pitch_coef,               /* Voicing (pitch) coefficient */
527 int   p,                        /* Number of LPC coeffs */
528 int   nsf,                      /* Number of samples in subframe */
529 SpeexBits *bits,
530 void *stack,
531 float *exc2,
532 float *r,
533 int complexity
534 )
535 {
536    int i;
537    if (pitch_coef>.9)
538       pitch_coef=.9;
539    for (i=0;i<nsf;i++)
540    {
541       exc[i]=exc[i-start]*pitch_coef;
542    }
543    return start;
544 }
545
546 /** Unquantize forced pitch delay and gain */
547 void forced_pitch_unquant(
548 float exc[],                    /* Excitation */
549 int   start,                    /* Smallest pitch value allowed */
550 int   end,                      /* Largest pitch value allowed */
551 float pitch_coef,               /* Voicing (pitch) coefficient */
552 void *par,
553 int   nsf,                      /* Number of samples in subframe */
554 int *pitch_val,
555 float *gain_val,
556 SpeexBits *bits,
557 void *stack,
558 int lost)
559 {
560    int i;
561    /*pitch_coef=.9;*/
562    if (pitch_coef>.9)
563       pitch_coef=.9;
564    for (i=0;i<nsf;i++)
565    {
566       exc[i]=exc[i-start]*pitch_coef;
567    }
568    *pitch_val = start;
569    gain_val[0]=gain_val[2]=0;
570    gain_val[1] = pitch_coef;
571 }