f9ec61078b27c4e50b28a1ff7901fbb38367bcee
[aubio.git] / src / pitch / pitchmcomb.c
1 /*
2   Copyright (C) 2003-2009 Paul Brossier <piem@aubio.org>
3
4   This file is part of aubio.
5
6   aubio is free software: you can redistribute it and/or modify
7   it under the terms of the GNU General Public License as published by
8   the Free Software Foundation, either version 3 of the License, or
9   (at your option) any later version.
10
11   aubio is distributed in the hope that it will be useful,
12   but WITHOUT ANY WARRANTY; without even the implied warranty of
13   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14   GNU General Public License for more details.
15
16   You should have received a copy of the GNU General Public License
17   along with aubio.  If not, see <http://www.gnu.org/licenses/>.
18
19 */
20
21 #include "aubio_priv.h"
22 #include "fvec.h"
23 #include "cvec.h"
24 #include "mathutils.h"
25 #include "pitch/pitchmcomb.h"
26
27 #define CAND_SWAP(a,b) { register aubio_spectralcandidate_t *t=(a);(a)=(b);(b)=t; }
28
29 typedef struct _aubio_spectralpeak_t aubio_spectralpeak_t;
30 typedef struct _aubio_spectralcandidate_t aubio_spectralcandidate_t;
31 uint_t aubio_pitchmcomb_get_root_peak (aubio_spectralpeak_t * peaks,
32     uint_t length);
33 uint_t aubio_pitchmcomb_quadpick (aubio_spectralpeak_t * spectral_peaks,
34     const fvec_t * X);
35 void aubio_pitchmcomb_spectral_pp (aubio_pitchmcomb_t * p, const fvec_t * oldmag);
36 void aubio_pitchmcomb_combdet (aubio_pitchmcomb_t * p, const fvec_t * newmag);
37 /* not used but useful : sort by amplitudes (or anything else)
38  * sort_pitchpeak(peaks, length);
39  */
40 /** spectral_peak comparison function (must return signed int) */
41 static sint_t aubio_pitchmcomb_sort_peak_comp (const void *x, const void *y);
42 /** sort spectral_peak against their mag */
43 void aubio_pitchmcomb_sort_peak (aubio_spectralpeak_t * peaks, uint_t nbins);
44 /** select the best candidates */
45 uint_t aubio_pitch_cands (aubio_pitchmcomb_t * p, const cvec_t * fftgrain,
46     smpl_t * cands);
47
48 /** sort spectral_candidate against their comb ene */
49 void aubio_pitchmcomb_sort_cand_ene (aubio_spectralcandidate_t ** candidates,
50     uint_t nbins);
51 /** sort spectral_candidate against their frequency */
52 void aubio_pitchmcomb_sort_cand_freq (aubio_spectralcandidate_t ** candidates,
53     uint_t nbins);
54
55 struct _aubio_pitchmcomb_t
56 {
57   smpl_t threshold;                        /**< offset threshold [0.033 or 0.01]     */
58   smpl_t alpha;                            /**< normalisation exponent [9]           */
59   smpl_t cutoff;                           /**< low-pass filter cutoff [0.34, 1]     */
60   smpl_t tol;                              /**< tolerance [0.05]                     */
61   // smpl_t tau;                              /**< frequency precision [44100/4096]     */
62   uint_t win_post;                         /**< median filter window length          */
63   uint_t win_pre;                          /**< median filter window                 */
64   uint_t ncand;                            /**< maximum number of candidates (combs) */
65   uint_t npartials;                        /**< maximum number of partials per combs */
66   uint_t count;                            /**< picked picks                         */
67   uint_t goodcandidate;                    /**< best candidate                       */
68   uint_t spec_partition;                   /**< spectrum partition to consider       */
69   aubio_spectralpeak_t *peaks;             /**< up to length win/spec_partition      */
70   aubio_spectralcandidate_t **candidates;  /** up to five candidates                 */
71   /* some scratch pads */
72   /** \bug  (unnecessary copied from fftgrain?) */
73   fvec_t *newmag;                          /**< vec to store mag                     */
74   fvec_t *scratch;                         /**< vec to store modified mag            */
75   fvec_t *scratch2;                        /**< vec to compute moving median         */
76   fvec_t *theta;                          /**< vec to store phase                     */
77   smpl_t phasediff;
78   smpl_t phasefreq;
79   /** threshfn: name or handle of fn for computing adaptive threshold [median] */
80   /** aubio_thresholdfn_t thresholdfn; */
81   /** picker: name or handle of fn for picking event times [quadpick] */
82   /** aubio_pickerfn_t pickerfn; */
83 };
84
85 /** spectral peak object */
86 struct _aubio_spectralpeak_t
87 {
88   uint_t bin;     /**< bin [0-(length-1)] */
89   smpl_t ebin;    /**< estimated bin */
90   smpl_t mag;     /**< peak magnitude */
91 };
92
93 /** spectral candidates array object */
94 struct _aubio_spectralcandidate_t
95 {
96   smpl_t ebin;    /**< interpolated bin */
97   smpl_t *ecomb;  /**< comb */
98   smpl_t ene;     /**< candidate energy */
99   smpl_t len;     /**< length */
100 };
101
102
103 void
104 aubio_pitchmcomb_do (aubio_pitchmcomb_t * p, const cvec_t * fftgrain, fvec_t * output)
105 {
106   uint_t j;
107   smpl_t instfreq;
108   fvec_t *newmag = (fvec_t *) p->newmag;
109   //smpl_t hfc; //fe=instfreq(theta1,theta,ops); //theta1=theta;
110   /* copy incoming grain to newmag */
111   for (j = 0; j < newmag->length; j++)
112     newmag->data[j] = fftgrain->norm[j];
113   /* detect only if local energy > 10. */
114   //if (aubio_level_lin (newmag) * newmag->length > 10.) {
115   //hfc = fvec_local_hfc(newmag); //not used
116   aubio_pitchmcomb_spectral_pp (p, newmag);
117   aubio_pitchmcomb_combdet (p, newmag);
118   //aubio_pitchmcomb_sort_cand_freq(p->candidates,p->ncand);
119   //return p->candidates[p->goodcandidate]->ebin;
120   j = (uint_t) FLOOR (p->candidates[p->goodcandidate]->ebin + .5);
121   instfreq = aubio_unwrap2pi (fftgrain->phas[j]
122       - p->theta->data[j] - j * p->phasediff);
123   instfreq *= p->phasefreq;
124   /* store phase for next run */
125   for (j = 0; j < p->theta->length; j++) {
126     p->theta->data[j] = fftgrain->phas[j];
127   }
128   //return p->candidates[p->goodcandidate]->ebin;
129   output->data[0] =
130       FLOOR (p->candidates[p->goodcandidate]->ebin + .5) + instfreq;
131   /*} else {
132      return -1.;
133      } */
134 }
135
136 uint_t
137 aubio_pitch_cands (aubio_pitchmcomb_t * p, const cvec_t * fftgrain, smpl_t * cands)
138 {
139   uint_t j;
140   uint_t k;
141   fvec_t *newmag = (fvec_t *) p->newmag;
142   aubio_spectralcandidate_t **scands =
143       (aubio_spectralcandidate_t **) (p->candidates);
144   //smpl_t hfc; //fe=instfreq(theta1,theta,ops); //theta1=theta;
145   /* copy incoming grain to newmag */
146   for (j = 0; j < newmag->length; j++)
147     newmag->data[j] = fftgrain->norm[j];
148   /* detect only if local energy > 10. */
149   if (aubio_level_lin (newmag) * newmag->length > 10.) {
150     /* hfc = fvec_local_hfc(newmag); do not use */
151     aubio_pitchmcomb_spectral_pp (p, newmag);
152     aubio_pitchmcomb_combdet (p, newmag);
153     aubio_pitchmcomb_sort_cand_freq (scands, p->ncand);
154     /* store ncand comb energies in cands[1:ncand] */
155     for (k = 0; k < p->ncand; k++)
156       cands[k] = p->candidates[k]->ene;
157     /* store ncand[end] freq in cands[end] */
158     cands[p->ncand] = p->candidates[p->ncand - 1]->ebin;
159     return 1;
160   } else {
161     for (k = 0; k < p->ncand; k++)
162       cands[k] = 0;
163     return 0;
164   }
165 }
166
167 void
168 aubio_pitchmcomb_spectral_pp (aubio_pitchmcomb_t * p, const fvec_t * newmag)
169 {
170   fvec_t *mag = (fvec_t *) p->scratch;
171   fvec_t *tmp = (fvec_t *) p->scratch2;
172   uint_t j;
173   uint_t length = mag->length;
174   /* copy newmag to mag (scracth) */
175   for (j = 0; j < length; j++) {
176     mag->data[j] = newmag->data[j];
177   }
178   fvec_min_removal (mag);       /* min removal          */
179   fvec_alpha_normalise (mag, p->alpha); /* alpha normalisation  */
180   /* skipped *//* low pass filtering   */
181   /** \bug fvec_moving_thres may write out of bounds */
182   fvec_adapt_thres (mag, tmp, p->win_post, p->win_pre);      /* adaptative threshold */
183   fvec_add (mag, -p->threshold);        /* fixed threshold      */
184   {
185     aubio_spectralpeak_t *peaks = (aubio_spectralpeak_t *) p->peaks;
186     uint_t count;
187     /*  return bin and ebin */
188     count = aubio_pitchmcomb_quadpick (peaks, mag);
189     for (j = 0; j < count; j++)
190       peaks[j].mag = newmag->data[peaks[j].bin];
191     /* reset non peaks */
192     for (j = count; j < length; j++)
193       peaks[j].mag = 0.;
194     p->peaks = peaks;
195     p->count = count;
196   }
197 }
198
199 void
200 aubio_pitchmcomb_combdet (aubio_pitchmcomb_t * p, const fvec_t * newmag)
201 {
202   aubio_spectralpeak_t *peaks = (aubio_spectralpeak_t *) p->peaks;
203   aubio_spectralcandidate_t **candidate =
204       (aubio_spectralcandidate_t **) p->candidates;
205
206   /* parms */
207   uint_t N = p->npartials;      /* maximum number of partials to be considered 10 */
208   uint_t M = p->ncand;          /* maximum number of combs to be considered 5 */
209   uint_t length = newmag->length;
210   uint_t count = p->count;
211   uint_t k;
212   uint_t l;
213   uint_t d;
214   uint_t curlen = 0;
215
216   smpl_t delta2;
217   smpl_t xx;
218   uint_t position = 0;
219
220   uint_t root_peak = 0;
221   uint_t tmpl = 0;
222   smpl_t tmpene = 0.;
223
224   /* get the biggest peak in the spectrum */
225   root_peak = aubio_pitchmcomb_get_root_peak (peaks, count);
226   /* not enough partials in highest notes, could be forced */
227   //if (peaks[root_peak].ebin >= aubio_miditofreq(85.)/p->tau) N=2;
228   //if (peaks[root_peak].ebin >= aubio_miditofreq(90.)/p->tau) N=1;
229   /* now calculate the energy of each of the 5 combs */
230   for (l = 0; l < M; l++) {
231     smpl_t scaler = (1. / (l + 1.));
232     candidate[l]->ene = 0.;     /* reset ene and len sums */
233     candidate[l]->len = 0.;
234     candidate[l]->ebin = scaler * peaks[root_peak].ebin;
235     /* if less than N peaks available, curlen < N */
236     if (candidate[l]->ebin != 0.)
237       curlen = (uint_t) FLOOR (length / (candidate[l]->ebin));
238     curlen = (N < curlen) ? N : curlen;
239     /* fill candidate[l]->ecomb[k] with (k+1)*candidate[l]->ebin */
240     for (k = 0; k < curlen; k++)
241       candidate[l]->ecomb[k] = (candidate[l]->ebin) * (k + 1.);
242     for (k = curlen; k < length; k++)
243       candidate[l]->ecomb[k] = 0.;
244     /* for each in candidate[l]->ecomb[k] */
245     for (k = 0; k < curlen; k++) {
246       xx = 100000.;
247       /** get the candidate->ecomb the closer to peaks.ebin
248        * (to cope with the inharmonicity)*/
249       for (d = 0; d < count; d++) {
250         delta2 = ABS (candidate[l]->ecomb[k] - peaks[d].ebin);
251         if (delta2 <= xx) {
252           position = d;
253           xx = delta2;
254         }
255       }
256       /* for a Q factor of 17, maintaining "constant Q filtering",
257        * and sum energy and length over non null combs */
258       if (17. * xx < candidate[l]->ecomb[k]) {
259         candidate[l]->ecomb[k] = peaks[position].ebin;
260         candidate[l]->ene +=    /* ecomb rounded to nearest int */
261             POW (newmag->data[(uint_t) FLOOR (candidate[l]->ecomb[k] + .5)],
262             0.25);
263         candidate[l]->len += 1. / curlen;
264       } else
265         candidate[l]->ecomb[k] = 0.;
266     }
267     /* punishment */
268     /*if (candidate[l]->len<0.6)
269        candidate[l]->ene=0.; */
270     /* remember best candidate energy (in polyphonic, could check for
271      * tmpene*1.1 < candidate->ene to reduce jumps towards low frequencies) */
272     if (tmpene < candidate[l]->ene) {
273       tmpl = l;
274       tmpene = candidate[l]->ene;
275     }
276   }
277   //p->candidates=candidate;
278   //p->peaks=peaks;
279   p->goodcandidate = tmpl;
280 }
281
282 /** T=quadpick(X): return indices of elements of X which are peaks and positive
283  * exact peak positions are retrieved by quadratic interpolation
284  *
285  * \bug peak-picking too picky, sometimes counts too many peaks ?
286  */
287 uint_t
288 aubio_pitchmcomb_quadpick (aubio_spectralpeak_t * spectral_peaks, const fvec_t * X)
289 {
290   uint_t j, ispeak, count = 0;
291   for (j = 1; j < X->length - 1; j++) {
292     ispeak = fvec_peakpick (X, j);
293     if (ispeak) {
294       count += ispeak;
295       spectral_peaks[count - 1].bin = j;
296       spectral_peaks[count - 1].ebin = fvec_quadratic_peak_pos (X, j);
297     }
298   }
299   return count;
300 }
301
302 /* get predominant partial */
303 uint_t
304 aubio_pitchmcomb_get_root_peak (aubio_spectralpeak_t * peaks, uint_t length)
305 {
306   uint_t i, pos = 0;
307   smpl_t tmp = 0.;
308   for (i = 0; i < length; i++)
309     if (tmp <= peaks[i].mag) {
310       pos = i;
311       tmp = peaks[i].mag;
312     }
313   return pos;
314 }
315
316 void
317 aubio_pitchmcomb_sort_peak (aubio_spectralpeak_t * peaks, uint_t nbins)
318 {
319   qsort (peaks, nbins, sizeof (aubio_spectralpeak_t),
320       aubio_pitchmcomb_sort_peak_comp);
321 }
322
323 static sint_t
324 aubio_pitchmcomb_sort_peak_comp (const void *x, const void *y)
325 {
326   return (((aubio_spectralpeak_t *) y)->mag -
327       ((aubio_spectralpeak_t *) x)->mag);
328 }
329
330
331 void
332 aubio_pitchmcomb_sort_cand_ene (aubio_spectralcandidate_t ** candidates,
333     uint_t nbins)
334 {
335   uint_t cur = 0;
336   uint_t run = 0;
337   for (cur = 0; cur < nbins; cur++) {
338     for (run = cur + 1; run < nbins; run++) {
339       if (candidates[run]->ene > candidates[cur]->ene)
340         CAND_SWAP (candidates[run], candidates[cur]);
341     }
342   }
343 }
344
345
346 void
347 aubio_pitchmcomb_sort_cand_freq (aubio_spectralcandidate_t ** candidates,
348     uint_t nbins)
349 {
350   uint_t cur = 0;
351   uint_t run = 0;
352   for (cur = 0; cur < nbins; cur++) {
353     for (run = cur + 1; run < nbins; run++) {
354       if (candidates[run]->ebin < candidates[cur]->ebin)
355         CAND_SWAP (candidates[run], candidates[cur]);
356     }
357   }
358 }
359
360 aubio_pitchmcomb_t *
361 new_aubio_pitchmcomb (uint_t bufsize, uint_t hopsize)
362 {
363   aubio_pitchmcomb_t *p = AUBIO_NEW (aubio_pitchmcomb_t);
364   /* bug: should check if size / 8 > post+pre+1 */
365   uint_t i, j;
366   uint_t spec_size;
367   p->spec_partition = 2;
368   p->ncand = 5;
369   p->npartials = 5;
370   p->cutoff = 1.;
371   p->threshold = 0.01;
372   p->win_post = 8;
373   p->win_pre = 7;
374   // p->tau              = samplerate/bufsize;
375   p->alpha = 9.;
376   p->goodcandidate = 0;
377   p->phasefreq = bufsize / hopsize / TWO_PI;
378   p->phasediff = TWO_PI * hopsize / bufsize;
379   spec_size = bufsize / p->spec_partition + 1;
380   //p->pickerfn = quadpick;
381   //p->biquad = new_biquad(0.1600,0.3200,0.1600, -0.5949, 0.2348);
382   /* allocate temp memory */
383   p->newmag = new_fvec (spec_size);
384   /* array for median */
385   p->scratch = new_fvec (spec_size);
386   /* array for phase */
387   p->theta = new_fvec (spec_size);
388   /* array for adaptative threshold */
389   p->scratch2 = new_fvec (p->win_post + p->win_pre + 1);
390   /* array of spectral peaks */
391   p->peaks = AUBIO_ARRAY (aubio_spectralpeak_t, spec_size);
392   for (i = 0; i < spec_size; i++) {
393     p->peaks[i].bin = 0.;
394     p->peaks[i].ebin = 0.;
395     p->peaks[i].mag = 0.;
396   }
397   /* array of pointers to spectral candidates */
398   p->candidates = AUBIO_ARRAY (aubio_spectralcandidate_t *, p->ncand);
399   for (i = 0; i < p->ncand; i++) {
400     p->candidates[i] = AUBIO_NEW (aubio_spectralcandidate_t);
401     p->candidates[i]->ecomb = AUBIO_ARRAY (smpl_t, spec_size);
402     for (j = 0; j < spec_size; j++) {
403       p->candidates[i]->ecomb[j] = 0.;
404     }
405     p->candidates[i]->ene = 0.;
406     p->candidates[i]->ebin = 0.;
407     p->candidates[i]->len = 0.;
408   }
409   return p;
410 }
411
412
413 void
414 del_aubio_pitchmcomb (aubio_pitchmcomb_t * p)
415 {
416   uint_t i;
417   del_fvec (p->newmag);
418   del_fvec (p->scratch);
419   del_fvec (p->theta);
420   del_fvec (p->scratch2);
421   AUBIO_FREE (p->peaks);
422   for (i = 0; i < p->ncand; i++) {
423     AUBIO_FREE (p->candidates[i]->ecomb);
424     AUBIO_FREE (p->candidates[i]);
425   }
426   AUBIO_FREE (p->candidates);
427   AUBIO_FREE (p);
428 }