Many improvements (hopefully) to packet loss concealing, part of it from a
[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;
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 void pitch_unquant_3tap(
416 float exc[],                    /* Excitation */
417 int   start,                    /* Smallest pitch value allowed */
418 int   end,                      /* Largest pitch value allowed */
419 float pitch_coef,               /* Voicing (pitch) coefficient */
420 void *par,
421 int   nsf,                      /* Number of samples in subframe */
422 int *pitch_val,
423 float *gain_val,
424 SpeexBits *bits,
425 void *stack,
426 int count_lost,
427 int subframe_offset,
428 float last_pitch_gain)
429 {
430    int i;
431    int pitch;
432    int gain_index;
433    float gain[3];
434    float *gain_cdbk;
435    ltp_params *params;
436    params = (ltp_params*) par;
437    gain_cdbk=params->gain_cdbk;
438
439    pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
440    pitch += start;
441    gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
442    /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
443    gain[0] = gain_cdbk[gain_index*12];
444    gain[1] = gain_cdbk[gain_index*12+1];
445    gain[2] = gain_cdbk[gain_index*12+2];
446
447    if (count_lost && pitch > subframe_offset)
448    {
449       float gain_sum;
450
451       if (1) {
452          float tmp = count_lost < 4 ? last_pitch_gain : 0.4 * last_pitch_gain;
453          if (tmp>.95)
454             tmp=.95;
455          gain_sum = fabs(gain[1]);
456          if (gain[0]>0)
457             gain_sum += gain[0];
458          else
459             gain_sum -= .5*gain[0];
460          if (gain[2]>0)
461             gain_sum += gain[2];
462          else
463             gain_sum -= .5*gain[0];
464          if (gain_sum > tmp) {
465             float fact = tmp/gain_sum;
466             for (i=0;i<3;i++)
467                gain[i]*=fact;
468
469 #ifdef DEBUG
470          printf ("recovered unquantized pitch: %d, gain: [%f %f %f] (fact=%f)\n", pitch, gain[0], gain[1], gain[2], fact);
471 #endif
472          }
473
474       }
475
476       if (0) {
477       gain_sum = fabs(gain[0])+fabs(gain[1])+fabs(gain[2]);
478          if (gain_sum>.95) {
479          float fact = .95/gain_sum;
480          for (i=0;i<3;i++)
481             gain[i]*=fact;
482       }
483    }
484    }
485
486    *pitch_val = pitch;
487    /**gain_val = gain[0]+gain[1]+gain[2];*/
488    gain_val[0]=gain[0];
489    gain_val[1]=gain[1];
490    gain_val[2]=gain[2];
491
492 #ifdef DEBUG
493    printf ("unquantized pitch: %d %f %f %f\n", pitch, gain[0], gain[1], gain[2]);
494 #endif
495
496    {
497       float *e[3];
498       float *tmp2;
499       tmp2=PUSH(stack, 3*nsf, float);
500       e[0]=tmp2;
501       e[1]=tmp2+nsf;
502       e[2]=tmp2+2*nsf;
503       
504       for (i=0;i<3;i++)
505       {
506          int j;
507          int pp=pitch+1-i;
508 #if 0
509          for (j=0;j<nsf;j++)
510          {
511             if (j-pp<0)
512                e[i][j]=exc[j-pp];
513             else if (j-pp-pitch<0)
514                e[i][j]=exc[j-pp-pitch];
515             else
516                e[i][j]=0;
517          }
518 #else
519          {
520             int tmp1, tmp2;
521             tmp1=nsf;
522             if (tmp1>pp)
523                tmp1=pp;
524             for (j=0;j<tmp1;j++)
525                e[i][j]=exc[j-pp];
526             tmp2=nsf;
527             if (tmp2>pp+pitch)
528                tmp2=pp+pitch;
529             for (j=tmp1;j<tmp2;j++)
530                e[i][j]=exc[j-pp-pitch];
531             for (j=tmp2;j<nsf;j++)
532                e[i][j]=0;
533          }
534 #endif
535       }
536       for (i=0;i<nsf;i++)
537            exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
538    }
539 }
540
541
542 /** Forced pitch delay and gain */
543 int forced_pitch_quant(
544 float target[],                 /* Target vector */
545 float *sw,
546 float ak[],                     /* LPCs for this subframe */
547 float awk1[],                   /* Weighted LPCs #1 for this subframe */
548 float awk2[],                   /* Weighted LPCs #2 for this subframe */
549 float exc[],                    /* Excitation */
550 void *par,
551 int   start,                    /* Smallest pitch value allowed */
552 int   end,                      /* Largest pitch value allowed */
553 float pitch_coef,               /* Voicing (pitch) coefficient */
554 int   p,                        /* Number of LPC coeffs */
555 int   nsf,                      /* Number of samples in subframe */
556 SpeexBits *bits,
557 void *stack,
558 float *exc2,
559 float *r,
560 int complexity
561 )
562 {
563    int i;
564    if (pitch_coef>.9)
565       pitch_coef=.9;
566    for (i=0;i<nsf;i++)
567    {
568       exc[i]=exc[i-start]*pitch_coef;
569    }
570    return start;
571 }
572
573 /** Unquantize forced pitch delay and gain */
574 void forced_pitch_unquant(
575 float exc[],                    /* Excitation */
576 int   start,                    /* Smallest pitch value allowed */
577 int   end,                      /* Largest pitch value allowed */
578 float pitch_coef,               /* Voicing (pitch) coefficient */
579 void *par,
580 int   nsf,                      /* Number of samples in subframe */
581 int *pitch_val,
582 float *gain_val,
583 SpeexBits *bits,
584 void *stack,
585 int count_lost,
586 int subframe_offset,
587 float last_pitch_gain)
588 {
589    int i;
590    /*pitch_coef=.9;*/
591    if (pitch_coef>.9)
592       pitch_coef=.9;
593    for (i=0;i<nsf;i++)
594    {
595       exc[i]=exc[i-start]*pitch_coef;
596    }
597    *pitch_val = start;
598    gain_val[0]=gain_val[2]=0;
599    gain_val[1] = pitch_coef;
600 }