Skip to main content

qualia_audio/features/multipitch/
klapuri.rs

1//! Klapuri-style iterative spectral-subtraction multi-pitch estimation.
2//!
3//! Repeatedly (1) find the most salient fundamental in the *residual* magnitude
4//! spectrum, (2) estimate + cancel its harmonic series, and (3) recurse on the
5//! residual — up to `max_polyphony` sources or until the strongest remaining
6//! salience falls below `salience_threshold`. This is the classic Klapuri
7//! predominant-F0 cancellation loop (Klapuri, "Multiple fundamental frequency
8//! estimation by summing harmonic amplitudes", ISMIR 2006).
9//!
10//! REUSE: the per-iteration salience is built from the shared primitives — the
11//! residual's spectral peaks come from
12//! [`crate::features::peaks::spectral_peaks()`] and are folded into a harmonic
13//! pitch-salience curve by [`crate::features::salience::pitch_salience`]. This
14//! module adds only the *iteration + cancellation* on top of those.
15//!
16//! EPISTEMIC RULE (declared): the returned F0s are **proposals**, not ground
17//! truth. The **max-polyphony assumption** is explicit — the caller states, via
18//! `max_polyphony`, the largest number of concurrent pitches it is willing to
19//! posit; the estimator never invents more than that, and it **abstains**
20//! (stops early, possibly at zero) as soon as the residual's best salience drops
21//! below `salience_threshold`. A single-tone spectrum therefore yields exactly
22//! one F0 and a flat/noise spectrum yields none.
23//!
24//! Zero-heap hot path: the only working buffer is the caller-owned `scratch`,
25//! which holds the residual magnitude copy followed by the salience curve; the
26//! bounded spectral-peak lists live in fixed stack arrays.
27
28use crate::features::peaks::spectral_peaks;
29use crate::features::salience::pitch_salience;
30use crate::types::AudioError;
31
32/// Salience-grid resolution (bins per semitone → 10 = 10 cents/bin).
33const BINS_PER_SEMITONE: f32 = 10.0;
34/// Bounded number of residual spectral peaks folded into the salience curve.
35const MAX_PEAKS: usize = 128;
36/// Harmonics cancelled per detected source (higher ones are usually silent).
37const N_SUBTRACT_HARMONICS: usize = 20;
38/// Half-width (semitones) of the magnitude window zeroed around each cancelled
39/// harmonic; ±0.5 semitone brackets a spectral peak without touching neighbours.
40const SUBTRACT_HALF_WIDTH_SEMITONES: f32 = 0.5;
41
42/// Number of logarithmic salience bins spanning `[f_min, f_max]` at
43/// [`BINS_PER_SEMITONE`] resolution. This is exactly the salience-buffer length
44/// the caller must reserve in `scratch` **beyond** `mag.len()` (see below).
45///
46/// `scratch.len()` must be at least `mag.len() + salience_bins(f_min, f_max)`.
47#[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/// Sub-bin refinement of the salience maximum at bin `bi` via a 3-point
56/// parabola; returns the fractional bin. Boundary maxima fall back to `bi`.
57#[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
73/// Zero the residual magnitude in a ±half-semitone window around every harmonic
74/// `h * f0` (for `h = 1..=N_SUBTRACT_HARMONICS`) that lies below Nyquist — i.e.
75/// cancel the detected source so it is neither re-detected nor left to skew the
76/// next salience curve.
77fn 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/// Iterative spectral-subtraction multi-pitch estimation.
104///
105/// - `mag`: one-sided magnitude spectrum covering DC..Nyquist inclusive (length
106///   `N/2+1` for an FFT of size `2*(mag.len()-1)`), e.g. from
107///   [`crate::features::fft::real_fft_magnitude`]. Not modified.
108/// - `sample_rate`: Hz; sets the bin spacing `sample_rate / (2*(mag.len()-1))`.
109/// - `f_min` / `f_max`: inclusive fundamental-frequency search range in Hz.
110/// - `max_polyphony`: the **declared** maximum number of concurrent pitches to
111///   propose (the estimator writes at most `min(max_polyphony, out_f0.len())`).
112/// - `salience_threshold`: absolute salience floor; the loop stops (abstains) as
113///   soon as the residual's strongest salience is below it.
114/// - `out_f0`: caller buffer; detected fundamentals are written **salience
115///   order** (strongest first).
116/// - `scratch`: caller buffer of length `>= mag.len() + salience_bins(f_min,
117///   f_max)`; used as the residual magnitude copy followed by the salience curve.
118///
119/// Returns the number of F0s written.
120///
121/// # Errors
122/// - [`AudioError::InvalidParameter`] if `sample_rate`, `f_min`, `f_max` are not
123///   positive finite with `f_max > f_min`, `salience_threshold` is negative or
124///   non-finite, or `mag.len() < 3`.
125/// - [`AudioError::OutputBufferTooSmall`] if `scratch` cannot hold the residual
126///   plus the salience curve.
127#[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    // scratch = [ residual magnitude (mlen) | salience curve (n_bins) ].
164    let (residual, tail) = scratch.split_at_mut(mlen);
165    let sal = &mut tail[..n_bins];
166    residual.copy_from_slice(mag);
167
168    // Bounded per-iteration spectral-peak lists (stack, zero-heap).
169    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        // (1) Salience of the current residual, via the shared primitives.
175        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; // nothing pitched left → abstain
184        }
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        // (2) Strongest salience ridge.
196        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; // abstain: residual no longer confidently pitched
206        }
207
208        // (3) Refine → Hz, record, and cancel the source from the residual.
209        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; // 4097
227    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    /// Add a harmonic tone (fundamental `f0`, `n_harm` harmonics with 1/h decay)
235    /// into a magnitude spectrum: each harmonic is a 3-bin triangle so it forms a
236    /// clean strict local maximum for `spectral_peaks`.
237    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    /// GOLDEN: two simultaneous harmonic tones (440 Hz + 660 Hz). With
261    /// `max_polyphony = 3` the estimator recovers BOTH fundamentals (within ~1
262    /// semitone) and does NOT invent a spurious third pitch.
263    #[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        // The two recovered F0s must be ~440 and ~660 in some order.
280        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    /// A single harmonic tone yields exactly one F0 (~440 Hz).
290    #[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    /// A flat spectrum (no pitched peaks) abstains entirely.
309    #[test]
310    fn flat_spectrum_abstains() {
311        let mag = vec![0.1f32; N_BINS_SPEC]; // constant → no strict local maxima
312        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    /// `max_polyphony` caps the count even when more tones are present.
320    #[test]
321    fn respects_max_polyphony() {
322        let mut mag = vec![0.0f32; N_BINS_SPEC];
323        add_tone(&mut mag, 261.63, 5); // C4
324        add_tone(&mut mag, 329.63, 5); // E4
325        add_tone(&mut mag, 392.0, 5); // G4
326
327        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]; // far too small
354        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}