speex.[ch] renamed to nb_celp.[ch] for consistency
[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 void pitch_search_3tap_unquant(
234 float target[],                 /* Target vector */
235 float ak[],                     /* LPCs for this subframe */
236 float awk1[],                   /* Weighted LPCs #1 for this subframe */
237 float awk2[],                   /* Weighted LPCs #2 for this subframe */
238 float exc[],                    /* Excitation */
239 void *par,
240 int   start,                    /* Smallest pitch value allowed */
241 int   end,                      /* Largest pitch value allowed */
242 int   p,                        /* Number of LPC coeffs */
243 int   nsf,                      /* Number of samples in subframe */
244 FrameBits *bits,
245 float *stack
246 )
247 {
248    int i,j;
249    float *tmp;
250    float *x[3];
251    float corr[3];
252    float A[3][3];
253    int pitch;
254    float gain[3];
255    tmp = PUSH(stack, 3*nsf);
256    x[0]=tmp;
257    x[1]=tmp+nsf;
258    x[2]=tmp+2*nsf;
259
260    /* Perform closed-loop 1-tap search*/
261    overlap_cb_search(target, ak, awk1, awk2,
262                      &exc[-end], end-start+1, gain, &pitch, p,
263                      nsf, stack);
264    /* Real pitch value */
265    pitch=end-pitch;
266    
267    
268    for (i=0;i<3;i++)
269    {
270       residue_zero(&exc[-pitch-1+i],awk1,x[i],nsf,p);
271       syn_filt_zero(x[i],ak,x[i],nsf,p);
272       syn_filt_zero(x[i],awk2,x[i],nsf,p);
273    }
274
275    for (i=0;i<3;i++)
276       corr[i]=xcorr(x[i],target,nsf);
277    
278    for (i=0;i<3;i++)
279       for (j=0;j<=i;j++)
280          A[i][j]=A[j][i]=xcorr(x[i],x[j],nsf);
281    A[0][0]+=1;
282    A[1][1]+=1;
283    A[2][2]+=1;
284    {
285       float tmp=A[1][0]/A[0][0];
286       for (i=0;i<3;i++)
287          A[1][i] -= tmp*A[0][i];
288       corr[1] -= tmp*corr[0];
289
290       tmp=A[2][0]/A[0][0];
291       for (i=0;i<3;i++)
292          A[2][i] -= tmp*A[0][i];
293       corr[2] -= tmp*corr[0];
294       
295       tmp=A[2][1]/A[1][1];
296       A[2][2] -= tmp*A[1][2];
297       corr[2] -= tmp*corr[1];
298
299       corr[2] /= A[2][2];
300       corr[1] = (corr[1] - A[1][2]*corr[2])/A[1][1];
301       corr[0] = (corr[0] - A[0][2]*corr[2] - A[0][1]*corr[1])/A[0][0];
302       /*printf ("\n%f %f %f\n", best_corr[0], best_corr[1], best_corr[2]);*/
303
304    }
305    /* Put gains in right order */
306    gain[0]=corr[2];gain[1]=corr[1];gain[2]=corr[0];
307
308    for (i=nsf-1;i>=0;i--)
309       exc[i]=gain[0]*exc[i-pitch+1]+gain[1]*exc[i-pitch]+gain[2]*exc[i-pitch-1];
310 #if 0
311    if (0){
312       float tmp1=0,tmp2=0;
313       for (i=0;i<nsf;i++)
314          tmp1+=target[i]*target[i];
315       for (i=0;i<nsf;i++)
316          tmp2+=(target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i])
317          * (target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i]);
318 #ifdef DEBUG
319       printf ("prediction gain = %f\n",tmp1/(tmp2+1));
320 #endif
321       return tmp1/(tmp2+1);
322    }
323 #endif
324    POP(stack);
325 }
326
327
328 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
329 float pitch_gain_search_3tap(
330 float target[],                 /* Target vector */
331 float ak[],                     /* LPCs for this subframe */
332 float awk1[],                   /* Weighted LPCs #1 for this subframe */
333 float awk2[],                   /* Weighted LPCs #2 for this subframe */
334 float exc[],                    /* Excitation */
335 void *par,
336 int   pitch,                    /* Pitch value */
337 int   p,                        /* Number of LPC coeffs */
338 int   nsf,                      /* Number of samples in subframe */
339 FrameBits *bits,
340 float *stack,
341 float *exc2,
342 int  *cdbk_index
343 )
344 {
345    int i,j;
346    float *tmp, *tmp2;
347    float *x[3];
348    float *e[3];
349    float corr[3];
350    float A[3][3];
351    float gain[3];
352    int   gain_cdbk_size;
353    float *gain_cdbk;
354    ltp_params *params;
355    params = (ltp_params*) par;
356    gain_cdbk=params->gain_cdbk;
357    gain_cdbk_size=1<<params->gain_bits;
358    tmp = PUSH(stack, 3*nsf);
359    tmp2 = PUSH(stack, 3*nsf);
360
361    x[0]=tmp;
362    x[1]=tmp+nsf;
363    x[2]=tmp+2*nsf;
364
365    e[0]=tmp2;
366    e[1]=tmp2+nsf;
367    e[2]=tmp2+2*nsf;
368    
369    for (i=0;i<3;i++)
370    {
371       int pp=pitch+1-i;
372       for (j=0;j<nsf;j++)
373       {
374          if (j-pp<0)
375             e[i][j]=exc2[j-pp];
376          else
377             e[i][j]=exc2[j-pp-pitch];
378       }
379       residue_zero(e[i],awk1,x[i],nsf,p);
380       syn_filt_zero(x[i],ak,x[i],nsf,p);
381       syn_filt_zero(x[i],awk2,x[i],nsf,p);
382    }
383
384    for (i=0;i<3;i++)
385       corr[i]=xcorr(x[i],target,nsf);
386    
387    for (i=0;i<3;i++)
388       for (j=0;j<=i;j++)
389          A[i][j]=A[j][i]=xcorr(x[i],x[j],nsf);
390    
391    {
392       int j;
393       float C[9];
394       float *ptr=gain_cdbk;
395       int best_cdbk=0;
396       float best_sum=0;
397       C[0]=corr[2];
398       C[1]=corr[1];
399       C[2]=corr[0];
400       C[3]=A[1][2];
401       C[4]=A[0][1];
402       C[5]=A[0][2];
403       C[6]=A[2][2];
404       C[7]=A[1][1];
405       C[8]=A[0][0];
406       
407       for (i=0;i<gain_cdbk_size;i++)
408       {
409          float sum=0;
410          ptr = gain_cdbk+12*i;
411          for (j=0;j<9;j++)
412             sum+=C[j]*ptr[j+3];
413          if (0) {
414             float tot=ptr[0]+ptr[1]+ptr[2];
415             if (tot < 1.1)
416                sum *= 1+.15*tot;
417          }
418          if (sum>best_sum || i==0)
419          {
420             best_sum=sum;
421             best_cdbk=i;
422          }
423       }
424       gain[0] = gain_cdbk[best_cdbk*12];
425       gain[1] = gain_cdbk[best_cdbk*12+1];
426       gain[2] = gain_cdbk[best_cdbk*12+2];
427
428       *cdbk_index=best_cdbk;
429    }
430    
431    for (i=0;i<nsf;i++)
432       exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
433 #ifdef DEBUG
434    printf ("3-tap pitch = %d, gains = [%f %f %f]\n",pitch, gain[0], gain[1], gain[2]);
435 #endif
436    {
437       float tmp1=0,tmp2=0;
438       for (i=0;i<nsf;i++)
439          tmp1+=target[i]*target[i];
440       for (i=0;i<nsf;i++)
441          tmp2+=(target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i])
442          * (target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i]);
443 #ifdef DEBUG
444       printf ("prediction gain = %f\n",tmp1/(tmp2+1));
445 #endif
446       return tmp2;
447    }
448 }
449
450
451 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
452 int pitch_search_3tap(
453 float target[],                 /* Target vector */
454 float *sw,
455 float ak[],                     /* LPCs for this subframe */
456 float awk1[],                   /* Weighted LPCs #1 for this subframe */
457 float awk2[],                   /* Weighted LPCs #2 for this subframe */
458 float exc[],                    /* Excitation */
459 void *par,
460 int   start,                    /* Smallest pitch value allowed */
461 int   end,                      /* Largest pitch value allowed */
462 int   p,                        /* Number of LPC coeffs */
463 int   nsf,                      /* Number of samples in subframe */
464 FrameBits *bits,
465 float *stack,
466 float *exc2
467 )
468 {
469    int i,j;
470    int cdbk_index, pitch, ol_pitch, best_gain_index=0;
471    float *best_exc;
472    int best_pitch=0;
473    float err, best_err=-1;
474    int N=1;
475    ltp_params *params;
476    params = (ltp_params*) par;
477
478    best_exc=PUSH(stack,nsf);
479
480    open_loop_pitch(sw, start, nsf, nsf, &ol_pitch);
481
482    if (ol_pitch-N<start)
483       ol_pitch=start+N;
484    if (ol_pitch+N>end)
485       ol_pitch=end-N;
486
487    for (pitch=ol_pitch-N; pitch<=ol_pitch+N; pitch++)
488    {
489       for (j=0;j<nsf;j++)
490          exc[j]=0;
491       err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, par, pitch, p, nsf,
492                              bits, stack, exc2, &cdbk_index);
493       if (err<best_err || best_err<0)
494       {
495          for (j=0;j<nsf;j++)
496             best_exc[j]=exc[j];
497          best_err=err;
498          best_pitch=pitch;
499          best_gain_index=cdbk_index;
500       }
501    }
502
503
504    open_loop_pitch(sw, nsf, end, nsf, &ol_pitch);
505
506    for (pitch=ol_pitch-N; pitch<=ol_pitch+N; pitch++)
507    {
508       for (j=0;j<nsf;j++)
509          exc[j]=0;
510       err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, par, pitch, p, nsf,
511                              bits, stack, exc2, &cdbk_index);
512       if (err<best_err || best_err<0)
513       {
514          for (j=0;j<nsf;j++)
515             best_exc[j]=exc[j];
516          best_err=err;
517          best_pitch=pitch;
518          best_gain_index=cdbk_index;
519       }
520    }
521
522    speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
523    speex_bits_pack(bits, best_gain_index, params->gain_bits);
524    printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);
525    for (i=0;i<nsf;i++)
526       exc[i]=best_exc[i];
527
528    POP(stack);
529    return pitch;
530 }
531
532
533 void pitch_unquant_3tap(
534 float exc[],                    /* Excitation */
535 int   start,                    /* Smallest pitch value allowed */
536 int   end,                      /* Largest pitch value allowed */
537 void *par,
538 int   nsf,                      /* Number of samples in subframe */
539 FrameBits *bits,
540 float *stack
541 )
542 {
543    int i;
544    int pitch;
545    int gain_index;
546    float gain[3];
547    float *gain_cdbk;
548    ltp_params *params;
549    params = (ltp_params*) par;
550    gain_cdbk=params->gain_cdbk;
551
552    pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
553    pitch += start;
554    gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
555    printf ("decode pitch: %d %d\n", pitch, gain_index);
556    gain[0] = gain_cdbk[gain_index*12];
557    gain[1] = gain_cdbk[gain_index*12+1];
558    gain[2] = gain_cdbk[gain_index*12+2];
559 #ifdef DEBUG
560    printf ("unquantized pitch: %d %f %f %f\n", pitch, gain[0], gain[1], gain[2]);
561 #endif
562
563    for(i=0;i<nsf;i++)
564       exc[i]=0;
565
566    {
567       float *e[3];
568       float *tmp2;
569       tmp2=PUSH(stack, 3*nsf);
570       e[0]=tmp2;
571       e[1]=tmp2+nsf;
572       e[2]=tmp2+2*nsf;
573       
574       for (i=0;i<3;i++)
575       {
576          int j;
577          int pp=pitch+1-i;
578          for (j=0;j<nsf;j++)
579          {
580             if (j-pp<0)
581                e[i][j]=exc[j-pp];
582             else
583                e[i][j]=exc[j-pp-pitch];
584          }
585       }
586       for (i=0;i<nsf;i++)
587          exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
588       
589       POP(stack);
590    }
591 }