libsamplerate/tests/calc_snr.c

   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