Added a post-filter for narrowband (and thus 0-4 kHz in wideband)
[speexdsp.git] / libspeex / ltp.c
1 /* Copyright (C) 2002 Jean-Marc Valin 
2    File: ltp.c
3    Lont-Term Prediction functions
4
5    This library is free software; you can redistribute it and/or
6    modify it under the terms of the GNU Lesser General Public
7    License as published by the Free Software Foundation; either
8    version 2.1 of the License, or (at your option) any later version.
9    
10    This library is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
13    Lesser General Public License for more details.
14    
15    You should have received a copy of the GNU Lesser General Public
16    License along with this library; if not, write to the Free Software
17    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
18 */
19
20 #include <math.h>
21 #include <stdio.h>
22 #include "ltp.h"
23 #include "cb_search.h"
24 #include "stack_alloc.h"
25 #include "filters.h"
26 #include "speex_bits.h"
27
28 #define abs(x) ((x)<0 ? -(x) : (x))
29
30 void open_loop_pitch(float *sw, int start, int end, int len, int *pitch)
31 {
32    int i;
33    float e0, corr, energy, best_gain=0, pred_gain, best_corr=0, best_energy=0;
34    float score, best_score=-1;
35    e0=xcorr(sw, sw, len);
36    energy=xcorr(sw-start, sw-start, len);
37    for (i=start;i<=end;i++)
38    {
39       corr=xcorr(sw, sw-i, len);
40       score=corr*corr/(energy+1);
41       if (score>best_score)
42       {
43          /*if ((abs(i-2**pitch)>4 && abs(i-3**pitch)>6) || score>0.9*best_score)*/
44          {
45          best_score=score;
46          best_gain=corr/(energy+1);
47          best_corr=corr;
48          best_energy=energy;
49          *pitch=i;
50          }
51       }
52
53       /* Update energy for next pitch*/
54       energy+=sw[-i-1]*sw[-i-1] - sw[-i+len]*sw[-i+len];
55    }
56    pred_gain=e0/(1+fabs(e0+best_gain*best_gain*best_energy-2*best_gain*best_corr));
57 #ifdef DEBUG
58    printf ("pred = %f\n", pred_gain);
59 #endif
60 }
61
62 void closed_loop_fractional_pitch(
63 float target[],                 /* Target vector */
64 float ak[],                     /* LPCs for this subframe */
65 float awk1[],                   /* Weighted LPCs #1 for this subframe */
66 float awk2[],                   /* Weighted LPCs #2 for this subframe */
67 float exc[],                    /* Overlapping codebook */
68 float *filt,                    /* Over-sampling filter */
69 int   filt_side,                /* Over-sampling factor */
70 int   fact,                     /* Over-sampling factor */
71 int   start,                    /* Smallest pitch value allowed */
72 int   end,                      /* Largest pitch value allowed */
73 float *gain,                    /* 3-tab gains of optimum entry */
74 int   *pitch,                   /* Index of optimum entry */
75 int   p,                        /* Number of LPC coeffs */
76 int   nsf,                      /* Number of samples in subframe */
77 float *stack
78 )
79 {
80    int i, j, size, filt_size, base, frac=0, best_cor=0;
81    float *oexc_mem, *oexc, *exc_ptr, *fexc, *f, frac_pitch=0, best_score=-1, best_gain=0;
82    float sc;
83    float corr[3];
84    float A[3][3];
85 #if 1
86    sc = overlap_cb_search(target, ak, awk1, awk2,
87                      &exc[-end], end-start+1, gain, pitch, p,
88                      nsf, stack);
89                      *pitch=end-*pitch;
90 #ifdef DEBUG
91    printf ("ol score: %d %f\n", *pitch, sc);
92 #endif
93 #endif
94    base=*pitch;
95    exc_ptr=exc-*pitch;
96    size = fact*nsf + filt_side*2 + 16*fact;
97    filt_size = 2*filt_side+1;
98    oexc_mem = PUSH(stack, size);
99    oexc=oexc_mem+filt_side;
100    fexc = PUSH(stack, size/fact);
101    f=filt+filt_side;
102
103    for(i=0;i<size;i++)
104       oexc_mem[i]=0;
105    for (i=-8;i<nsf+8;i++)
106    {
107       for (j=-filt_side;j<=filt_side;j++)
108          oexc[fact*(i+8)+j] += fact*exc_ptr[i]*f[j];
109    }
110
111    /*for (i=0;i<size;i++)
112      printf ("%f ", oexc_mem[i]);
113    printf ("eee\n");
114    */
115    for (j=0;j<fact;j++)
116    {
117       int correction;
118       float score;
119       for (i=0;i<size/fact;i++)
120          fexc[i]=oexc[fact*i+j];
121       score=overlap_cb_search(target, ak, awk1, awk2,
122                         fexc, 16, gain, &correction, p,
123                         nsf, stack);
124       if (score>best_score)
125       {
126          best_cor = correction;
127          *pitch = base+8-correction;
128          frac = j;
129          best_gain=*gain;
130          frac_pitch = *pitch-(j/(float)fact);
131          best_score=score;
132       }
133 #ifdef DEBUG
134       printf ("corr: %d %d %f\n", correction, *pitch, score);
135 #endif
136    }
137    /*for (i=0;i<nsf;i++)
138      printf ("%f ", oexc[4*(i+8)]);
139    printf ("aaa\n");
140    for (i=0;i<nsf;i++)
141      printf ("%f ", exc[i-base]);
142      printf ("bbb\n");*/
143
144    /*if (best_gain>1.2)
145       best_gain=1.2;
146    if (best_gain<-.2)
147    best_gain=-.2;*/
148    for (i=0;i<nsf;i++)
149      exc[i]=best_gain*oexc[fact*(best_cor+i)+frac];
150    
151    {
152       float *x[3];
153       x[0] = PUSH(stack, nsf);
154       x[1] = PUSH(stack, nsf);
155       x[2] = PUSH(stack, nsf);
156       
157       for (j=0;j<3;j++)
158          for (i=0;i<nsf;i++)
159             x[j][i]=oexc[fact*(best_cor+i)+frac+fact*(j-1)];
160
161
162    for (i=0;i<3;i++)
163    {
164       residue_zero(x[i],awk1,x[i],nsf,p);
165       syn_filt_zero(x[i],ak,x[i],nsf,p);
166       syn_filt_zero(x[i],awk2,x[i],nsf,p);
167    }
168
169    for (i=0;i<3;i++)
170       corr[i]=xcorr(x[i],target,nsf);
171    
172    for (i=0;i<3;i++)
173       for (j=0;j<=i;j++)
174          A[i][j]=A[j][i]=xcorr(x[i],x[j],nsf);
175    /*for (i=0;i<3;i++)
176    {
177       for (j=0;j<3;j++)
178          printf ("%f ", A[i][j]);
179       printf ("\n");
180       }*/
181    A[0][0]+=1;
182    A[1][1]+=1;
183    A[2][2]+=1;
184    {
185       float tmp=A[1][0]/A[0][0];
186       for (i=0;i<3;i++)
187          A[1][i] -= tmp*A[0][i];
188       corr[1] -= tmp*corr[0];
189
190       tmp=A[2][0]/A[0][0];
191       for (i=0;i<3;i++)
192          A[2][i] -= tmp*A[0][i];
193       corr[2] -= tmp*corr[0];
194       
195       tmp=A[2][1]/A[1][1];
196       A[2][2] -= tmp*A[1][2];
197       corr[2] -= tmp*corr[1];
198
199       corr[2] /= A[2][2];
200       corr[1] = (corr[1] - A[1][2]*corr[2])/A[1][1];
201       corr[0] = (corr[0] - A[0][2]*corr[2] - A[0][1]*corr[1])/A[0][0];
202       /*printf ("\n%f %f %f\n", best_corr[0], best_corr[1], best_corr[2]);*/
203
204    }
205    /* Put gains in right order */
206    /*gain[0]=corr[2];gain[1]=corr[1];gain[2]=corr[0];*/
207    gain[0]=corr[0];gain[1]=corr[1];gain[2]=corr[2];
208
209    for (i=0;i<nsf;i++)
210       exc[i]=gain[0]*oexc[fact*(best_cor+i)+frac-fact] + 
211              gain[1]*oexc[fact*(best_cor+i)+frac] + 
212              gain[2]*oexc[fact*(best_cor+i)+frac+fact];
213    
214    /*for (i=0;i<nsf;i++)
215      exc[i]=best_gain*x[1][i];*/
216    /*for (i=0;i<nsf;i++)
217      exc[i]=best_gain*oexc[fact*(best_cor+i)+frac];*/
218 #ifdef DEBUG
219    printf ("frac gains: %f %f %f\n", gain[0], gain[1], gain[2]);
220 #endif
221       POP(stack);
222       POP(stack);
223       POP(stack);
224    }
225 #ifdef DEBUG
226    printf ("frac pitch = %f %f\n", frac_pitch, best_score);
227 #endif
228    POP(stack);
229    POP(stack);
230
231 }
232
233 int pitch_search_3tap_unquant(
234 float target[],                 /* Target vector */
235 float *sw,
236 float ak[],                     /* LPCs for this subframe */
237 float awk1[],                   /* Weighted LPCs #1 for this subframe */
238 float awk2[],                   /* Weighted LPCs #2 for this subframe */
239 float exc[],                    /* Excitation */
240 void *par,
241 int   start,                    /* Smallest pitch value allowed */
242 int   end,                      /* Largest pitch value allowed */
243 int   p,                        /* Number of LPC coeffs */
244 int   nsf,                      /* Number of samples in subframe */
245 SpeexBits *bits,
246 float *stack,
247 float *exc2
248 )
249 {
250    int i,j;
251    float *tmp;
252    float *x[3];
253    float corr[3];
254    float A[3][3];
255    int pitch;
256    float gain[3];
257    tmp = PUSH(stack, 3*nsf);
258    x[0]=tmp;
259    x[1]=tmp+nsf;
260    x[2]=tmp+2*nsf;
261
262    /* Perform closed-loop 1-tap search*/
263    overlap_cb_search(target, ak, awk1, awk2,
264                      &exc[-end], end-start+1, gain, &pitch, p,
265                      nsf, stack);
266    /* Real pitch value */
267    pitch=end-pitch;
268    
269    
270    for (i=0;i<3;i++)
271    {
272       residue_zero(&exc[-pitch-1+i],awk1,x[i],nsf,p);
273       syn_filt_zero(x[i],ak,x[i],nsf,p);
274       syn_filt_zero(x[i],awk2,x[i],nsf,p);
275    }
276
277    for (i=0;i<3;i++)
278       corr[i]=xcorr(x[i],target,nsf);
279    
280    for (i=0;i<3;i++)
281       for (j=0;j<=i;j++)
282          A[i][j]=A[j][i]=xcorr(x[i],x[j],nsf);
283    A[0][0]+=1;
284    A[1][1]+=1;
285    A[2][2]+=1;
286    {
287       float tmp=A[1][0]/A[0][0];
288       for (i=0;i<3;i++)
289          A[1][i] -= tmp*A[0][i];
290       corr[1] -= tmp*corr[0];
291
292       tmp=A[2][0]/A[0][0];
293       for (i=0;i<3;i++)
294          A[2][i] -= tmp*A[0][i];
295       corr[2] -= tmp*corr[0];
296       
297       tmp=A[2][1]/A[1][1];
298       A[2][2] -= tmp*A[1][2];
299       corr[2] -= tmp*corr[1];
300
301       corr[2] /= A[2][2];
302       corr[1] = (corr[1] - A[1][2]*corr[2])/A[1][1];
303       corr[0] = (corr[0] - A[0][2]*corr[2] - A[0][1]*corr[1])/A[0][0];
304       /*printf ("\n%f %f %f\n", best_corr[0], best_corr[1], best_corr[2]);*/
305
306    }
307    /* Put gains in right order */
308    gain[0]=corr[2];gain[1]=corr[1];gain[2]=corr[0];
309
310    for (i=nsf-1;i>=0;i--)
311       exc[i]=gain[0]*exc[i-pitch+1]+gain[1]*exc[i-pitch]+gain[2]*exc[i-pitch-1];
312 #if 0
313    if (0){
314       float tmp1=0,tmp2=0;
315       for (i=0;i<nsf;i++)
316          tmp1+=target[i]*target[i];
317       for (i=0;i<nsf;i++)
318          tmp2+=(target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i])
319          * (target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i]);
320 #ifdef DEBUG
321       printf ("prediction gain = %f\n",tmp1/(tmp2+1));
322 #endif
323       return tmp1/(tmp2+1);
324    }
325 #endif
326    POP(stack);
327    return pitch;
328 }
329
330
331 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
332 float pitch_gain_search_3tap(
333 float target[],                 /* Target vector */
334 float ak[],                     /* LPCs for this subframe */
335 float awk1[],                   /* Weighted LPCs #1 for this subframe */
336 float awk2[],                   /* Weighted LPCs #2 for this subframe */
337 float exc[],                    /* Excitation */
338 void *par,
339 int   pitch,                    /* Pitch value */
340 int   p,                        /* Number of LPC coeffs */
341 int   nsf,                      /* Number of samples in subframe */
342 SpeexBits *bits,
343 float *stack,
344 float *exc2,
345 int  *cdbk_index
346 )
347 {
348    int i,j;
349    float *tmp, *tmp2;
350    float *x[3];
351    float *e[3];
352    float corr[3];
353    float A[3][3];
354    float gain[3];
355    int   gain_cdbk_size;
356    float *gain_cdbk;
357    ltp_params *params;
358    params = (ltp_params*) par;
359    gain_cdbk=params->gain_cdbk;
360    gain_cdbk_size=1<<params->gain_bits;
361    tmp = PUSH(stack, 3*nsf);
362    tmp2 = PUSH(stack, 3*nsf);
363
364    x[0]=tmp;
365    x[1]=tmp+nsf;
366    x[2]=tmp+2*nsf;
367
368    e[0]=tmp2;
369    e[1]=tmp2+nsf;
370    e[2]=tmp2+2*nsf;
371    
372    for (i=0;i<3;i++)
373    {
374       int pp=pitch+1-i;
375       for (j=0;j<nsf;j++)
376       {
377          if (j-pp<0)
378             e[i][j]=exc2[j-pp];
379          else
380             e[i][j]=exc2[j-pp-pitch];
381       }
382       residue_zero(e[i],awk1,x[i],nsf,p);
383       syn_filt_zero(x[i],ak,x[i],nsf,p);
384       syn_filt_zero(x[i],awk2,x[i],nsf,p);
385    }
386
387    for (i=0;i<3;i++)
388       corr[i]=xcorr(x[i],target,nsf);
389    
390    for (i=0;i<3;i++)
391       for (j=0;j<=i;j++)
392          A[i][j]=A[j][i]=xcorr(x[i],x[j],nsf);
393    
394    {
395       int j;
396       float C[9];
397       float *ptr=gain_cdbk;
398       int best_cdbk=0;
399       float best_sum=0;
400       C[0]=corr[2];
401       C[1]=corr[1];
402       C[2]=corr[0];
403       C[3]=A[1][2];
404       C[4]=A[0][1];
405       C[5]=A[0][2];
406       C[6]=A[2][2];
407       C[7]=A[1][1];
408       C[8]=A[0][0];
409       
410       for (i=0;i<gain_cdbk_size;i++)
411       {
412          float sum=0;
413          ptr = gain_cdbk+12*i;
414          for (j=0;j<9;j++)
415             sum+=C[j]*ptr[j+3];
416          if (0) {
417             float tot=ptr[0]+ptr[1]+ptr[2];
418             if (tot < 1.1)
419                sum *= 1+.15*tot;
420          }
421          if (sum>best_sum || i==0)
422          {
423             best_sum=sum;
424             best_cdbk=i;
425          }
426       }
427       gain[0] = gain_cdbk[best_cdbk*12];
428       gain[1] = gain_cdbk[best_cdbk*12+1];
429       gain[2] = gain_cdbk[best_cdbk*12+2];
430
431       *cdbk_index=best_cdbk;
432    }
433    
434    for (i=0;i<nsf;i++)
435       exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
436 #ifdef DEBUG
437    printf ("3-tap pitch = %d, gains = [%f %f %f]\n",pitch, gain[0], gain[1], gain[2]);
438 #endif
439    {
440       float tmp1=0,tmp2=0;
441       for (i=0;i<nsf;i++)
442          tmp1+=target[i]*target[i];
443       for (i=0;i<nsf;i++)
444          tmp2+=(target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i])
445          * (target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i]);
446 #ifdef DEBUG
447       printf ("prediction gain = %f\n",tmp1/(tmp2+1));
448 #endif
449       return tmp2;
450    }
451 }
452
453
454 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
455 int pitch_search_3tap(
456 float target[],                 /* Target vector */
457 float *sw,
458 float ak[],                     /* LPCs for this subframe */
459 float awk1[],                   /* Weighted LPCs #1 for this subframe */
460 float awk2[],                   /* Weighted LPCs #2 for this subframe */
461 float exc[],                    /* Excitation */
462 void *par,
463 int   start,                    /* Smallest pitch value allowed */
464 int   end,                      /* Largest pitch value allowed */
465 int   p,                        /* Number of LPC coeffs */
466 int   nsf,                      /* Number of samples in subframe */
467 SpeexBits *bits,
468 float *stack,
469 float *exc2
470 )
471 {
472    int i,j;
473    int cdbk_index, pitch, ol_pitch, best_gain_index=0;
474    float *best_exc;
475    int best_pitch=0;
476    float err, best_err=-1;
477    int N=1;
478    ltp_params *params;
479    params = (ltp_params*) par;
480
481    best_exc=PUSH(stack,nsf);
482
483    open_loop_pitch(sw, start, nsf, nsf, &ol_pitch);
484
485    if (ol_pitch-N<start)
486       ol_pitch=start+N;
487    if (ol_pitch+N>end)
488       ol_pitch=end-N;
489
490    for (pitch=ol_pitch-N; pitch<=ol_pitch+N; pitch++)
491    {
492       for (j=0;j<nsf;j++)
493          exc[j]=0;
494       err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, par, pitch, p, nsf,
495                              bits, stack, exc2, &cdbk_index);
496       if (err<best_err || best_err<0)
497       {
498          for (j=0;j<nsf;j++)
499             best_exc[j]=exc[j];
500          best_err=err;
501          best_pitch=pitch;
502          best_gain_index=cdbk_index;
503       }
504    }
505
506
507    open_loop_pitch(sw, nsf, end, nsf, &ol_pitch);
508
509    for (pitch=ol_pitch-N; pitch<=ol_pitch+N; pitch++)
510    {
511       for (j=0;j<nsf;j++)
512          exc[j]=0;
513       err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, par, pitch, p, nsf,
514                              bits, stack, exc2, &cdbk_index);
515       if (err<best_err || best_err<0)
516       {
517          for (j=0;j<nsf;j++)
518             best_exc[j]=exc[j];
519          best_err=err;
520          best_pitch=pitch;
521          best_gain_index=cdbk_index;
522       }
523    }
524
525    /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/
526    speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
527    speex_bits_pack(bits, best_gain_index, params->gain_bits);
528    /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/
529    for (i=0;i<nsf;i++)
530       exc[i]=best_exc[i];
531
532    POP(stack);
533    return pitch;
534 }
535
536
537 void pitch_unquant_3tap(
538 float exc[],                    /* Excitation */
539 int   start,                    /* Smallest pitch value allowed */
540 int   end,                      /* Largest pitch value allowed */
541 void *par,
542 int   nsf,                      /* Number of samples in subframe */
543 int *pitch_val,
544 float *gain_val,
545 SpeexBits *bits,
546 float *stack,
547 int lost)
548 {
549    int i;
550    int pitch;
551    int gain_index;
552    float gain[3];
553    float *gain_cdbk;
554    ltp_params *params;
555    params = (ltp_params*) par;
556    gain_cdbk=params->gain_cdbk;
557
558    pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
559    pitch += start;
560    gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
561    /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
562    gain[0] = gain_cdbk[gain_index*12];
563    gain[1] = gain_cdbk[gain_index*12+1];
564    gain[2] = gain_cdbk[gain_index*12+2];
565    if (lost)
566    {
567       float gain_sum = gain[0]+gain[1]+gain[2];
568       if (gain_sum>.8)
569       {
570          float fact = .8/gain_sum;
571          for (i=0;i<3;i++)
572             gain[i]*=fact;
573       }
574    }
575
576    *pitch_val = pitch;
577    *gain_val = gain[0]+gain[1]+gain[2];
578
579 #ifdef DEBUG
580    printf ("unquantized pitch: %d %f %f %f\n", pitch, gain[0], gain[1], gain[2]);
581 #endif
582
583    for(i=0;i<nsf;i++)
584       exc[i]=0;
585
586    {
587       float *e[3];
588       float *tmp2;
589       tmp2=PUSH(stack, 3*nsf);
590       e[0]=tmp2;
591       e[1]=tmp2+nsf;
592       e[2]=tmp2+2*nsf;
593       
594       for (i=0;i<3;i++)
595       {
596          int j;
597          int pp=pitch+1-i;
598          for (j=0;j<nsf;j++)
599          {
600             if (j-pp<0)
601                e[i][j]=exc[j-pp];
602             else
603                e[i][j]=exc[j-pp-pitch];
604          }
605       }
606       for (i=0;i<nsf;i++)
607          exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
608       
609       POP(stack);
610    }
611 }