Added low bit-rate (8 kbps) narrowband mode. It is still sub-optimal but
[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
31 void open_loop_nbest_pitch(float *sw, int start, int end, int len, int *pitch, int N, float *stack)
32 {
33    int i,j,k;
34    float corr;
35    float energy;
36    float score, *best_score;
37
38    best_score = PUSH(stack,N);
39    for (i=0;i<N;i++)
40         best_score[i]=-1;
41    energy=xcorr(sw-start, sw-start, len);
42    for (i=start;i<=end;i++)
43    {
44       corr=xcorr(sw, sw-i, len);
45       score=corr*corr/(energy+1);
46       if (score>best_score[N-1])
47       {
48          for (j=0;j<N;j++)
49          {
50             if (score > best_score[j])
51             {
52                for (k=N-1;k>j;k--)
53                {
54                   best_score[k]=best_score[k-1];
55                   pitch[k]=pitch[k-1];
56                }
57                best_score[j]=score;
58                pitch[j]=i;
59                break;
60             }
61          }
62       }
63       /* Update energy for next pitch*/
64       energy+=sw[-i-1]*sw[-i-1] - sw[-i+len-1]*sw[-i+len-1];
65    }
66
67    POP(stack);
68 }
69
70
71
72 int pitch_search_3tap_unquant(
73 float target[],                 /* Target vector */
74 float *sw,
75 float ak[],                     /* LPCs for this subframe */
76 float awk1[],                   /* Weighted LPCs #1 for this subframe */
77 float awk2[],                   /* Weighted LPCs #2 for this subframe */
78 float exc[],                    /* Excitation */
79 void *par,
80 int   start,                    /* Smallest pitch value allowed */
81 int   end,                      /* Largest pitch value allowed */
82 int   p,                        /* Number of LPC coeffs */
83 int   nsf,                      /* Number of samples in subframe */
84 SpeexBits *bits,
85 float *stack,
86 float *exc2
87 )
88 {
89    int i,j;
90    float *tmp;
91    float *x[3];
92    float corr[3];
93    float A[3][3];
94    int pitch;
95    float gain[3];
96    tmp = PUSH(stack, 3*nsf);
97    x[0]=tmp;
98    x[1]=tmp+nsf;
99    x[2]=tmp+2*nsf;
100
101    /* Perform closed-loop 1-tap search*/
102    overlap_cb_search(target, ak, awk1, awk2,
103                      &exc[-end], end-start+1, gain, &pitch, p,
104                      nsf, stack);
105    /* Real pitch value */
106    pitch=end-pitch;
107    
108    
109    for (i=0;i<3;i++)
110    {
111       residue_zero(&exc[-pitch-1+i],awk1,x[i],nsf,p);
112       syn_filt_zero(x[i],ak,x[i],nsf,p);
113       syn_filt_zero(x[i],awk2,x[i],nsf,p);
114    }
115
116    for (i=0;i<3;i++)
117       corr[i]=xcorr(x[i],target,nsf);
118    
119    for (i=0;i<3;i++)
120       for (j=0;j<=i;j++)
121          A[i][j]=A[j][i]=xcorr(x[i],x[j],nsf);
122    A[0][0]+=1;
123    A[1][1]+=1;
124    A[2][2]+=1;
125    {
126       float tmp=A[1][0]/A[0][0];
127       for (i=0;i<3;i++)
128          A[1][i] -= tmp*A[0][i];
129       corr[1] -= tmp*corr[0];
130
131       tmp=A[2][0]/A[0][0];
132       for (i=0;i<3;i++)
133          A[2][i] -= tmp*A[0][i];
134       corr[2] -= tmp*corr[0];
135       
136       tmp=A[2][1]/A[1][1];
137       A[2][2] -= tmp*A[1][2];
138       corr[2] -= tmp*corr[1];
139
140       corr[2] /= A[2][2];
141       corr[1] = (corr[1] - A[1][2]*corr[2])/A[1][1];
142       corr[0] = (corr[0] - A[0][2]*corr[2] - A[0][1]*corr[1])/A[0][0];
143       /*printf ("\n%f %f %f\n", best_corr[0], best_corr[1], best_corr[2]);*/
144
145    }
146    /* Put gains in right order */
147    gain[0]=corr[2];gain[1]=corr[1];gain[2]=corr[0];
148
149    for (i=nsf-1;i>=0;i--)
150       exc[i]=gain[0]*exc[i-pitch+1]+gain[1]*exc[i-pitch]+gain[2]*exc[i-pitch-1];
151 #if 0
152    if (0){
153       float tmp1=0,tmp2=0;
154       for (i=0;i<nsf;i++)
155          tmp1+=target[i]*target[i];
156       for (i=0;i<nsf;i++)
157          tmp2+=(target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i])
158          * (target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i]);
159 #ifdef DEBUG
160       printf ("prediction gain = %f\n",tmp1/(tmp2+1));
161 #endif
162       return tmp1/(tmp2+1);
163    }
164 #endif
165    POP(stack);
166    return pitch;
167 }
168
169
170 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
171 float pitch_gain_search_3tap(
172 float target[],                 /* Target vector */
173 float ak[],                     /* LPCs for this subframe */
174 float awk1[],                   /* Weighted LPCs #1 for this subframe */
175 float awk2[],                   /* Weighted LPCs #2 for this subframe */
176 float exc[],                    /* Excitation */
177 void *par,
178 int   pitch,                    /* Pitch value */
179 int   p,                        /* Number of LPC coeffs */
180 int   nsf,                      /* Number of samples in subframe */
181 SpeexBits *bits,
182 float *stack,
183 float *exc2,
184 int  *cdbk_index
185 )
186 {
187    int i,j;
188    float *tmp, *tmp2;
189    float *x[3];
190    float *e[3];
191    float corr[3];
192    float A[3][3];
193    float gain[3];
194    int   gain_cdbk_size;
195    float *gain_cdbk;
196    float err1,err2;
197    ltp_params *params;
198    params = (ltp_params*) par;
199    gain_cdbk=params->gain_cdbk;
200    gain_cdbk_size=1<<params->gain_bits;
201    tmp = PUSH(stack, 3*nsf);
202    tmp2 = PUSH(stack, 3*nsf);
203
204    x[0]=tmp;
205    x[1]=tmp+nsf;
206    x[2]=tmp+2*nsf;
207
208    e[0]=tmp2;
209    e[1]=tmp2+nsf;
210    e[2]=tmp2+2*nsf;
211    
212    for (i=0;i<3;i++)
213    {
214       int pp=pitch+1-i;
215       for (j=0;j<nsf;j++)
216       {
217          if (j-pp<0)
218             e[i][j]=exc2[j-pp];
219          else if (j-pp-pitch<0)
220             e[i][j]=exc2[j-pp-pitch];
221          else
222             e[i][j]=0;
223       }
224
225       if (p==10)
226       {
227          syn_percep_zero(e[i], ak, awk1, awk2, x[i], nsf, p);
228       } else {
229          residue_zero(e[i],awk1,x[i],nsf,p);
230          syn_filt_zero(x[i],ak,x[i],nsf,p);
231          syn_filt_zero(x[i],awk2,x[i],nsf,p);
232       }
233    }
234
235    for (i=0;i<3;i++)
236       corr[i]=xcorr(x[i],target,nsf);
237    
238    for (i=0;i<3;i++)
239       for (j=0;j<=i;j++)
240          A[i][j]=A[j][i]=xcorr(x[i],x[j],nsf);
241    
242    {
243       int j;
244       float C[9];
245       float *ptr=gain_cdbk;
246       int best_cdbk=0;
247       float best_sum=0;
248       C[0]=corr[2];
249       C[1]=corr[1];
250       C[2]=corr[0];
251       C[3]=A[1][2];
252       C[4]=A[0][1];
253       C[5]=A[0][2];
254       C[6]=A[2][2];
255       C[7]=A[1][1];
256       C[8]=A[0][0];
257       
258       for (i=0;i<gain_cdbk_size;i++)
259       {
260          float sum=0;
261          ptr = gain_cdbk+12*i;
262          for (j=0;j<9;j++)
263             sum+=C[j]*ptr[j+3];
264          if (0) {
265             float tot=ptr[0]+ptr[1]+ptr[2];
266             if (tot < 1.1)
267                sum *= 1+.15*tot;
268          }
269          if (sum>best_sum || i==0)
270          {
271             best_sum=sum;
272             best_cdbk=i;
273          }
274       }
275       gain[0] = gain_cdbk[best_cdbk*12];
276       gain[1] = gain_cdbk[best_cdbk*12+1];
277       gain[2] = gain_cdbk[best_cdbk*12+2];
278
279       *cdbk_index=best_cdbk;
280    }
281    /* Calculate gains by matrix inversion... (unquantized) */
282    if (0) {
283       float tmp;
284       float B[3][3];
285       A[0][0]+=1;
286       A[1][1]+=1;
287       A[2][2]+=1;
288       
289       for (i=0;i<3;i++)
290          for (j=0;j<3;j++)
291             B[i][j]=A[i][j];
292
293
294       tmp=A[1][0]/A[0][0];
295       for (i=0;i<3;i++)
296          A[1][i] -= tmp*A[0][i];
297       corr[1] -= tmp*corr[0];
298
299       tmp=A[2][0]/A[0][0];
300       for (i=0;i<3;i++)
301          A[2][i] -= tmp*A[0][i];
302       corr[2] -= tmp*corr[0];
303       
304       tmp=A[2][1]/A[1][1];
305       A[2][2] -= tmp*A[1][2];
306       corr[2] -= tmp*corr[1];
307
308       corr[2] /= A[2][2];
309       corr[1] = (corr[1] - A[1][2]*corr[2])/A[1][1];
310       corr[0] = (corr[0] - A[0][2]*corr[2] - A[0][1]*corr[1])/A[0][0];
311       /*printf ("\n%f %f %f\n", best_corr[0], best_corr[1], best_corr[2]);*/
312
313    
314       /* Put gains in right order */
315       gain[0]=corr[2];gain[1]=corr[1];gain[2]=corr[0];
316
317       {
318          float gain_sum = gain[0]+gain[1]+gain[2];
319          if (fabs(gain_sum)>2.5)
320          {
321             float fact = 2.5/gain_sum;
322             for (i=0;i<3;i++)
323                gain[i]*=fact;
324          }
325       }
326       
327    }
328    
329    for (i=0;i<nsf;i++)
330       exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
331 #ifdef DEBUG
332    printf ("3-tap pitch = %d, gains = [%f %f %f]\n",pitch, gain[0], gain[1], gain[2]);
333 #endif
334    
335    err1=0;
336    err2=0;
337    for (i=0;i<nsf;i++)
338       err1+=target[i]*target[i];
339    for (i=0;i<nsf;i++)
340       err2+=(target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i])
341       * (target[i]-gain[2]*x[0][i]-gain[1]*x[1][i]-gain[0]*x[2][i]);
342 #ifdef DEBUG
343    printf ("prediction gain = %f\n",err1/(err2+1));
344 #endif
345    
346    POP(stack);
347    POP(stack);
348    return err2;
349 }
350
351
352 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
353 int pitch_search_3tap(
354 float target[],                 /* Target vector */
355 float *sw,
356 float ak[],                     /* LPCs for this subframe */
357 float awk1[],                   /* Weighted LPCs #1 for this subframe */
358 float awk2[],                   /* Weighted LPCs #2 for this subframe */
359 float exc[],                    /* Excitation */
360 void *par,
361 int   start,                    /* Smallest pitch value allowed */
362 int   end,                      /* Largest pitch value allowed */
363 int   p,                        /* Number of LPC coeffs */
364 int   nsf,                      /* Number of samples in subframe */
365 SpeexBits *bits,
366 float *stack,
367 float *exc2
368 )
369 {
370    int i,j;
371    int cdbk_index, pitch, best_gain_index=0;
372    float *best_exc;
373    int best_pitch=0;
374    float err, best_err=-1;
375    int N=3;
376    ltp_params *params;
377    int *nbest=(int*)PUSH(stack, N);
378    params = (ltp_params*) par;
379    
380    best_exc=PUSH(stack,nsf);
381    
382    
383    open_loop_nbest_pitch(sw, start, end, nsf, nbest, N, stack);
384    for (i=0;i<N;i++)
385    {
386       pitch=nbest[i];
387       for (j=0;j<nsf;j++)
388          exc[j]=0;
389       err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, par, pitch, p, nsf,
390                                  bits, stack, exc2, &cdbk_index);
391       if (err<best_err || best_err<0)
392       {
393          for (j=0;j<nsf;j++)
394             best_exc[j]=exc[j];
395          best_err=err;
396          best_pitch=pitch;
397          best_gain_index=cdbk_index;
398       }
399    }
400    
401    /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/
402    speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
403    speex_bits_pack(bits, best_gain_index, params->gain_bits);
404    /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/
405    for (i=0;i<nsf;i++)
406       exc[i]=best_exc[i];
407
408    POP(stack);
409    POP(stack);
410
411    return pitch;
412 }
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 void *par,
420 int   nsf,                      /* Number of samples in subframe */
421 int *pitch_val,
422 float *gain_val,
423 SpeexBits *bits,
424 float *stack,
425 int lost)
426 {
427    int i;
428    int pitch;
429    int gain_index;
430    float gain[3];
431    float *gain_cdbk;
432    ltp_params *params;
433    params = (ltp_params*) par;
434    gain_cdbk=params->gain_cdbk;
435
436    pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
437    pitch += start;
438    gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
439    /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
440    gain[0] = gain_cdbk[gain_index*12];
441    gain[1] = gain_cdbk[gain_index*12+1];
442    gain[2] = gain_cdbk[gain_index*12+2];
443    if (lost)
444    {
445       float gain_sum = gain[0]+gain[1]+gain[2];
446       if (gain_sum>.8)
447       {
448          float fact = .8/gain_sum;
449          for (i=0;i<3;i++)
450             gain[i]*=fact;
451       }
452    }
453
454    *pitch_val = pitch;
455    /**gain_val = gain[0]+gain[1]+gain[2];*/
456    gain_val[0]=gain[0];
457    gain_val[1]=gain[1];
458    gain_val[2]=gain[2];
459
460 #ifdef DEBUG
461    printf ("unquantized pitch: %d %f %f %f\n", pitch, gain[0], gain[1], gain[2]);
462 #endif
463    for(i=0;i<nsf;i++)
464       exc[i]=0;
465
466    {
467       float *e[3];
468       float *tmp2;
469       tmp2=PUSH(stack, 3*nsf);
470       e[0]=tmp2;
471       e[1]=tmp2+nsf;
472       e[2]=tmp2+2*nsf;
473       
474       for (i=0;i<3;i++)
475       {
476          int j;
477          int pp=pitch+1-i;
478          for (j=0;j<nsf;j++)
479          {
480             if (j-pp<0)
481                e[i][j]=exc[j-pp];
482             else
483                e[i][j]=exc[j-pp-pitch];
484          }
485       }
486       for (i=0;i<nsf;i++)
487          exc[i]=gain[0]*e[2][i]+gain[1]*e[1][i]+gain[2]*e[0][i];
488       
489       POP(stack);
490    }
491 }