cannam@85: /* cannam@85: ** Copyright (C) 2002-2011 Erik de Castro Lopo cannam@85: ** cannam@85: ** This program is free software; you can redistribute it and/or modify cannam@85: ** it under the terms of the GNU General Public License as published by cannam@85: ** the Free Software Foundation; either version 2 of the License, or cannam@85: ** (at your option) any later version. cannam@85: ** cannam@85: ** This program is distributed in the hope that it will be useful, cannam@85: ** but WITHOUT ANY WARRANTY; without even the implied warranty of cannam@85: ** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the cannam@85: ** GNU General Public License for more details. cannam@85: ** cannam@85: ** You should have received a copy of the GNU General Public License cannam@85: ** along with this program; if not, write to the Free Software cannam@85: ** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA. cannam@85: */ cannam@85: cannam@85: #include "config.h" cannam@85: cannam@85: #include "util.h" cannam@85: cannam@85: #if (HAVE_FFTW3 == 1) cannam@85: cannam@85: #include cannam@85: #include cannam@85: #include cannam@85: #include cannam@85: cannam@85: #include cannam@85: cannam@85: #define MAX_SPEC_LEN (1<<18) cannam@85: #define MAX_PEAKS 10 cannam@85: cannam@85: static void log_mag_spectrum (double *input, int len, double *magnitude) ; cannam@85: static void smooth_mag_spectrum (double *magnitude, int len) ; cannam@85: static double find_snr (const double *magnitude, int len, int expected_peaks) ; cannam@85: cannam@85: typedef struct cannam@85: { double peak ; cannam@85: int index ; cannam@85: } PEAK_DATA ; cannam@85: cannam@85: double cannam@85: calculate_snr (float *data, int len, int expected_peaks) cannam@85: { static double magnitude [MAX_SPEC_LEN] ; cannam@85: static double datacopy [MAX_SPEC_LEN] ; cannam@85: cannam@85: double snr = 200.0 ; cannam@85: int k ; cannam@85: cannam@85: if (len > MAX_SPEC_LEN) cannam@85: { printf ("%s : line %d : data length too large.\n", __FILE__, __LINE__) ; cannam@85: exit (1) ; cannam@85: } ; cannam@85: cannam@85: for (k = 0 ; k < len ; k++) cannam@85: datacopy [k] = data [k] ; cannam@85: cannam@85: /* Pad the data just a little to speed up the FFT. */ cannam@85: while ((len & 0x1F) && len < MAX_SPEC_LEN) cannam@85: { datacopy [len] = 0.0 ; cannam@85: len ++ ; cannam@85: } ; cannam@85: cannam@85: log_mag_spectrum (datacopy, len, magnitude) ; cannam@85: smooth_mag_spectrum (magnitude, len / 2) ; cannam@85: cannam@85: snr = find_snr (magnitude, len, expected_peaks) ; cannam@85: cannam@85: return snr ; cannam@85: } /* calculate_snr */ cannam@85: cannam@85: /*============================================================================== cannam@85: ** There is a slight problem with trying to measure SNR with the method used cannam@85: ** here; the side lobes of the windowed FFT can look like a noise/aliasing peak. cannam@85: ** The solution is to smooth the magnitude spectrum by wiping out troughs cannam@85: ** between adjacent peaks as done here. cannam@85: ** This removes side lobe peaks without affecting noise/aliasing peaks. cannam@85: */ cannam@85: cannam@85: static void linear_smooth (double *mag, PEAK_DATA *larger, PEAK_DATA *smaller) ; cannam@85: cannam@85: static void cannam@85: smooth_mag_spectrum (double *mag, int len) cannam@85: { PEAK_DATA peaks [2] ; cannam@85: cannam@85: int k ; cannam@85: cannam@85: memset (peaks, 0, sizeof (peaks)) ; cannam@85: cannam@85: /* Find first peak. */ cannam@85: for (k = 1 ; k < len - 1 ; k++) cannam@85: { if (mag [k - 1] < mag [k] && mag [k] >= mag [k + 1]) cannam@85: { peaks [0].peak = mag [k] ; cannam@85: peaks [0].index = k ; cannam@85: break ; cannam@85: } ; cannam@85: } ; cannam@85: cannam@85: /* Find subsequent peaks ans smooth between peaks. */ cannam@85: for (k = peaks [0].index + 1 ; k < len - 1 ; k++) cannam@85: { if (mag [k - 1] < mag [k] && mag [k] >= mag [k + 1]) cannam@85: { peaks [1].peak = mag [k] ; cannam@85: peaks [1].index = k ; cannam@85: cannam@85: if (peaks [1].peak > peaks [0].peak) cannam@85: linear_smooth (mag, &peaks [1], &peaks [0]) ; cannam@85: else cannam@85: linear_smooth (mag, &peaks [0], &peaks [1]) ; cannam@85: peaks [0] = peaks [1] ; cannam@85: } ; cannam@85: } ; cannam@85: cannam@85: } /* smooth_mag_spectrum */ cannam@85: cannam@85: static void cannam@85: linear_smooth (double *mag, PEAK_DATA *larger, PEAK_DATA *smaller) cannam@85: { int k ; cannam@85: cannam@85: if (smaller->index < larger->index) cannam@85: { for (k = smaller->index + 1 ; k < larger->index ; k++) cannam@85: mag [k] = (mag [k] < mag [k - 1]) ? 0.999 * mag [k - 1] : mag [k] ; cannam@85: } cannam@85: else cannam@85: { for (k = smaller->index - 1 ; k >= larger->index ; k--) cannam@85: mag [k] = (mag [k] < mag [k + 1]) ? 0.999 * mag [k + 1] : mag [k] ; cannam@85: } ; cannam@85: cannam@85: } /* linear_smooth */ cannam@85: cannam@85: /*============================================================================== cannam@85: */ cannam@85: cannam@85: static int cannam@85: peak_compare (const void *vp1, const void *vp2) cannam@85: { const PEAK_DATA *peak1, *peak2 ; cannam@85: cannam@85: peak1 = (const PEAK_DATA*) vp1 ; cannam@85: peak2 = (const PEAK_DATA*) vp2 ; cannam@85: cannam@85: return (peak1->peak < peak2->peak) ? 1 : -1 ; cannam@85: } /* peak_compare */ cannam@85: cannam@85: static double cannam@85: find_snr (const double *magnitude, int len, int expected_peaks) cannam@85: { PEAK_DATA peaks [MAX_PEAKS] ; cannam@85: cannam@85: int k, peak_count = 0 ; cannam@85: double snr ; cannam@85: cannam@85: memset (peaks, 0, sizeof (peaks)) ; cannam@85: cannam@85: /* Find the MAX_PEAKS largest peaks. */ cannam@85: for (k = 1 ; k < len - 1 ; k++) cannam@85: { if (magnitude [k - 1] < magnitude [k] && magnitude [k] >= magnitude [k + 1]) cannam@85: { if (peak_count < MAX_PEAKS) cannam@85: { peaks [peak_count].peak = magnitude [k] ; cannam@85: peaks [peak_count].index = k ; cannam@85: peak_count ++ ; cannam@85: qsort (peaks, peak_count, sizeof (PEAK_DATA), peak_compare) ; cannam@85: } cannam@85: else if (magnitude [k] > peaks [MAX_PEAKS - 1].peak) cannam@85: { peaks [MAX_PEAKS - 1].peak = magnitude [k] ; cannam@85: peaks [MAX_PEAKS - 1].index = k ; cannam@85: qsort (peaks, MAX_PEAKS, sizeof (PEAK_DATA), peak_compare) ; cannam@85: } ; cannam@85: } ; cannam@85: } ; cannam@85: cannam@85: if (peak_count < expected_peaks) cannam@85: { printf ("\n%s : line %d : bad peak_count (%d), expected %d.\n\n", __FILE__, __LINE__, peak_count, expected_peaks) ; cannam@85: return -1.0 ; cannam@85: } ; cannam@85: cannam@85: /* Sort the peaks. */ cannam@85: qsort (peaks, peak_count, sizeof (PEAK_DATA), peak_compare) ; cannam@85: cannam@85: snr = peaks [0].peak ; cannam@85: for (k = 1 ; k < peak_count ; k++) cannam@85: if (fabs (snr - peaks [k].peak) > 10.0) cannam@85: return fabs (peaks [k].peak) ; cannam@85: cannam@85: return snr ; cannam@85: } /* find_snr */ cannam@85: cannam@85: static void cannam@85: log_mag_spectrum (double *input, int len, double *magnitude) cannam@85: { fftw_plan plan = NULL ; cannam@85: cannam@85: double maxval ; cannam@85: int k ; cannam@85: cannam@85: if (input == NULL || magnitude == NULL) cannam@85: return ; cannam@85: cannam@85: plan = fftw_plan_r2r_1d (len, input, magnitude, FFTW_R2HC, FFTW_ESTIMATE | FFTW_PRESERVE_INPUT) ; cannam@85: if (plan == NULL) cannam@85: { printf ("%s : line %d : create plan failed.\n", __FILE__, __LINE__) ; cannam@85: exit (1) ; cannam@85: } ; cannam@85: cannam@85: fftw_execute (plan) ; cannam@85: cannam@85: fftw_destroy_plan (plan) ; cannam@85: cannam@85: /* (k < N/2 rounded up) */ cannam@85: maxval = 0.0 ; cannam@85: for (k = 1 ; k < len / 2 ; k++) cannam@85: { magnitude [k] = sqrt (magnitude [k] * magnitude [k] + magnitude [len - k - 1] * magnitude [len - k - 1]) ; cannam@85: maxval = (maxval < magnitude [k]) ? magnitude [k] : maxval ; cannam@85: } ; cannam@85: cannam@85: memset (magnitude + len / 2, 0, len / 2 * sizeof (magnitude [0])) ; cannam@85: cannam@85: /* Don't care about DC component. Make it zero. */ cannam@85: magnitude [0] = 0.0 ; cannam@85: cannam@85: /* log magnitude. */ cannam@85: for (k = 0 ; k < len ; k++) cannam@85: { magnitude [k] = magnitude [k] / maxval ; cannam@85: magnitude [k] = (magnitude [k] < 1e-15) ? -200.0 : 20.0 * log10 (magnitude [k]) ; cannam@85: } ; cannam@85: cannam@85: return ; cannam@85: } /* log_mag_spectrum */ cannam@85: cannam@85: #else /* ! (HAVE_LIBFFTW && HAVE_LIBRFFTW) */ cannam@85: cannam@85: double cannam@85: calculate_snr (float *data, int len, int expected_peaks) cannam@85: { double snr = 200.0 ; cannam@85: cannam@85: data = data ; cannam@85: len = len ; cannam@85: expected_peaks = expected_peaks ; cannam@85: cannam@85: return snr ; cannam@85: } /* calculate_snr */ cannam@85: cannam@85: #endif cannam@85: