Fixed warnings
[speexdsp.git] / libspeex / cb_search.c
1 /* Original copyright */
2 /*-----------------------------------------------------------------------*\
3
4     FILE........: GAINSHAPE.C
5     TYPE........: C Module
6     AUTHOR......: David Rowe
7     COMPANY.....: Voicetronix
8     DATE CREATED: 19/2/02
9
10     General gain-shape codebook search.
11
12 \*-----------------------------------------------------------------------*/
13
14
15 /* Modified by Jean-Marc Valin 2002
16
17    This library is free software; you can redistribute it and/or
18    modify it under the terms of the GNU Lesser General Public
19    License as published by the Free Software Foundation; either
20    version 2.1 of the License, or (at your option) any later version.
21    
22    This library is distributed in the hope that it will be useful,
23    but WITHOUT ANY WARRANTY; without even the implied warranty of
24    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
25    Lesser General Public License for more details.
26    
27    You should have received a copy of the GNU Lesser General Public
28    License along with this library; if not, write to the Free Software
29    Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
30 */
31
32
33
34 #include <stdlib.h>
35 #include <cb_search.h>
36 #include "filters.h"
37 #include <math.h>
38 #include <stdio.h>
39 #include "stack_alloc.h"
40 #include "vq.h"
41 #include "matrix.h"
42
43 #define min(a,b) ((a) < (b) ? (a) : (b))
44 #define max(a,b) ((a) > (b) ? (a) : (b))
45
46
47 static float scal_gains4[16] = {
48    0.27713,
49    0.49282,
50    0.69570,
51    0.90786,
52    1.14235,
53    1.42798,
54    1.80756,
55    2.42801
56 };
57
58
59 /*---------------------------------------------------------------------------*\
60                                                                              
61  void overlap_cb_search()                                                             
62                                                                               
63  Searches a gain/shape codebook consisting of overlapping entries for the    
64  closest vector to the target.  Gives identical results to search() above   
65  buts uses fast end correction algorithm for the synthesis of response       
66  vectors.                                                                     
67                                                                              
68 \*---------------------------------------------------------------------------*/
69
70 float overlap_cb_search(
71 float target[],                 /* target vector */
72 float ak[],                     /* LPCs for this subframe */
73 float awk1[],                   /* Weighted LPCs for this subframe */
74 float awk2[],                   /* Weighted LPCs for this subframe */
75 float codebook[],               /* overlapping codebook */
76 int   entries,                  /* number of overlapping entries to search */
77 float *gain,                    /* gain of optimum entry */
78 int   *index,                   /* index of optimum entry */
79 int   p,                        /* number of LPC coeffs */
80 int   nsf,                      /* number of samples in subframe */
81 float *stack
82 )
83 {
84   float *resp;                  /* zero state response to current entry */
85   float *h;                     /* impulse response of synthesis filter */
86   float *impulse;               /* excitation vector containing one impulse */
87   float d,e,g,score;            /* codebook searching variables */
88   float bscore;                 /* score of "best" vector so far */
89   int i,k;                      /* loop variables */
90
91   /* Initialise */
92   
93   /*resp = (float*)malloc(sizeof(float)*nsf);
94   h = (float*)malloc(sizeof(float)*nsf);
95   impulse = (float*)malloc(sizeof(float)*nsf);
96   */
97   resp=PUSH(stack, nsf);
98   h=PUSH(stack, nsf);
99   impulse=PUSH(stack, nsf);
100
101   for(i=0; i<nsf; i++)
102     impulse[i] = 0.0;
103    
104   *gain = 0.0;
105   *index = 0;
106   bscore = 0.0;
107   impulse[0] = 1.0;
108
109   /* Calculate impulse response of  A(z/g1) / ( A(z)*(z/g2) ) */
110   residue_zero(impulse, awk1, h, nsf, p);
111   syn_filt_zero(h, ak, h, nsf, p);
112   syn_filt_zero(h, awk2, h, nsf,p);
113   
114   /* Calculate codebook zero-response */
115   residue_zero(&codebook[entries-1],awk1,resp,nsf,p);
116   syn_filt_zero(resp,ak,resp,nsf,p);
117   syn_filt_zero(resp,awk2,resp,nsf,p);
118     
119   /* Search codebook backwards using end correction for synthesis */
120   
121   for(k=entries-1; k>=0; k--) {
122
123     d = 0.0; e = 0.0;
124     for(i=0; i<nsf; i++) {
125       d += target[i]*resp[i];
126       e += resp[i]*resp[i];
127     }
128     g = d/(e+.0001);
129     score = g*d;
130     /*printf ("score: %f %f %f %f\n", target[0],d,e,score);*/
131     if (score >= bscore) {
132       bscore = score;
133       *gain = g;
134       *index = k;
135     }
136     
137     /* Synthesise next entry */
138     
139     if (k) {
140       for(i=nsf-1; i>=1; i--)
141         resp[i] = resp[i-1] + codebook[k-1]*h[i];
142       resp[0] = codebook[k-1]*h[0];
143     }
144   }
145
146   /*free(resp);
147   free(h);
148   free(impulse);*/
149   POP(stack);
150   POP(stack);
151   POP(stack);
152   return bscore;
153 }
154
155
156
157 void split_cb_search(
158 float target[],                 /* target vector */
159 float ak[],                     /* LPCs for this subframe */
160 float awk1[],                   /* Weighted LPCs for this subframe */
161 float awk2[],                   /* Weighted LPCs for this subframe */
162 void *par,                      /* Codebook/search parameters*/
163 int   p,                        /* number of LPC coeffs */
164 int   nsf,                      /* number of samples in subframe */
165 float *exc,
166 FrameBits *bits,
167 float *stack
168 )
169 {
170    int i,j, id;
171    float *resp, *E, q;
172    float *t, *r, *e;
173    float *gains;
174    int *ind;
175    float *shape_cb;
176    int shape_cb_size, subvect_size, nb_subvect;
177    float exc_energy=0;
178    split_cb_params *params;
179
180    params = (split_cb_params *) par;
181    subvect_size = params->subvect_size;
182    nb_subvect = params->nb_subvect;
183    shape_cb_size = 1<<params->shape_bits;
184    shape_cb = params->shape_cb;
185    resp = PUSH(stack, shape_cb_size*subvect_size);
186    E = PUSH(stack, shape_cb_size);
187    t = PUSH(stack, nsf);
188    r = PUSH(stack, nsf);
189    e = PUSH(stack, nsf);
190    gains = PUSH(stack, nb_subvect);
191    ind = (int*)PUSH(stack, nb_subvect);
192
193    /* Compute energy of the "real excitation" */
194    syn_filt_zero(target, awk1, e, nsf, p);
195    residue_zero(e, ak, e, nsf, p);
196    residue_zero(e, awk2, e, nsf, p);
197    for (i=0;i<nsf;i++)
198       exc_energy += e[i]*e[i];
199    exc_energy=sqrt(exc_energy/nb_subvect);
200
201    /* Quantize global ("average") gain */
202    q=log(exc_energy+.1);
203    q=floor(.5+2*(q-2));
204    if (q<0)
205       q=0;
206    if (q>15)
207       q=15;
208    id = (int)q;
209    frame_bits_pack(bits, id, 4);
210    exc_energy=exp(.5*q+2);
211
212
213    for (i=0;i<nsf;i++)
214       t[i]=target[i];
215
216    e[0]=1;
217    for (i=1;i<nsf;i++)
218       e[i]=0;
219    residue_zero(e, awk1, r, nsf, p);
220    syn_filt_zero(r, ak, r, nsf, p);
221    syn_filt_zero(r, awk2, r, nsf,p);
222    
223    /* Pre-compute codewords response and energy */
224    for (i=0;i<shape_cb_size;i++)
225    {
226       float *res = resp+i*subvect_size;
227
228       /* Compute codeword response */
229       int k;
230       for(j=0;j<subvect_size;j++)
231          res[j]=0;
232       for(j=0;j<subvect_size;j++)
233       {
234          for (k=j;k<subvect_size;k++)
235             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
236       }
237       /* Compute energy of codeword response */
238       E[i]=0;
239       for(j=0;j<subvect_size;j++)
240          E[i]+=res[j]*res[j];
241       E[i]=1/(.001+E[i]);
242    }
243
244    for (i=0;i<nb_subvect;i++)
245    {
246       int best_index=0, k, m;
247       float g, corr, best_gain=0, score, best_score=-1;
248       /* Find best codeword for current sub-vector */
249       for (j=0;j<shape_cb_size;j++)
250       {
251          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
252          score=corr*corr*E[j];
253          g = corr*E[j];
254          if (score>best_score)
255          {
256             best_index=j;
257             best_score=score;
258             best_gain=g;
259          }
260       }
261       frame_bits_pack(bits,best_index,params->shape_bits);
262       
263       /* Quantize gain */
264       {
265          int s=0, best_id;
266          best_gain /= .01+exc_energy;
267          if (best_gain<0)
268          {
269             best_gain=-best_gain;
270             s=1;
271          }
272
273          /* Find gain index (it's a scalar but we use the VQ code anyway)*/
274          best_id = vq_index(&best_gain, scal_gains4, 1, 8);
275
276          best_gain=scal_gains4[best_id];
277          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
278          if (s)
279             best_gain=-best_gain;
280          best_gain *= exc_energy;
281          frame_bits_pack(bits,s,1);
282          frame_bits_pack(bits,best_id,3);
283       }
284       ind[i]=best_index;
285       gains[i]=best_gain;
286       /* Update target for next subvector */
287       for (j=0;j<subvect_size;j++)
288       {
289          g=best_gain*shape_cb[best_index*subvect_size+j];
290          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
291             t[k] -= g*r[m];
292       }
293    }
294    
295    /* Put everything back together */
296    for (i=0;i<nb_subvect;i++)
297       for (j=0;j<subvect_size;j++)
298          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
299
300    /* Update excitation */
301    for (j=0;j<nsf;j++)
302       exc[j]+=e[j];
303    
304    /* Update target */
305    residue_zero(e, awk1, r, nsf, p);
306    syn_filt_zero(r, ak, r, nsf, p);
307    syn_filt_zero(r, awk2, r, nsf,p);
308    for (j=0;j<nsf;j++)
309       target[j]-=r[j];
310
311    
312
313
314    POP(stack);
315    POP(stack);
316    POP(stack);
317    POP(stack);
318    POP(stack);
319    POP(stack);
320 }
321
322 void split_cb_search_nogain(
323 float target[],                 /* target vector */
324 float ak[],                     /* LPCs for this subframe */
325 float awk1[],                   /* Weighted LPCs for this subframe */
326 float awk2[],                   /* Weighted LPCs for this subframe */
327 void *par,                      /* Codebook/search parameters*/
328 int   p,                        /* number of LPC coeffs */
329 int   nsf,                      /* number of samples in subframe */
330 float *exc,
331 FrameBits *bits,
332 float *stack
333 )
334 {
335    int i,j;
336    float *resp;
337    float *t, *r, *e;
338    int *ind;
339    float *shape_cb;
340    int shape_cb_size, subvect_size, nb_subvect;
341    split_cb_params *params;
342
343    params = (split_cb_params *) par;
344    subvect_size = params->subvect_size;
345    nb_subvect = params->nb_subvect;
346    shape_cb_size = 1<<params->shape_bits;
347    shape_cb = params->shape_cb;
348    resp = PUSH(stack, shape_cb_size*subvect_size);
349    t = PUSH(stack, nsf);
350    r = PUSH(stack, nsf);
351    e = PUSH(stack, nsf);
352    ind = (int*)PUSH(stack, nb_subvect);
353
354    for (i=0;i<nsf;i++)
355       t[i]=target[i];
356
357    e[0]=1;
358    for (i=1;i<nsf;i++)
359       e[i]=0;
360    residue_zero(e, awk1, r, nsf, p);
361    syn_filt_zero(r, ak, r, nsf, p);
362    syn_filt_zero(r, awk2, r, nsf,p);
363    
364    /* Pre-compute codewords response and energy */
365    for (i=0;i<shape_cb_size;i++)
366    {
367       float *res = resp+i*subvect_size;
368
369       /* Compute codeword response */
370       int k;
371       for(j=0;j<subvect_size;j++)
372          res[j]=0;
373       for(j=0;j<subvect_size;j++)
374       {
375          for (k=j;k<subvect_size;k++)
376             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
377       }
378    }
379
380    for (i=0;i<nb_subvect;i++)
381    {
382       int best_index=0, k, m;
383       float g, dist, best_dist=-1;
384       float *a, *b;
385       /* Find best codeword for current sub-vector */
386       for (j=0;j<shape_cb_size;j++)
387       {
388          dist=0;
389          a=resp+j*subvect_size;
390          b=t+subvect_size*i;
391          for (k=0;k<subvect_size;k++)
392             dist += (a[k]-b[k])*(a[k]-b[k]);
393          if (dist<best_dist || j==0)
394          {
395             best_dist=dist;
396             best_index=j;
397          }
398       }
399       /*printf ("best index: %d/%d\n", best_index, shape_cb_size);*/
400       frame_bits_pack(bits,best_index,params->shape_bits);
401
402       ind[i]=best_index;
403       /* Update target for next subvector */
404       for (j=0;j<subvect_size;j++)
405       {
406          g=shape_cb[best_index*subvect_size+j];
407          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
408             t[k] -= g*r[m];
409       }
410    }
411    
412    /* Put everything back together */
413    for (i=0;i<nb_subvect;i++)
414       for (j=0;j<subvect_size;j++)
415          e[subvect_size*i+j]=shape_cb[ind[i]*subvect_size+j];
416
417    /* Update excitation */
418    for (j=0;j<nsf;j++)
419       exc[j]+=e[j];
420    
421    /* Update target */
422    residue_zero(e, awk1, r, nsf, p);
423    syn_filt_zero(r, ak, r, nsf, p);
424    syn_filt_zero(r, awk2, r, nsf,p);
425    for (j=0;j<nsf;j++)
426       target[j]-=r[j];
427
428    
429
430
431    POP(stack);
432    POP(stack);
433    POP(stack);
434    POP(stack);
435 }
436
437
438
439
440 void split_cb_search2(
441 float target[],                 /* target vector */
442 float ak[],                     /* LPCs for this subframe */
443 float awk1[],                   /* Weighted LPCs for this subframe */
444 float awk2[],                   /* Weighted LPCs for this subframe */
445 void *par,                      /* Codebook/search parameters*/
446 int   p,                        /* number of LPC coeffs */
447 int   nsf,                      /* number of samples in subframe */
448 float *exc,
449 FrameBits *bits,
450 float *stack
451 )
452 {
453    int i,j, id;
454    float *resp, *E, q;
455    float *t, *r, *e;
456    float *gains;
457    int *ind;
458    float *shape_cb;
459    int shape_cb_size, subvect_size, nb_subvect;
460    float exc_energy=0;
461    split_cb_params *params;
462
463    params = (split_cb_params *) par;
464    subvect_size = params->subvect_size;
465    nb_subvect = params->nb_subvect;
466    shape_cb_size = 1<<params->shape_bits;
467    shape_cb = params->shape_cb;
468    resp = PUSH(stack, shape_cb_size*subvect_size);
469    E = PUSH(stack, shape_cb_size);
470    t = PUSH(stack, nsf);
471    r = PUSH(stack, nsf);
472    e = PUSH(stack, nsf);
473    gains = PUSH(stack, nb_subvect);
474    ind = (int*)PUSH(stack, nb_subvect);
475
476    /* Compute energy of the "real excitation" */
477    syn_filt_zero(target, awk1, e, nsf, p);
478    residue_zero(e, ak, e, nsf, p);
479    residue_zero(e, awk2, e, nsf, p);
480    for (i=0;i<nsf;i++)
481       exc_energy += e[i]*e[i];
482    exc_energy=sqrt(exc_energy/nb_subvect);
483
484    /* Quantize global ("average") gain */
485    q=log(exc_energy+.1);
486    q=floor(.5+2*(q-2));
487    if (q<0)
488       q=0;
489    if (q>15)
490       q=15;
491    id = (int)q;
492    frame_bits_pack(bits, id, 4);
493    exc_energy=exp(.5*q+2);
494
495
496    for (i=0;i<nsf;i++)
497       t[i]=target[i];
498
499    e[0]=1;
500    for (i=1;i<nsf;i++)
501       e[i]=0;
502    residue_zero(e, awk1, r, nsf, p);
503    syn_filt_zero(r, ak, r, nsf, p);
504    syn_filt_zero(r, awk2, r, nsf,p);
505    
506    /* Pre-compute codewords response and energy */
507    for (i=0;i<shape_cb_size;i++)
508    {
509       float *res = resp+i*subvect_size;
510
511       /* Compute codeword response */
512       int k;
513       for(j=0;j<subvect_size;j++)
514          res[j]=0;
515       for(j=0;j<subvect_size;j++)
516       {
517          for (k=j;k<subvect_size;k++)
518             res[k]+=shape_cb[i*subvect_size+j]*r[k-j];
519       }
520       /* Compute energy of codeword response */
521       E[i]=0;
522       for(j=0;j<subvect_size;j++)
523          E[i]+=res[j]*res[j];
524       E[i]=1/(.001+E[i]);
525    }
526
527    for (i=0;i<nb_subvect;i++)
528    {
529       int best_index[2]={0,0}, k, m;
530       float g, corr, best_gain[2]={0,0}, score, best_score[2]={-1,-1};
531       /* Find best codeword for current sub-vector */
532       for (j=0;j<shape_cb_size;j++)
533       {
534          corr=xcorr(resp+j*subvect_size,t+subvect_size*i,subvect_size);
535          score=corr*corr*E[j];
536          g = corr*E[j];
537          if (score>best_score[0])
538          {
539             best_index[1]=best_index[0];
540             best_score[1]=best_score[0];
541             best_gain[1]=best_gain[0];
542
543             best_index[0]=j;
544             best_score[0]=score;
545             best_gain[0]=g;
546          } else if (score>best_score[1]) {
547             best_index[1]=j;
548             best_score[1]=score;
549             best_gain[1]=g;            
550          }
551       }
552       
553       /* Quantize gain */
554       for (k=0;k<2;k++) {
555          int s=0, best_id;
556          best_gain[k] /= .01+exc_energy;
557          if (best_gain[k]<0)
558          {
559             best_gain[k]=-best_gain[k];
560             s=1;
561          }
562
563          /* Find gain index (it's a scalar but we use the VQ code anyway)*/
564          best_id = vq_index(&best_gain[k], scal_gains4, 1, 8);
565
566          best_gain[k]=scal_gains4[best_id];
567          /*printf ("gain_quant: %f %d %f\n", best_gain, best_id, scal_gains4[best_id]);*/
568          if (s)
569             best_gain[k]=-best_gain[k];
570          best_gain[k] *= exc_energy;
571       }
572
573
574
575       if (i<nb_subvect-1) {
576          int best_index2=0;
577          float best_score2=-1, best_gain2=0;
578          int nbest;
579          float err[2]={0,0};
580          float *tt=PUSH(stack,nsf);
581          for (nbest=0;nbest<2;nbest++)
582          {
583             for (j=0;j<nsf;j++)
584                tt[j]=t[j];
585             for (j=0;j<subvect_size;j++)
586             {
587                g=best_gain[nbest]*shape_cb[best_index[nbest]*subvect_size+j];
588                for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
589                   tt[k] -= g*r[m];
590             }
591             
592
593             for (j=0;j<shape_cb_size;j++)
594             {
595                corr=xcorr(resp+j*subvect_size,tt+subvect_size*(i+1),subvect_size);
596                score=corr*corr*E[j];
597                g = corr*E[j];
598                if (score>best_score2)
599                {
600                   best_index2=j;
601                   best_score2=score;
602                   best_gain2=g;
603                }
604             }
605
606             {
607                int s=0, best_id;
608                best_gain2 /= .01+exc_energy;
609                if (best_gain2<0)
610                {
611                   best_gain2=-best_gain2;
612                   s=1;
613                }
614                best_id = vq_index(&best_gain2, scal_gains4, 1, 8);
615                best_gain2=scal_gains4[best_id];
616                if (s)
617                   best_gain2=-best_gain2;
618                best_gain2 *= exc_energy;
619             }
620
621             for (j=0;j<subvect_size;j++)
622             {
623                g=best_gain2*shape_cb[best_index2*subvect_size+j];
624                for (k=subvect_size*(i+1)+j,m=0;k<nsf;k++,m++)
625                   tt[k] -= g*r[m];
626             }
627             for (j=subvect_size*i;j<subvect_size*(i+2);j++)
628                err[nbest]-=tt[j]*tt[j];
629             
630             best_score[nbest]=err[nbest];
631          }
632
633          if (best_score[1]>best_score[0])
634          {
635             best_index[0]=best_index[1];
636             best_score[0]=best_score[1];
637             best_gain[0]=best_gain[1];
638
639          }
640          POP(stack);
641       }
642
643
644       
645
646       ind[i]=best_index[0];
647       gains[i]=best_gain[0];
648       /* Update target for next subvector */
649       for (j=0;j<subvect_size;j++)
650       {
651          g=best_gain[0]*shape_cb[best_index[0]*subvect_size+j];
652          for (k=subvect_size*i+j,m=0;k<nsf;k++,m++)
653             t[k] -= g*r[m];
654       }
655    }
656    
657    /* Put everything back together */
658    for (i=0;i<nb_subvect;i++)
659       for (j=0;j<subvect_size;j++)
660          e[subvect_size*i+j]=gains[i]*shape_cb[ind[i]*subvect_size+j];
661
662    /* Update excitation */
663    for (j=0;j<nsf;j++)
664       exc[j]+=e[j];
665    
666    /* Update target */
667    residue_zero(e, awk1, r, nsf, p);
668    syn_filt_zero(r, ak, r, nsf, p);
669    syn_filt_zero(r, awk2, r, nsf,p);
670    for (j=0;j<nsf;j++)
671       target[j]-=r[j];
672
673    
674
675
676    POP(stack);
677    POP(stack);
678    POP(stack);
679    POP(stack);
680    POP(stack);
681    POP(stack);
682 }
683
684
685
686
687 void split_cb_unquant(
688 float *exc,
689 void *par,                      /* non-overlapping codebook */
690 int   nsf,                      /* number of samples in subframe */
691 FrameBits *bits,
692 float *stack
693 )
694 {
695    int i,j;
696    int *ind;
697    float *gains;
698    float *sign;
699    float *shape_cb, exc_energy;
700    int shape_cb_size, subvect_size, nb_subvect;
701    split_cb_params *params;
702
703    params = (split_cb_params *) par;
704    subvect_size = params->subvect_size;
705    nb_subvect = params->nb_subvect;
706    shape_cb_size = 1<<params->shape_bits;
707    shape_cb = params->shape_cb;
708    
709    ind = (int*)PUSH(stack, nb_subvect);
710    gains = PUSH(stack, nb_subvect);
711    sign = PUSH(stack, nb_subvect);
712
713    /* Decode global (average) gain */
714    {
715       int id;
716       id = frame_bits_unpack_unsigned(bits, 4);
717       exc_energy=exp(.5*id+2);
718    }
719
720    /* Decode codewords and gains */
721    for (i=0;i<nb_subvect;i++)
722    {
723       int gain_id;
724       ind[i] = frame_bits_unpack_unsigned(bits, params->shape_bits);
725       if (frame_bits_unpack_unsigned(bits, 1))
726          sign[i]=-1;
727       else
728          sign[i]=1;
729       
730       gain_id = frame_bits_unpack_unsigned(bits, 3);
731       gains[i]=scal_gains4[gain_id];
732       gains[i] *= sign[i];
733       gains[i] *= exc_energy;
734
735
736    }
737
738    /* Compute decoded excitation */
739    for (i=0;i<nb_subvect;i++)
740       for (j=0;j<subvect_size;j++)
741          exc[subvect_size*i+j]+=gains[i]*shape_cb[ind[i]*subvect_size+j];
742
743    POP(stack);
744    POP(stack);
745    POP(stack);
746 }