qualia_audio/features/multipitch/
klapuri.rs1use crate::features::peaks::spectral_peaks;
29use crate::features::salience::pitch_salience;
30use crate::types::AudioError;
31
32const BINS_PER_SEMITONE: f32 = 10.0;
34const MAX_PEAKS: usize = 128;
36const N_SUBTRACT_HARMONICS: usize = 20;
38const SUBTRACT_HALF_WIDTH_SEMITONES: f32 = 0.5;
41
42#[inline]
48pub(crate) fn salience_bins(f_min: f32, f_max: f32) -> usize {
49 if !(f_min.is_finite() && f_max.is_finite()) || f_min <= 0.0 || f_max <= f_min {
50 return 0;
51 }
52 1 + (12.0 * BINS_PER_SEMITONE * (f_max / f_min).log2()).floor() as usize
53}
54
55#[inline]
58fn parabolic_bin(sal: &[f32], bi: usize, n_bins: usize) -> f32 {
59 if bi == 0 || bi + 1 >= n_bins {
60 return bi as f32;
61 }
62 let ym1 = sal[bi - 1];
63 let y0 = sal[bi];
64 let yp1 = sal[bi + 1];
65 let denom = ym1 - 2.0 * y0 + yp1;
66 if denom == 0.0 || !denom.is_finite() {
67 return bi as f32;
68 }
69 let off = (0.5 * (ym1 - yp1) / denom).clamp(-0.5, 0.5);
70 bi as f32 + off
71}
72
73fn subtract_harmonics(residual: &mut [f32], f0: f32, bin_hz: f32) {
78 let n = residual.len();
79 if n == 0 || bin_hz <= 0.0 {
80 return;
81 }
82 let nyquist = (n - 1) as f32 * bin_hz;
83 let lo_ratio = (-SUBTRACT_HALF_WIDTH_SEMITONES / 12.0).exp2();
84 let hi_ratio = (SUBTRACT_HALF_WIDTH_SEMITONES / 12.0).exp2();
85 for h in 1..=N_SUBTRACT_HARMONICS {
86 let fh = f0 * h as f32;
87 if fh > nyquist {
88 break;
89 }
90 let lo = (fh * lo_ratio / bin_hz).floor().max(0.0) as usize;
91 let hi_f = (fh * hi_ratio / bin_hz).ceil();
92 let hi = if hi_f < 0.0 {
93 0
94 } else {
95 (hi_f as usize).min(n - 1)
96 };
97 for b in lo..=hi {
98 residual[b] = 0.0;
99 }
100 }
101}
102
103#[allow(clippy::too_many_arguments)]
128pub fn multipitch_klapuri(
129 mag: &[f32],
130 sample_rate: f32,
131 f_min: f32,
132 f_max: f32,
133 max_polyphony: usize,
134 salience_threshold: f32,
135 out_f0: &mut [f32],
136 scratch: &mut [f32],
137) -> Result<usize, AudioError> {
138 if !(sample_rate.is_finite() && sample_rate > 0.0)
139 || !(f_min.is_finite() && f_min > 0.0)
140 || !(f_max.is_finite() && f_max > f_min)
141 || !(salience_threshold.is_finite() && salience_threshold >= 0.0)
142 || mag.len() < 3
143 {
144 return Err(AudioError::InvalidParameter);
145 }
146
147 let mlen = mag.len();
148 let n_bins = salience_bins(f_min, f_max);
149 if n_bins < 3 || scratch.len() < mlen + n_bins {
150 return Err(AudioError::OutputBufferTooSmall);
151 }
152
153 let cap = max_polyphony.min(out_f0.len());
154 for v in out_f0.iter_mut().take(cap) {
155 *v = 0.0;
156 }
157 if cap == 0 {
158 return Ok(0);
159 }
160
161 let bin_hz = sample_rate / (2.0 * (mlen - 1) as f32);
162
163 let (residual, tail) = scratch.split_at_mut(mlen);
165 let sal = &mut tail[..n_bins];
166 residual.copy_from_slice(mag);
167
168 let mut peak_freqs = [0.0f32; MAX_PEAKS];
170 let mut peak_mags = [0.0f32; MAX_PEAKS];
171
172 let mut found = 0usize;
173 while found < cap {
174 let n_peaks = spectral_peaks(
176 residual,
177 sample_rate,
178 MAX_PEAKS,
179 &mut peak_freqs,
180 &mut peak_mags,
181 )?;
182 if n_peaks == 0 {
183 break; }
185 pitch_salience(
186 &peak_freqs,
187 &peak_mags,
188 n_peaks,
189 f_min,
190 BINS_PER_SEMITONE,
191 n_bins,
192 sal,
193 )?;
194
195 let mut bi = 0usize;
197 let mut bv = sal[0];
198 for b in 1..n_bins {
199 if sal[b] > bv {
200 bv = sal[b];
201 bi = b;
202 }
203 }
204 if bv < salience_threshold {
205 break; }
207
208 let refined = parabolic_bin(sal, bi, n_bins);
210 let f0 = f_min * (refined / (12.0 * BINS_PER_SEMITONE)).exp2();
211 out_f0[found] = f0;
212 found += 1;
213
214 subtract_harmonics(residual, f0, bin_hz);
215 }
216
217 Ok(found)
218}
219
220#[cfg(test)]
221mod tests {
222 use super::*;
223
224 const FFT_SIZE: usize = 8192;
225 const SR: f32 = 44_100.0;
226 const N_BINS_SPEC: usize = FFT_SIZE / 2 + 1; const F_MIN: f32 = 100.0;
228 const F_MAX: f32 = 2_000.0;
229
230 fn bin_hz() -> f32 {
231 SR / FFT_SIZE as f32
232 }
233
234 fn add_tone(mag: &mut [f32], f0: f32, n_harm: usize) {
238 let bh = bin_hz();
239 for h in 1..=n_harm {
240 let fh = f0 * h as f32;
241 let k = (fh / bh).round() as usize;
242 if k == 0 || k + 1 >= mag.len() {
243 continue;
244 }
245 let amp = 1.0 / h as f32;
246 mag[k] += amp;
247 mag[k - 1] += 0.4 * amp;
248 mag[k + 1] += 0.4 * amp;
249 }
250 }
251
252 fn semitone_err(a: f32, b: f32) -> f32 {
253 12.0 * (a / b).log2().abs()
254 }
255
256 fn scratch_buf() -> Vec<f32> {
257 vec![0.0f32; N_BINS_SPEC + salience_bins(F_MIN, F_MAX)]
258 }
259
260 #[test]
264 fn recovers_two_simultaneous_tones() {
265 let mut mag = vec![0.0f32; N_BINS_SPEC];
266 add_tone(&mut mag, 440.0, 5);
267 add_tone(&mut mag, 660.0, 5);
268
269 let mut out = [0.0f32; 4];
270 let mut scratch = scratch_buf();
271 let n = multipitch_klapuri(&mag, SR, F_MIN, F_MAX, 3, 0.5, &mut out, &mut scratch)
272 .expect("klapuri");
273
274 assert_eq!(
275 n, 2,
276 "two tones present → exactly two F0s (no invented third)"
277 );
278
279 let (a, b) = (out[0], out[1]);
281 let matches_440_660 = (semitone_err(a, 440.0) < 1.0 && semitone_err(b, 660.0) < 1.0)
282 || (semitone_err(a, 660.0) < 1.0 && semitone_err(b, 440.0) < 1.0);
283 assert!(
284 matches_440_660,
285 "recovered F0s {a:.2} Hz, {b:.2} Hz do not match 440 & 660"
286 );
287 }
288
289 #[test]
291 fn single_tone_returns_one() {
292 let mut mag = vec![0.0f32; N_BINS_SPEC];
293 add_tone(&mut mag, 440.0, 5);
294
295 let mut out = [0.0f32; 4];
296 let mut scratch = scratch_buf();
297 let n = multipitch_klapuri(&mag, SR, F_MIN, F_MAX, 4, 0.5, &mut out, &mut scratch)
298 .expect("klapuri");
299
300 assert_eq!(n, 1, "one tone → one F0");
301 assert!(
302 semitone_err(out[0], 440.0) < 1.0,
303 "recovered {:.2} Hz vs 440",
304 out[0]
305 );
306 }
307
308 #[test]
310 fn flat_spectrum_abstains() {
311 let mag = vec![0.1f32; N_BINS_SPEC]; let mut out = [0.0f32; 4];
313 let mut scratch = scratch_buf();
314 let n = multipitch_klapuri(&mag, SR, F_MIN, F_MAX, 4, 0.5, &mut out, &mut scratch)
315 .expect("klapuri");
316 assert_eq!(n, 0, "flat/noise spectrum → abstain (0 F0s)");
317 }
318
319 #[test]
321 fn respects_max_polyphony() {
322 let mut mag = vec![0.0f32; N_BINS_SPEC];
323 add_tone(&mut mag, 261.63, 5); add_tone(&mut mag, 329.63, 5); add_tone(&mut mag, 392.0, 5); let mut out = [0.0f32; 4];
328 let mut scratch = scratch_buf();
329 let n = multipitch_klapuri(&mag, SR, F_MIN, F_MAX, 2, 0.5, &mut out, &mut scratch)
330 .expect("klapuri");
331 assert_eq!(n, 2, "declared max-polyphony of 2 is honoured");
332 }
333
334 #[test]
335 fn rejects_bad_params() {
336 let mag = vec![0.0f32; N_BINS_SPEC];
337 let mut out = [0.0f32; 4];
338 let mut scratch = scratch_buf();
339 assert_eq!(
340 multipitch_klapuri(&mag, 0.0, F_MIN, F_MAX, 2, 0.5, &mut out, &mut scratch),
341 Err(AudioError::InvalidParameter)
342 );
343 assert_eq!(
344 multipitch_klapuri(&mag, SR, F_MAX, F_MIN, 2, 0.5, &mut out, &mut scratch),
345 Err(AudioError::InvalidParameter)
346 );
347 }
348
349 #[test]
350 fn rejects_small_scratch() {
351 let mag = vec![0.0f32; N_BINS_SPEC];
352 let mut out = [0.0f32; 4];
353 let mut scratch = vec![0.0f32; 8]; assert_eq!(
355 multipitch_klapuri(&mag, SR, F_MIN, F_MAX, 2, 0.5, &mut out, &mut scratch),
356 Err(AudioError::OutputBufferTooSmall)
357 );
358 }
359}