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