[audio-libsamplerate.git] / libsamplerate / tests / calc_snr.c

/*
** Copyright (c) 2002-2016, Erik de Castro Lopo <erikd@mega-nerd.com>
** All rights reserved.
**
** This code is released under 2-clause BSD license. Please see the
** file at : https://github.com/erikd/libsamplerate/blob/master/COPYING
*/

#include "config.h"

#include "util.h"

#if (HAVE_FFTW3 == 1)

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

#include <fftw3.h>

#define	MAX_SPEC_LEN	(1<<18)
#define	MAX_PEAKS		10

static void log_mag_spectrum (double *input, int len, double *magnitude) ;
static void smooth_mag_spectrum (double *magnitude, int len) ;
static double find_snr (const double *magnitude, int len, int expected_peaks) ;

typedef struct
{	double	peak ;
	int		index ;
} PEAK_DATA ;

double
calculate_snr (float *data, int len, int expected_peaks)
{	static double magnitude [MAX_SPEC_LEN] ;
	static double datacopy [MAX_SPEC_LEN] ;

	double snr = 200.0 ;
	int k ;

	if (len > MAX_SPEC_LEN)
	{	printf ("%s : line %d : data length too large.\n", __FILE__, __LINE__) ;
		exit (1) ;
		} ;

	for (k = 0 ; k < len ; k++)
		datacopy [k] = data [k] ;

	/* Pad the data just a little to speed up the FFT. */
	while ((len & 0x1F) && len < MAX_SPEC_LEN)
	{	datacopy [len] = 0.0 ;
		len ++ ;
		} ;

	log_mag_spectrum (datacopy, len, magnitude) ;
	smooth_mag_spectrum (magnitude, len / 2) ;

	snr = find_snr (magnitude, len, expected_peaks) ;

	return snr ;
} /* calculate_snr */

/*==============================================================================
** There is a slight problem with trying to measure SNR with the method used
** here; the side lobes of the windowed FFT can look like a noise/aliasing peak.
** The solution is to smooth the magnitude spectrum by wiping out troughs
** between adjacent peaks as done here.
** This removes side lobe peaks without affecting noise/aliasing peaks.
*/

static void linear_smooth (double *mag, PEAK_DATA *larger, PEAK_DATA *smaller) ;

static void
smooth_mag_spectrum (double *mag, int len)
{	PEAK_DATA peaks [2] ;

	int k ;

	memset (peaks, 0, sizeof (peaks)) ;

	/* Find first peak. */
	for (k = 1 ; k < len - 1 ; k++)
	{	if (mag [k - 1] < mag [k] && mag [k] >= mag [k + 1])
		{	peaks [0].peak = mag [k] ;
			peaks [0].index = k ;
			break ;
			} ;
		} ;

	/* Find subsequent peaks ans smooth between peaks. */
	for (k = peaks [0].index + 1 ; k < len - 1 ; k++)
	{	if (mag [k - 1] < mag [k] && mag [k] >= mag [k + 1])
		{	peaks [1].peak = mag [k] ;
			peaks [1].index = k ;

			if (peaks [1].peak > peaks [0].peak)
				linear_smooth (mag, &peaks [1], &peaks [0]) ;
			else
				linear_smooth (mag, &peaks [0], &peaks [1]) ;
			peaks [0] = peaks [1] ;
			} ;
		} ;

} /* smooth_mag_spectrum */

static void
linear_smooth (double *mag, PEAK_DATA *larger, PEAK_DATA *smaller)
{	int k ;

	if (smaller->index < larger->index)
	{	for (k = smaller->index + 1 ; k < larger->index ; k++)
			mag [k] = (mag [k] < mag [k - 1]) ? 0.999 * mag [k - 1] : mag [k] ;
		}
	else
	{	for (k = smaller->index - 1 ; k >= larger->index ; k--)
			mag [k] = (mag [k] < mag [k + 1]) ? 0.999 * mag [k + 1] : mag [k] ;
		} ;

} /* linear_smooth */

/*==============================================================================
*/

static int
peak_compare (const void *vp1, const void *vp2)
{	const PEAK_DATA *peak1, *peak2 ;

	peak1 = (const PEAK_DATA*) vp1 ;
	peak2 = (const PEAK_DATA*) vp2 ;

	return (peak1->peak < peak2->peak) ? 1 : -1 ;
} /* peak_compare */

