Chris@41: /* Chris@41: ** Copyright (c) 2002-2016, Erik de Castro Lopo Chris@41: ** All rights reserved. Chris@41: ** Chris@41: ** This code is released under 2-clause BSD license. Please see the Chris@41: ** file at : https://github.com/erikd/libsamplerate/blob/master/COPYING Chris@41: */ Chris@41: Chris@41: #include "config.h" Chris@41: Chris@41: #include "util.h" Chris@41: Chris@41: #if (HAVE_FFTW3 == 1) Chris@41: Chris@41: #include Chris@41: #include Chris@41: #include Chris@41: #include Chris@41: Chris@41: #include Chris@41: Chris@41: #define MAX_SPEC_LEN (1<<18) Chris@41: #define MAX_PEAKS 10 Chris@41: Chris@41: static void log_mag_spectrum (double *input, int len, double *magnitude) ; Chris@41: static void smooth_mag_spectrum (double *magnitude, int len) ; Chris@41: static double find_snr (const double *magnitude, int len, int expected_peaks) ; Chris@41: Chris@41: typedef struct Chris@41: { double peak ; Chris@41: int index ; Chris@41: } PEAK_DATA ; Chris@41: Chris@41: double Chris@41: calculate_snr (float *data, int len, int expected_peaks) Chris@41: { static double magnitude [MAX_SPEC_LEN] ; Chris@41: static double datacopy [MAX_SPEC_LEN] ; Chris@41: Chris@41: double snr = 200.0 ; Chris@41: int k ; Chris@41: Chris@41: if (len > MAX_SPEC_LEN) Chris@41: { printf ("%s : line %d : data length too large.\n", __FILE__, __LINE__) ; Chris@41: exit (1) ; Chris@41: } ; Chris@41: Chris@41: for (k = 0 ; k < len ; k++) Chris@41: datacopy [k] = data [k] ; Chris@41: Chris@41: /* Pad the data just a little to speed up the FFT. */ Chris@41: while ((len & 0x1F) && len < MAX_SPEC_LEN) Chris@41: { datacopy [len] = 0.0 ; Chris@41: len ++ ; Chris@41: } ; Chris@41: Chris@41: log_mag_spectrum (datacopy, len, magnitude) ; Chris@41: smooth_mag_spectrum (magnitude, len / 2) ; Chris@41: Chris@41: snr = find_snr (magnitude, len, expected_peaks) ; Chris@41: Chris@41: return snr ; Chris@41: } /* calculate_snr */ Chris@41: Chris@41: /*============================================================================== Chris@41: ** There is a slight problem with trying to measure SNR with the method used Chris@41: ** here; the side lobes of the windowed FFT can look like a noise/aliasing peak. Chris@41: ** The solution is to smooth the magnitude spectrum by wiping out troughs Chris@41: ** between adjacent peaks as done here. Chris@41: ** This removes side lobe peaks without affecting noise/aliasing peaks. Chris@41: */ Chris@41: Chris@41: static void linear_smooth (double *mag, PEAK_DATA *larger, PEAK_DATA *smaller) ; Chris@41: Chris@41: static void Chris@41: smooth_mag_spectrum (double *mag, int len) Chris@41: { PEAK_DATA peaks [2] ; Chris@41: Chris@41: int k ; Chris@41: Chris@41: memset (peaks, 0, sizeof (peaks)) ; Chris@41: Chris@41: /* Find first peak. */ Chris@41: for (k = 1 ; k < len - 1 ; k++) Chris@41: { if (mag [k - 1] < mag [k] && mag [k] >= mag [k + 1]) Chris@41: { peaks [0].peak = mag [k] ; Chris@41: peaks [0].index = k ; Chris@41: break ; Chris@41: } ; Chris@41: } ; Chris@41: Chris@41: /* Find subsequent peaks ans smooth between peaks. */ Chris@41: for (k = peaks [0].index + 1 ; k < len - 1 ; k++) Chris@41: { if (mag [k - 1] < mag [k] && mag [k] >= mag [k + 1]) Chris@41: { peaks [1].peak = mag [k] ; Chris@41: peaks [1].index = k ; Chris@41: Chris@41: if (peaks [1].peak > peaks [0].peak) Chris@41: linear_smooth (mag, &peaks [1], &peaks [0]) ; Chris@41: else Chris@41: linear_smooth (mag, &peaks [0], &peaks [1]) ; Chris@41: peaks [0] = peaks [1] ; Chris@41: } ; Chris@41: } ; Chris@41: Chris@41: } /* smooth_mag_spectrum */ Chris@41: Chris@41: static void Chris@41: linear_smooth (double *mag, PEAK_DATA *larger, PEAK_DATA *smaller) Chris@41: { int k ; Chris@41: Chris@41: if (smaller->index < larger->index) Chris@41: { for (k = smaller->index + 1 ; k < larger->index ; k++) Chris@41: mag [k] = (mag [k] < mag [k - 1]) ? 0.999 * mag [k - 1] : mag [k] ; Chris@41: } Chris@41: else Chris@41: { for (k = smaller->index - 1 ; k >= larger->index ; k--) Chris@41: mag [k] = (mag [k] < mag [k + 1]) ? 0.999 * mag [k + 1] : mag [k] ; Chris@41: } ; Chris@41: Chris@41: } /* linear_smooth */ Chris@41: Chris@41: /*============================================================================== Chris@41: */ Chris@41: Chris@41: static int Chris@41: peak_compare (const void *vp1, const void *vp2) Chris@41: { const PEAK_DATA *peak1, *peak2 ; Chris@41: Chris@41: peak1 = (const PEAK_DATA*) vp1 ; Chris@41: peak2 = (const PEAK_DATA*) vp2 ; Chris@41: Chris@41: return (peak1->peak < peak2->peak) ? 1 : -1 ; Chris@41: } /* peak_compare */ Chris@41: Chris@41: static double Chris@41: find_snr (const double *magnitude, int len, int expected_peaks) Chris@41: { PEAK_DATA peaks [MAX_PEAKS] ; Chris@41: Chris@41: int k, peak_count = 0 ; Chris@41: double snr ; Chris@41: Chris@41: memset (peaks, 0, sizeof (peaks)) ; Chris@41: Chris@41: /* Find the MAX_PEAKS largest peaks. */ Chris@41: for (k = 1 ; k < len - 1 ; k++) Chris@41: { if (magnitude [k - 1] < magnitude [k] && magnitude [k] >= magnitude [k + 1]) Chris@41: { if (peak_count < MAX_PEAKS) Chris@41: { peaks [peak_count].peak = magnitude [k] ; Chris@41: peaks [peak_count].index = k ; Chris@41: peak_count ++ ; Chris@41: qsort (peaks, peak_count, sizeof (PEAK_DATA), peak_compare) ; Chris@41: } Chris@41: else if (magnitude [k] > peaks [MAX_PEAKS - 1].peak) Chris@41: { peaks [MAX_PEAKS - 1].peak = magnitude [k] ; Chris@41: peaks [MAX_PEAKS - 1].index = k ; Chris@41: qsort (peaks, MAX_PEAKS, sizeof (PEAK_DATA), peak_compare) ; Chris@41: } ; Chris@41: } ; Chris@41: } ; Chris@41: Chris@41: if (peak_count < expected_peaks) Chris@41: { printf ("\n%s : line %d : bad peak_count (%d), expected %d.\n\n", __FILE__, __LINE__, peak_count, expected_peaks) ; Chris@41: return -1.0 ; Chris@41: } ; Chris@41: Chris@41: /* Sort the peaks. */ Chris@41: qsort (peaks, peak_count, sizeof (PEAK_DATA), peak_compare) ; Chris@41: Chris@41: snr = peaks [0].peak ; Chris@41: for (k = 1 ; k < peak_count ; k++) Chris@41: if (fabs (snr - peaks [k].peak) > 10.0) Chris@41: return fabs (peaks [k].peak) ; Chris@41: Chris@41: return snr ; Chris@41: } /* find_snr */ Chris@41: Chris@41: static void Chris@41: log_mag_spectrum (double *input, int len, double *magnitude) Chris@41: { fftw_plan plan = NULL ; Chris@41: Chris@41: double maxval ; Chris@41: int k ; Chris@41: Chris@41: if (input == NULL || magnitude == NULL) Chris@41: return ; Chris@41: Chris@41: plan = fftw_plan_r2r_1d (len, input, magnitude, FFTW_R2HC, FFTW_ESTIMATE | FFTW_PRESERVE_INPUT) ; Chris@41: if (plan == NULL) Chris@41: { printf ("%s : line %d : create plan failed.\n", __FILE__, __LINE__) ; Chris@41: exit (1) ; Chris@41: } ; Chris@41: Chris@41: fftw_execute (plan) ; Chris@41: Chris@41: fftw_destroy_plan (plan) ; Chris@41: Chris@41: maxval = 0.0 ; Chris@41: for (k = 1 ; k < len / 2 ; k++) Chris@41: { /* Chris@41: ** From : http://www.fftw.org/doc/Real_002dto_002dReal-Transform-Kinds.html#Real_002dto_002dReal-Transform-Kinds Chris@41: ** Chris@41: ** FFTW_R2HC computes a real-input DFT with output in “halfcomplex” format, i.e. real and imaginary parts Chris@41: ** for a transform of size n stored as: Chris@41: ** Chris@41: ** r0, r1, r2, ..., rn/2, i(n+1)/2-1, ..., i2, i1 Chris@41: */ Chris@41: double re = magnitude [k] ; Chris@41: double im = magnitude [len - k] ; Chris@41: magnitude [k] = sqrt (re * re + im * im) ; Chris@41: maxval = (maxval < magnitude [k]) ? magnitude [k] : maxval ; Chris@41: } ; Chris@41: Chris@41: memset (magnitude + len / 2, 0, len / 2 * sizeof (magnitude [0])) ; Chris@41: Chris@41: /* Don't care about DC component. Make it zero. */ Chris@41: magnitude [0] = 0.0 ; Chris@41: Chris@41: /* log magnitude. */ Chris@41: for (k = 0 ; k < len ; k++) Chris@41: { magnitude [k] = magnitude [k] / maxval ; Chris@41: magnitude [k] = (magnitude [k] < 1e-15) ? -200.0 : 20.0 * log10 (magnitude [k]) ; Chris@41: } ; Chris@41: Chris@41: return ; Chris@41: } /* log_mag_spectrum */ Chris@41: Chris@41: #else /* ! (HAVE_LIBFFTW && HAVE_LIBRFFTW) */ Chris@41: Chris@41: double Chris@41: calculate_snr (float *data, int len, int expected_peaks) Chris@41: { double snr = 200.0 ; Chris@41: Chris@41: data = data ; Chris@41: len = len ; Chris@41: expected_peaks = expected_peaks ; Chris@41: Chris@41: return snr ; Chris@41: } /* calculate_snr */ Chris@41: Chris@41: #endif Chris@41: