src/pitch/pitchmcomb.c

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