static double
find_snr (const double *magnitude, int len, int expected_peaks)
{	PEAK_DATA peaks [MAX_PEAKS] ;

	int		k, peak_count = 0 ;
	double	snr ;

	memset (peaks, 0, sizeof (peaks)) ;

	/* Find the MAX_PEAKS largest peaks. */
	for (k = 1 ; k < len - 1 ; k++)
	{	if (magnitude [k - 1] < magnitude [k] && magnitude [k] >= magnitude [k + 1])
		{	if (peak_count < MAX_PEAKS)
			{	peaks [peak_count].peak = magnitude [k] ;
				peaks [peak_count].index = k ;
				peak_count ++ ;
				qsort (peaks, peak_count, sizeof (PEAK_DATA), peak_compare) ;
				}
			else if (magnitude [k] > peaks [MAX_PEAKS - 1].peak)
			{	peaks [MAX_PEAKS - 1].peak = magnitude [k] ;
				peaks [MAX_PEAKS - 1].index = k ;
				qsort (peaks, MAX_PEAKS, sizeof (PEAK_DATA), peak_compare) ;
				} ;
			} ;
		} ;

	if (peak_count < expected_peaks)
	{	printf ("\n%s : line %d : bad peak_count (%d), expected %d.\n\n", __FILE__, __LINE__, peak_count, expected_peaks) ;
		return -1.0 ;
		} ;

	/* Sort the peaks. */
	qsort (peaks, peak_count, sizeof (PEAK_DATA), peak_compare) ;

	snr = peaks [0].peak ;
	for (k = 1 ; k < peak_count ; k++)
		if (fabs (snr - peaks [k].peak) > 10.0)
			return fabs (peaks [k].peak) ;

	return snr ;
} /* find_snr */

static void
log_mag_spectrum (double *input, int len, double *magnitude)
{	fftw_plan plan = NULL ;

	double	maxval ;
	int		k ;

	if (input == NULL || magnitude == NULL)
		return ;

	plan = fftw_plan_r2r_1d (len, input, magnitude, FFTW_R2HC, FFTW_ESTIMATE | FFTW_PRESERVE_INPUT) ;
	if (plan == NULL)
	{	printf ("%s : line %d : create plan failed.\n", __FILE__, __LINE__) ;
		exit (1) ;
		} ;

	fftw_execute (plan) ;

	fftw_destroy_plan (plan) ;

	maxval = 0.0 ;
	for (k = 1 ; k < len / 2 ; k++)
	{	/*
		** From : http://www.fftw.org/doc/Real_002dto_002dReal-Transform-Kinds.html#Real_002dto_002dReal-Transform-Kinds
		**
		** FFTW_R2HC computes a real-input DFT with output in “halfcomplex” format, i.e. real and imaginary parts
		** for a transform of size n stored as:
		**
		**      r0, r1, r2, ..., rn/2, i(n+1)/2-1, ..., i2, i1
		*/
		double re = magnitude [k] ;
		double im = magnitude [len - k] ;
		magnitude [k] = sqrt (re * re + im * im) ;
		maxval = (maxval < magnitude [k]) ? magnitude [k] : maxval ;
		} ;

	memset (magnitude + len / 2, 0, len / 2 * sizeof (magnitude [0])) ;

	/* Don't care about DC component. Make it zero. */
	magnitude [0] = 0.0 ;

	/* log magnitude. */
	for (k = 0 ; k < len ; k++)
	{	magnitude [k] = magnitude [k] / maxval ;
		magnitude [k] = (magnitude [k] < 1e-15) ? -200.0 : 20.0 * log10 (magnitude [k]) ;
		} ;

	return ;
} /* log_mag_spectrum */

#else /* ! (HAVE_LIBFFTW && HAVE_LIBRFFTW) */

double
calculate_snr (float *data, int len, int expected_peaks)
{	double snr = 200.0 ;

	data = data ;
	len = len ;
	expected_peaks = expected_peaks ;

	return snr ;
} /* calculate_snr */

#endif
Commit	Line	Data
	1	/*
	2	** Copyright (c) 2002-2016, Erik de Castro Lopo <erikd@mega-nerd.com>
	3	** All rights reserved.
	4	**
	5	** This code is released under 2-clause BSD license. Please see the
	6	** file at : https://github.com/erikd/libsamplerate/blob/master/COPYING
	7	*/
	8
	9	#include "config.h"
	10
	11	#include "util.h"
	12
	13	#if (HAVE_FFTW3 == 1)
	14
	15	#include <stdio.h>
	16	#include <stdlib.h>
	17	#include <string.h>
	18	#include <math.h>
	19
	20	#include <fftw3.h>
	21
	22	#define MAX_SPEC_LEN (1<<18)
	23	#define MAX_PEAKS 10
	24
	25	static void log_mag_spectrum (double input, int len, double magnitude) ;
	26	static void smooth_mag_spectrum (double *magnitude, int len) ;
	27	static double find_snr (const double *magnitude, int len, int expected_peaks) ;
	28
	29	typedef struct
	30	{ double peak ;
	31	int index ;
	32	} PEAK_DATA ;
	33
	34	double
	35	calculate_snr (float *data, int len, int expected_peaks)
	36	{ static double magnitude [MAX_SPEC_LEN] ;
	37	static double datacopy [MAX_SPEC_LEN] ;
	38
	39	double snr = 200.0 ;
	40	int k ;
	41
	42	if (len > MAX_SPEC_LEN)
	43	{ printf ("%s : line %d : data length too large.\n", __FILE__, __LINE__) ;
	44	exit (1) ;
	45	} ;
	46
	47	for (k = 0 ; k < len ; k++)
	48	datacopy [k] = data [k] ;
	49
	50	/* Pad the data just a little to speed up the FFT. */
	51	while ((len & 0x1F) && len < MAX_SPEC_LEN)
	52	{ datacopy [len] = 0.0 ;
	53	len ++ ;
	54	} ;
	55
	56	log_mag_spectrum (datacopy, len, magnitude) ;
	57	smooth_mag_spectrum (magnitude, len / 2) ;
	58
	59	snr = find_snr (magnitude, len, expected_peaks) ;
	60
	61	return snr ;
	62	} /* calculate_snr */
	63
	64	/*==============================================================================
	65	** There is a slight problem with trying to measure SNR with the method used
	66	** here; the side lobes of the windowed FFT can look like a noise/aliasing peak.
	67	** The solution is to smooth the magnitude spectrum by wiping out troughs
	68	** between adjacent peaks as done here.
	69	** This removes side lobe peaks without affecting noise/aliasing peaks.
	70	*/
	71
	72	static void linear_smooth (double mag, PEAK_DATA larger, PEAK_DATA *smaller) ;
	73
	74	static void
	75	smooth_mag_spectrum (double *mag, int len)
	76	{ PEAK_DATA peaks [2] ;
	77
	78	int k ;
	79
	80	memset (peaks, 0, sizeof (peaks)) ;
	81
	82	/* Find first peak. */
	83	for (k = 1 ; k < len - 1 ; k++)
	84	{ if (mag [k - 1] < mag [k] && mag [k] >= mag [k + 1])
	85	{ peaks [0].peak = mag [k] ;
	86	peaks [0].index = k ;
	87	break ;
	88	} ;
	89	} ;
	90
	91	/* Find subsequent peaks ans smooth between peaks. */
	92	for (k = peaks [0].index + 1 ; k < len - 1 ; k++)
	93	{ if (mag [k - 1] < mag [k] && mag [k] >= mag [k + 1])
	94	{ peaks [1].peak = mag [k] ;
	95	peaks [1].index = k ;
	96
	97	if (peaks [1].peak > peaks [0].peak)
	98	linear_smooth (mag, &peaks [1], &peaks [0]) ;
	99	else
	100	linear_smooth (mag, &peaks [0], &peaks [1]) ;
	101	peaks [0] = peaks [1] ;
	102	} ;
	103	} ;
	104
	105	} /* smooth_mag_spectrum */
	106
	107	static void
	108	linear_smooth (double mag, PEAK_DATA larger, PEAK_DATA *smaller)
	109	{ int k ;
	110
	111	if (smaller->index < larger->index)
	112	{ for (k = smaller->index + 1 ; k < larger->index ; k++)
	113	mag [k] = (mag [k] < mag [k - 1]) ? 0.999 * mag [k - 1] : mag [k] ;
	114	}
	115	else
	116	{ for (k = smaller->index - 1 ; k >= larger->index ; k--)
	117	mag [k] = (mag [k] < mag [k + 1]) ? 0.999 * mag [k + 1] : mag [k] ;
	118	} ;
	119
	120	} /* linear_smooth */
	121
	122	/*==============================================================================
	123	*/
	124
	125	static int
	126	peak_compare (const void vp1, const void vp2)
	127	{ const PEAK_DATA peak1, peak2 ;
	128
	129	peak1 = (const PEAK_DATA*) vp1 ;
	130	peak2 = (const PEAK_DATA*) vp2 ;
	131
	132	return (peak1->peak < peak2->peak) ? 1 : -1 ;
	133	} /* peak_compare */
	134
	135	static double
	136	find_snr (const double *magnitude, int len, int expected_peaks)
	137	{ PEAK_DATA peaks [MAX_PEAKS] ;
	138
	139	int k, peak_count = 0 ;
	140	double snr ;
	141
	142	memset (peaks, 0, sizeof (peaks)) ;
	143
	144	/* Find the MAX_PEAKS largest peaks. */
	145	for (k = 1 ; k < len - 1 ; k++)
	146	{ if (magnitude [k - 1] < magnitude [k] && magnitude [k] >= magnitude [k + 1])
	147	{ if (peak_count < MAX_PEAKS)
	148	{ peaks [peak_count].peak = magnitude [k] ;
	149	peaks [peak_count].index = k ;
	150	peak_count ++ ;
	151	qsort (peaks, peak_count, sizeof (PEAK_DATA), peak_compare) ;
	152	}
	153	else if (magnitude [k] > peaks [MAX_PEAKS - 1].peak)
	154	{ peaks [MAX_PEAKS - 1].peak = magnitude [k] ;
	155	peaks [MAX_PEAKS - 1].index = k ;
	156	qsort (peaks, MAX_PEAKS, sizeof (PEAK_DATA), peak_compare) ;
	157	} ;
	158	} ;
	159	} ;
	160
	161	if (peak_count < expected_peaks)
	162	{ printf ("\n%s : line %d : bad peak_count (%d), expected %d.\n\n", __FILE__, __LINE__, peak_count, expected_peaks) ;
	163	return -1.0 ;
	164	} ;
	165
	166	/* Sort the peaks. */
	167	qsort (peaks, peak_count, sizeof (PEAK_DATA), peak_compare) ;
	168
	169	snr = peaks [0].peak ;
	170	for (k = 1 ; k < peak_count ; k++)
	171	if (fabs (snr - peaks [k].peak) > 10.0)
	172	return fabs (peaks [k].peak) ;
	173
	174	return snr ;
	175	} /* find_snr */
	176
	177	static void
	178	log_mag_spectrum (double input, int len, double magnitude)
	179	{ fftw_plan plan = NULL ;
	180
	181	double maxval ;
	182	int k ;
	183
	184	if (input == NULL \|\| magnitude == NULL)
	185	return ;
	186
	187	plan = fftw_plan_r2r_1d (len, input, magnitude, FFTW_R2HC, FFTW_ESTIMATE \| FFTW_PRESERVE_INPUT) ;
	188	if (plan == NULL)
	189	{ printf ("%s : line %d : create plan failed.\n", __FILE__, __LINE__) ;
	190	exit (1) ;
	191	} ;
	192
	193	fftw_execute (plan) ;
	194
	195	fftw_destroy_plan (plan) ;
	196
	197	maxval = 0.0 ;
	198	for (k = 1 ; k < len / 2 ; k++)
	199	{ /*
	200	** From : http://www.fftw.org/doc/Real_002dto_002dReal-Transform-Kinds.html#Real_002dto_002dReal-Transform-Kinds
	201	**
	202	** FFTW_R2HC computes a real-input DFT with output in “halfcomplex” format, i.e. real and imaginary parts
	203	** for a transform of size n stored as:
	204	**
	205	** r0, r1, r2, ..., rn/2, i(n+1)/2-1, ..., i2, i1
	206	*/
	207	double re = magnitude [k] ;
	208	double im = magnitude [len - k] ;
	209	magnitude [k] = sqrt (re * re + im * im) ;
	210	maxval = (maxval < magnitude [k]) ? magnitude [k] : maxval ;
	211	} ;
	212
	213	memset (magnitude + len / 2, 0, len / 2 * sizeof (magnitude [0])) ;
	214
	215	/* Don't care about DC component. Make it zero. */
	216	magnitude [0] = 0.0 ;
	217
	218	/* log magnitude. */
	219	for (k = 0 ; k < len ; k++)
	220	{ magnitude [k] = magnitude [k] / maxval ;
	221	magnitude [k] = (magnitude [k] < 1e-15) ? -200.0 : 20.0 * log10 (magnitude [k]) ;
	222	} ;
	223
	224	return ;
	225	} /* log_mag_spectrum */
	226
	227	#else /* ! (HAVE_LIBFFTW && HAVE_LIBRFFTW) */
	228
	229	double
	230	calculate_snr (float *data, int len, int expected_peaks)
	231	{ double snr = 200.0 ;
	232
	233	data = data ;
	234	len = len ;
	235	expected_peaks = expected_peaks ;
	236
	237	return snr ;
	238	} /* calculate_snr */
	239
	240	#endif
	241