initial import
authorSteinar H. Gunderson <sgunderson@bigfoot.com>
Mon, 11 Jul 2005 10:11:14 +0000 (10:11 +0000)
committerSteinar H. Gunderson <sgunderson@bigfoot.com>
Mon, 11 Jul 2005 10:11:14 +0000 (10:11 +0000)
(automatically generated log message)

pitch.cpp [new file with mode: 0644]

diff --git a/pitch.cpp b/pitch.cpp
new file mode 100644 (file)
index 0000000..8080748
--- /dev/null
+++ b/pitch.cpp
@@ -0,0 +1,334 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <complex>
+#include <cassert>
+#include <algorithm>
+#include <fftw3.h>
+#include <sys/ioctl.h>
+#include <linux/soundcard.h>
+
+#define SAMPLE_RATE     22050
+#define FFT_LENGTH      4096     /* in samples */
+#define PAD_FACTOR      2        /* 1/pf of the FFT samples are real samples, the rest are padding */
+#define OVERLAP         4        /* 1/ol samples will be replaced in the buffer every frame. Should be
+                                 * a multiple of 2 for the Hamming window (see
+                                 * http://www-ccrma.stanford.edu/~jos/parshl/Choice_Hop_Size.html).
+                                 */
+
+int get_dsp_fd();
+void read_chunk(int fd, double *in, unsigned num_samples);
+void apply_window(double *in, double *out, unsigned num_samples);
+std::pair<double, double> find_peak(double *in, unsigned num_samples);
+void find_peak_magnitudes(std::complex<double> *in, double *out, unsigned num_samples);
+double bin_to_freq(double bin, unsigned num_samples);
+std::string freq_to_tonename(double freq);
+std::pair<double, double> interpolate_peak(double ym1, double y0, double y1);
+void print_spectrogram(double freq, double amp);
+void write_sine(int dsp_fd, double freq, unsigned num_samples);
+
+int main()
+{
+       double *in, *in_windowed;
+       std::complex<double> *out;
+       double *bins;
+       fftw_plan p;
+
+       // allocate memory
+       in = reinterpret_cast<double *> (fftw_malloc(sizeof(double) * FFT_LENGTH / PAD_FACTOR));
+       in_windowed = reinterpret_cast<double *> (fftw_malloc(sizeof(double) * FFT_LENGTH));
+       out = reinterpret_cast<std::complex<double> *> (fftw_malloc(sizeof(std::complex<double>) * (FFT_LENGTH / 2 + 1)));
+       bins = reinterpret_cast<double *> (fftw_malloc(sizeof(double) * FFT_LENGTH / 2 + 1));
+
+       memset(in, 0, sizeof(double) * FFT_LENGTH / PAD_FACTOR);
+
+       // init FFTW
+       p = fftw_plan_dft_r2c_1d(FFT_LENGTH, in_windowed, reinterpret_cast<fftw_complex *> (out), FFTW_ESTIMATE);
+       
+       int fd = get_dsp_fd();
+       for ( ;; ) {
+               read_chunk(fd, in, FFT_LENGTH);
+               apply_window(in, in_windowed, FFT_LENGTH);
+               fftw_execute(p);
+               find_peak_magnitudes(out, bins, FFT_LENGTH);
+               std::pair<double, double> peak = find_peak(bins, FFT_LENGTH);
+
+               if (peak.first < 50.0 || peak.second - log10(FFT_LENGTH) < 0.0) {
+                       printf("............\n");
+               } else {
+                       print_spectrogram(peak.first, peak.second - log10(FFT_LENGTH));
+               }
+       }
+}
+
+int get_dsp_fd()
+{
+       int fd = open("/dev/dsp", O_RDWR);
+       if (fd == -1) {
+               perror("/dev/dsp");
+               exit(1);
+       }
+       
+       ioctl(3, SNDCTL_DSP_RESET, 0);
+       
+       int fmt = AFMT_S16_LE;   // FIXME
+       ioctl(fd, SNDCTL_DSP_SETFMT, &fmt);
+
+       int chan = 1;
+       ioctl(fd, SOUND_PCM_WRITE_CHANNELS, &chan);
+       
+       int rate = 22050;
+       ioctl(fd, SOUND_PCM_WRITE_RATE, &rate);
+
+       ioctl(3, SNDCTL_DSP_SYNC, 0);
+       
+       return fd;
+}
+
+#if 1
+void read_chunk(int fd, double *in, unsigned num_samples)
+{
+       int ret;
+       unsigned to_read = num_samples / PAD_FACTOR / OVERLAP;
+       short buf[to_read];
+
+       memmove(in, in + to_read, (num_samples / PAD_FACTOR - to_read) * sizeof(double));
+       
+       ret = read(fd, buf, to_read * sizeof(short));
+       if (ret == 0) {
+               printf("EOF\n");
+               exit(0);
+       }
+
+       if (ret != int(to_read * sizeof(short))) {
+               // blah
+               perror("read");
+               exit(1);
+       }
+       
+       for (unsigned i = 0; i < to_read; ++i)
+               in[i + (num_samples / PAD_FACTOR - to_read)] = double(buf[i]);
+}
+#else
+// make a pure 440hz sine for testing
+void read_chunk(int fd, double *in, unsigned num_samples)
+{
+       static double theta = 0.0;
+       for (unsigned i = 0; i < num_samples; ++i) {
+               in[i] = cos(theta);
+               theta += 2.0 * M_PI * 440.0 / double(SAMPLE_RATE);
+       }
+}
+#endif
+
+void write_sine(int dsp_fd, double freq, unsigned num_samples) 
+{
+       static double theta = 0.0;
+       short buf[num_samples];
+       
+       for (unsigned i = 0; i < num_samples; ++i) {
+               buf[i] = short(cos(theta) * 16384.0);
+               theta += 2.0 * M_PI * freq / double(SAMPLE_RATE);
+       }
+
+       write(dsp_fd, buf, num_samples * sizeof(short));
+}
+
+// Apply a standard Hamming window to our input data.
+void apply_window(double *in, double *out, unsigned num_samples)
+{
+       static double *win_data;
+       static unsigned win_len;
+       static bool win_inited = false;
+
+       // Initialize the window for the first time
+       if (!win_inited) {
+               win_len = num_samples / PAD_FACTOR;
+               win_data = new double[win_len];
+
+               for (unsigned i = 0; i < win_len; ++i) {
+                       win_data[i] = 0.54 - 0.46 * cos(2.0 * M_PI * double(i) / double(win_len - 1));
+               }
+
+               win_inited = true;
+       }
+
+       assert(win_len == num_samples / PAD_FACTOR);
+       
+       for (unsigned i = 0; i < win_len; ++i) {
+               out[i] = in[i] * win_data[i];
+       }
+       for (unsigned i = win_len; i < num_samples; ++i) {
+               out[i] = 0.0;
+       }
+}
+
+void find_peak_magnitudes(std::complex<double> *in, double *out, unsigned num_samples)
+{
+       for (unsigned i = 0; i < num_samples / 2 + 1; ++i)
+               out[i] = abs(in[i]);
+}
+
+std::pair<double, double> find_peak(double *in, unsigned num_samples)
+{
+       double best_peak = in[0];
+       unsigned best_bin = 0;
+
+       for (unsigned i = 1; i < num_samples / 2 + 1; ++i) {
+               if (in[i] > best_peak) {
+                       best_peak = in[i];
+                       best_bin = i;
+               }
+       }
+       
+       if (best_bin == 0 || best_bin == num_samples / 2) {
+               return std::make_pair(-1.0, 0.0);
+       }
+
+#if 0
+       printf("undertone strength: %+4.2f %+4.2f %+4.2f [%+4.2f] %+4.2f %+4.2f %+4.2f\n",
+               20.0 * log10(in[best_bin*4] / FFT_LENGTH),
+               20.0 * log10(in[best_bin*3] / FFT_LENGTH),
+               20.0 * log10(in[best_bin*2] / FFT_LENGTH),
+               20.0 * log10(in[best_bin] / FFT_LENGTH),
+               20.0 * log10(in[best_bin/2] / FFT_LENGTH),
+               20.0 * log10(in[best_bin/3] / FFT_LENGTH),
+               20.0 * log10(in[best_bin/4] / FFT_LENGTH));
+#endif
+
+       // see if we might have hit an overtone (set a limit of 5dB)
+       for (unsigned i = 4; i >= 1; --i) {
+               if (best_bin != best_bin / i &&
+                   20.0 * log10(in[best_bin] / in[best_bin / i]) < 5.0f) {
+#if 0
+                       printf("Overtone of degree %u!\n", i);
+#endif
+                       best_bin /= i;
+
+                       // consider sliding one bin up or down
+                       if (best_bin > 0 && in[best_bin - 1] > in[best_bin] && in[best_bin - 1] > in[best_bin - 2]) {
+                               --best_bin;
+                       } else if (best_bin < num_samples / 2 && in[best_bin + 1] > in[best_bin] && in[best_bin + 1] > in[best_bin + 2]) {
+                               ++best_bin;
+                       }
+                          
+                       break;
+               }
+       }
+
+       std::pair<double, double> peak = 
+               interpolate_peak(in[best_bin - 1],
+                                in[best_bin],
+                                in[best_bin + 1]);
+               
+       return std::make_pair(bin_to_freq(double(best_bin) + peak.first, num_samples), peak.second);
+}
+
+double bin_to_freq(double bin, unsigned num_samples)
+{
+       return bin * SAMPLE_RATE / double(num_samples);
+}
+
+/*
+ * Given three bins, find the interpolated real peak based
+ * on their magnitudes. To do this, we execute the following
+ * plan:
+ * 
+ * Fit a polynomial of the form ax^2 + bx + c = 0 to the data
+ * we have. Maple readily yields our coefficients, assuming
+ * that we have the values at x=-1, x=0 and x=1:
+ *
+ * > f := x -> a*x^2 + b*x + c;                                
+ * 
+ *                               2
+ *           f := x -> a x  + b x + c
+ * 
+ * > cf := solve({ f(-1) = ym1, f(0) = y0, f(1) = y1 }, { a, b, c });
+ * 
+ *                            y1    ym1       y1    ym1
+ *        cf := {c = y0, b = ---- - ---, a = ---- + --- - y0}
+ *                            2      2        2      2
+ *
+ * Now let's find the maximum point for the polynomial (it has to be
+ * a maximum, since y0 is the greatest value):
+ *
+ * > xmax := solve(subs(cf, diff(f(x), x)) = 0, x);
+ * 
+ *                                -y1 + ym1
+ *                   xmax := -------------------
+ *                           2 (y1 + ym1 - 2 y0)
+ *
+ * We could go further and insert {fmax,a,b,c} into the original
+ * polynomial, but it just gets hairy. We calculate a, b and c separately
+ * instead.
+ *
+ * http://www-ccrma.stanford.edu/~jos/parshl/Peak_Detection_Steps_3.html
+ * claims that detection is almost twice as good when using dB scale instead
+ * of linear scale for the input values, so we use that. (As a tiny bonus,
+ * we get back dB scale from the function.)
+ */
+std::pair<double, double> interpolate_peak(double ym1, double y0, double y1)
+{
+       ym1 = log10(ym1);
+       y0 = log10(y0);
+       y1 = log10(y1);
+
+#if 0
+       assert(y0 >= y1);
+       assert(y0 >= ym1);
+#endif
+       
+       double a = 0.5 * y1 + 0.5 * ym1 - y0;
+       double b = 0.5 * y1 - 0.5 * ym1;
+       double c = y0;
+       
+       double xmax = (ym1 - y1) / (2.0 * (y1 + ym1 - 2.0 * y0));
+       double ymax = 20.0 * (a * xmax * xmax + b * xmax + c) - 90.0;
+
+       return std::make_pair(xmax, ymax);
+}
+
+std::string freq_to_tonename(double freq)
+{
+       std::string notenames[] = { "C", "C#", "D", "D#", "E", "F", "F#", "G", "G#", "A", "A#", "B" };
+       double half_notes_away = 12.0 * log2(freq / 440.0) - 3.0;
+       int hnai = int(floor(half_notes_away + 0.5));
+       int octave = (hnai + 48) / 12;
+
+       char buf[256];
+       sprintf(buf, "%s%d + %.2f [%d]", notenames[((hnai % 12) + 12) % 12].c_str(), octave, half_notes_away - hnai, hnai);
+       return buf;
+}
+
+void print_spectrogram(double freq, double amp)
+{
+       std::string notenames[] = { "C", "C#", "D", "D#", "E", "F", "F#", "G", "G#", "A", "A#", "B" };
+       double half_notes_away = 12.0 * log2(freq / 440.0) - 3.0;
+       int hnai = int(floor(half_notes_away + 0.5));
+       int octave = (hnai + 48) / 12;
+
+       for (int i = 0; i < 12; ++i)
+               if (i == ((hnai % 12) + 12) % 12)
+                       printf("#");
+               else
+                       printf(".");
+
+       printf(" (%-2s%d %+.2f, %5.2fHz) [%5.2fdB]  [", notenames[((hnai % 12) + 12) % 12].c_str(), octave, half_notes_away - hnai,
+               freq, amp);
+
+       double off = half_notes_away - hnai;
+       for (int i = -10; i <= 10; ++i) {
+               if (off >= (i-0.5) * 0.05 && off < (i+0.5) * 0.05) {
+                       printf("#");
+               } else {
+                       if (i == 0) {
+                               printf("|");
+                       } else {
+                               printf("-");
+                       }
+               }
+       }
+       printf("]\n");
+
+}